From 597ac4db2a3d77c964e4e608d1e13ed67f5a6d25 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Feb 2014 14:09:50 +0100 Subject: [PATCH 1/6] OP-1195 increase priority for pios radio link driver task --- flight/pios/common/pios_rfm22b.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index c9ea2bd35..93bef7c5c 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -64,7 +64,7 @@ /* Local Defines */ #define STACK_SIZE_BYTES 200 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // flight control relevant device driver (ppm link) #define ISR_TIMEOUT 1 // ms #define EVENT_QUEUE_SIZE 5 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 From 8b25ac189405265904294b8037201e135450c15f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Feb 2014 14:13:25 +0100 Subject: [PATCH 2/6] OP-1195 adapted task priorities for manualcontrol and stabilization --- flight/modules/Actuator/actuator.c | 2 +- flight/modules/ManualControl/manualcontrol.c | 2 +- flight/modules/Stabilization/stabilization.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 7d92efdf2..782cc8ed1 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -55,7 +55,7 @@ #define STACK_SIZE_BYTES 1312 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) // device driver #define FAILSAFE_TIMEOUT_MS 100 #define MAX_MIX_ACTUATORS ACTUATORCOMMAND_CHANNEL_NUMELEM diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 4ed56a1ad..ea1c41ff6 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -64,7 +64,7 @@ #define STACK_SIZE_BYTES 1152 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control #define UPDATE_PERIOD_MS 20 #define THROTTLE_FAILSAFE -0.1f #define ARMED_THRESHOLD 0.50f diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 65462ae0c..fa03330bb 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -73,7 +73,7 @@ #define STACK_SIZE_BYTES 840 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority #define FAILSAFE_TIMEOUT_MS 30 // number of flight mode switch positions From 723e22aa1a7ce2cadcfcc301da83c88fe896339c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 19:05:17 +0100 Subject: [PATCH 3/6] OP-1211 time_measurement_helper --- flight/modules/Stabilization/stabilization.c | 12 +++- flight/pios/common/pios_deltatime.c | 68 +++++++++++++++++++ flight/pios/inc/pios_deltatime.h | 53 +++++++++++++++ flight/pios/pios.h | 4 ++ flight/pios/pios_sim_posix.h | 1 + .../coptercontrol/firmware/inc/pios_config.h | 1 + .../firmware/inc/pios_config_posix.h | 1 + .../revolution/firmware/inc/pios_config.h | 1 + .../revolution/firmware/inc/pios_config_sim.h | 1 + .../revoproto/firmware/inc/pios_config.h | 1 + .../revoproto/firmware/inc/pios_config_sim.h | 2 + .../targets/boards/simposix/firmware/Makefile | 1 + .../simposix/firmware/inc/pios_config.h | 1 + make/apps-defs.mk | 1 + 14 files changed, 145 insertions(+), 3 deletions(-) create mode 100644 flight/pios/common/pios_deltatime.c create mode 100644 flight/pios/inc/pios_deltatime.h diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 65462ae0c..dc86fc104 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -65,6 +65,11 @@ #include "relay_tuning.h" // Private constants +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-3f + #define MAX_QUEUE_SIZE 1 #if defined(PIOS_STABILIZATION_STACK_SIZE) @@ -194,7 +199,9 @@ MODULE_INITCALL(StabilizationInitialize, StabilizationStart); static void stabilizationTask(__attribute__((unused)) void *parameters) { UAVObjEvent ev; - uint32_t timeval = PIOS_DELAY_GetRaw(); + PiOSDeltatimeConfig timeval; + + PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; @@ -226,8 +233,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) continue; } - dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; - timeval = PIOS_DELAY_GetRaw(); + dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); diff --git a/flight/pios/common/pios_deltatime.c b/flight/pios/common/pios_deltatime.c new file mode 100644 index 000000000..5a2b56f02 --- /dev/null +++ b/flight/pios/common/pios_deltatime.c @@ -0,0 +1,68 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELTATIME time measurement Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_deltatime.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include + +#ifdef PIOS_INCLUDE_DELTATIME + +void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha) +{ + PIOS_Assert(config); + config->average = average; + config->min = min; + config->max = max; + config->alpha = alpha; + config->last = PIOS_DELAY_GetRaw(); +}; + + +float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config) +{ + PIOS_Assert(config); + float dT = PIOS_DELAY_DiffuS(config->last) * 1.0e-6f; + config->last = PIOS_DELAY_GetRaw(); + if (dT < config->min) { + dT = config->min; + } + if (dT > config->max) { + dT = config->max; + } + config->average = config->average * (1.0f - config->alpha) + dT * config->alpha; + return config->average; +} + + +#endif // PIOS_INCLUDE_DELTATIME + + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_deltatime.h b/flight/pios/inc/pios_deltatime.h new file mode 100644 index 000000000..968acff40 --- /dev/null +++ b/flight/pios/inc/pios_deltatime.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_DELTATIME time measurement Functions + * @brief PiOS Delay functionality + * @{ + * + * @file pios_deltatime.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Settings functions header + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_DELTATIME_H +#define PIOS_DELTATIME_H + +struct PiOSDeltatimeConfigStruct { + uint32_t last; + float average; + float min; + float max; + float alpha; +}; +typedef struct PiOSDeltatimeConfigStruct PiOSDeltatimeConfig; + +/* Public Functions */ +void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha); + +float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config); + +#endif /* PIOS_DELTATIME_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 6ba15b104..95ee81d47 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -104,6 +104,10 @@ #include #endif +#ifdef PIOS_INCLUDE_DELTATIME +#include +#endif + #ifdef PIOS_INCLUDE_INITCALL #include "pios_initcall.h" #endif diff --git a/flight/pios/pios_sim_posix.h b/flight/pios/pios_sim_posix.h index 84fd5440a..5b378eb46 100644 --- a/flight/pios/pios_sim_posix.h +++ b/flight/pios/pios_sim_posix.h @@ -79,6 +79,7 @@ extern void PIOS_LED_Init(void); #include #include #include +#include #include #include #include diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index ea94abb02..e0498e3e1 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -49,6 +49,7 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h index eda0a2511..4dc76d13d 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h @@ -32,6 +32,7 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index fbaff9438..c2d7673a8 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -49,6 +49,7 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h index 293516522..9402af396 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h @@ -32,6 +32,7 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index c8e3d340c..afd32c159 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -49,6 +49,7 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h index 56470b59b..0d6dd38a3 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h @@ -1,4 +1,5 @@ /** + #define PIOS_INCLUDE_DELTATIME ****************************************************************************** * * @file pios_config.h @@ -32,6 +33,7 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 000539682..221580e7f 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -101,6 +101,7 @@ SRC += $(MATHLIB)/pid.c SRC += $(PIOSCORECOMMON)/pios_task_monitor.c SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c SRC += $(PIOSCORECOMMON)/pios_debuglog.c +SRC += $(PIOSCORECOMMON)/pios_deltatime.c ## PIOS Hardware include $(PIOS)/posix/library.mk diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index f3b494e91..0738b6e44 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -41,6 +41,7 @@ /* Enable/Disable PiOS Modules */ // #define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY +#define PIOS_INCLUDE_DELTATIME // #define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED diff --git a/make/apps-defs.mk b/make/apps-defs.mk index b326b218d..fe268f73a 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -82,6 +82,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c SRC += $(PIOSCOMMON)/pios_debuglog.c +SRC += $(PIOSCOMMON)/pios_deltatime.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_rfm22b.c SRC += $(PIOSCOMMON)/pios_rfm22b_com.c From a3bb523bf3945ae56e5487f7cebd5575e99a866f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 19:21:33 +0100 Subject: [PATCH 4/6] OP-1211 adjusted alpha value to more sensible default --- flight/modules/Stabilization/stabilization.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index dc86fc104..c8da3c487 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -68,7 +68,7 @@ #define UPDATE_EXPECTED (1.0f / 666.0f) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f -#define UPDATE_ALPHA 1.0e-3f +#define UPDATE_ALPHA 1.0e-2f #define MAX_QUEUE_SIZE 1 @@ -234,7 +234,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - + fprintf(stderr, "dt is %f\n", dT); FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeStateGet(&attitudeState); From 28be9cc8ce177572889b2bae00a5868b181a1abb Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 19:48:33 +0100 Subject: [PATCH 5/6] OP-1211 adapted existing code to use new pios functionality instead of separate error prone implementations --- flight/modules/Attitude/attitude.c | 15 +++-- flight/modules/Stabilization/stabilization.c | 1 - .../modules/StateEstimation/filteraltitude.c | 63 +++++++------------ flight/modules/StateEstimation/filterekf.c | 34 ++++------ 4 files changed, 43 insertions(+), 70 deletions(-) diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 84ee51bb3..6a2bb4ce7 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -72,10 +72,16 @@ #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + // Private types // Private variables static xTaskHandle taskHandle; +static PiOSDeltatimeConfig dtconfig; // Private functions static void AttitudeTask(void *parameters); @@ -214,6 +220,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); + PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); + // Main task loop while (1) { FlightStatusData flightStatus; @@ -473,12 +481,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) { - float dT; - portTickType thisSysTime = xTaskGetTickCount(); - static portTickType lastSysTime = 0; - - dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; - lastSysTime = thisSysTime; + float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); // Bad practice to assume structure order, but saves memory float *gyros = &gyrosData->x; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index c8da3c487..ab5c7ab61 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -234,7 +234,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - fprintf(stderr, "dt is %f\n", dT); FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeStateGet(&attitudeState); diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 01d38a1b8..2b46b35af 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -40,20 +40,22 @@ #define STACK_REQUIRED 128 -#define DT_ALPHA 1e-3f +#define DT_ALPHA 1e-2f +#define DT_MIN 1e-6f +#define DT_MAX 1.0f +#define DT_AVERAGE 1e-3f // Private types struct data { - float state[4]; // state = altitude,velocity,accel_offset,accel - float pos[3]; // position updates from other filters - float vel[3]; // position updates from other filters - float dTA; - float dTA2; - int32_t lastTime; - float accelLast; - float baroLast; - int32_t baroLastTime; - bool first_run; + float state[4]; // state = altitude,velocity,accel_offset,accel + float pos[3]; // position updates from other filters + float vel[3]; // position updates from other filters + + PiOSDeltatimeConfig dt1config; + PiOSDeltatimeConfig dt2config; + float accelLast; + float baroLast; + bool first_run; AltitudeFilterSettingsData settings; }; @@ -89,8 +91,8 @@ static int32_t init(stateFilter *self) this->vel[0] = 0.0f; this->vel[1] = 0.0f; this->vel[2] = 0.0f; - this->dTA = -1.0f; - this->dTA2 = -1.0f; + PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA); + PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA); this->baroLast = 0.0f; this->accelLast = 0.0f; this->first_run = 1; @@ -104,12 +106,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location - if (IS_SET(state->updated, SENSORUPDATES_accel)) { - this->lastTime = PIOS_DELAY_GetRaw(); - } if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->first_run = 0; - this->baroLastTime = PIOS_DELAY_GetRaw(); + this->first_run = 0; } } else { // save existing position and velocity updates so GPS will still work @@ -141,22 +139,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler - float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f; - this->lastTime = PIOS_DELAY_GetRaw(); - if (dT < 0.001f) { - dT = 0.001f; - } - if (this->dTA < 0) { - this->dTA = dT; - } else { - this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA; - } + // + float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config); float speedLast = this->state[1]; - this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA; + this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT; this->accelLast = this->state[3] - this->state[2]; - this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA; + this->state[0] += 0.5f * (speedLast + this->state[1]) * dT; state->pos[0] = this->pos[0]; @@ -175,17 +165,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // correct the velocity state (low pass differentiation) // low pass for average dT, compensate timing jitter from scheduler - float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f; - this->baroLastTime = PIOS_DELAY_GetRaw(); - if (dT < 0.001f) { - dT = 0.001f; - } - if (this->dTA2 < 0) { - this->dTA2 = dT; - } else { - this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA; - } - this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2; + float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config); + this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT; this->baroLast = state->baro[0]; state->pos[0] = this->pos[0]; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index a141e4d8f..0aa906dda 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -45,6 +45,8 @@ #define STACK_REQUIRED 2048 #define DT_ALPHA 1e-3f +#define DT_MIN 1e-6f +#define DT_MAX 1.0f #define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo) #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ @@ -66,10 +68,9 @@ struct data { stateEstimation work; - uint32_t ins_last_time; - bool inited; + bool inited; - float dTa; + PiOSDeltatimeConfig dtconfig; }; // Private variables @@ -154,11 +155,10 @@ static int32_t maininit(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->inited = false; - this->init_stage = 0; - this->work.updated = 0; - this->ins_last_time = PIOS_DELAY_GetRaw(); - this->dTa = DT_INIT; + this->inited = false; + this->init_stage = 0; + this->work.updated = 0; + PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA); EKFConfigurationGet(&this->ekfConfiguration); int t; @@ -224,17 +224,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } - dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f; - this->ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if (dT > 0.01f) { - dT = 0.01f; - } else if (dT <= 0.001f) { - dT = 0.001f; - } - - this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler + dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig); if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read @@ -300,7 +290,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Run prediction a bit before any corrections float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; - INSStatePrediction(gyros, this->work.accel, this->dTa); + INSStatePrediction(gyros, this->work.accel, dT); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -335,7 +325,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; // Advance the state estimate - INSStatePrediction(gyros, this->work.accel, this->dTa); + INSStatePrediction(gyros, this->work.accel, dT); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -355,7 +345,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel; // Advance the covariance estimate - INSCovariancePrediction(this->dTa); + INSCovariancePrediction(dT); if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS; From 216ec095e45035a6b556363223ec1698ea47d643 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Feb 2014 19:34:34 +0100 Subject: [PATCH 6/6] OP-1211 removed unneeded define --- flight/pios/common/pios_deltatime.c | 4 ---- flight/pios/pios.h | 3 --- .../targets/boards/coptercontrol/firmware/inc/pios_config.h | 1 - .../boards/coptercontrol/firmware/inc/pios_config_posix.h | 1 - flight/targets/boards/revolution/firmware/inc/pios_config.h | 1 - .../targets/boards/revolution/firmware/inc/pios_config_sim.h | 1 - flight/targets/boards/revoproto/firmware/inc/pios_config.h | 1 - .../targets/boards/revoproto/firmware/inc/pios_config_sim.h | 2 -- flight/targets/boards/simposix/firmware/inc/pios_config.h | 1 - 9 files changed, 15 deletions(-) diff --git a/flight/pios/common/pios_deltatime.c b/flight/pios/common/pios_deltatime.c index 5a2b56f02..ec5ee8eb9 100644 --- a/flight/pios/common/pios_deltatime.c +++ b/flight/pios/common/pios_deltatime.c @@ -30,7 +30,6 @@ #include -#ifdef PIOS_INCLUDE_DELTATIME void PIOS_DELTATIME_Init(PiOSDeltatimeConfig *config, float average, float min, float max, float alpha) { @@ -59,9 +58,6 @@ float PIOS_DELTATIME_GetAverageSeconds(PiOSDeltatimeConfig *config) } -#endif // PIOS_INCLUDE_DELTATIME - - /** * @} * @} diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 95ee81d47..c1371d866 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -102,9 +102,6 @@ /* PIOS system functions */ #ifdef PIOS_INCLUDE_DELAY #include -#endif - -#ifdef PIOS_INCLUDE_DELTATIME #include #endif diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index e0498e3e1..ea94abb02 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -49,7 +49,6 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h index 4dc76d13d..eda0a2511 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h @@ -32,7 +32,6 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index c2d7673a8..fbaff9438 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -49,7 +49,6 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h index 9402af396..293516522 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h @@ -32,7 +32,6 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index afd32c159..c8e3d340c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -49,7 +49,6 @@ /* PIOS system functions */ #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_INITCALL #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_TASK_MONITOR diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h index 0d6dd38a3..56470b59b 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h @@ -1,5 +1,4 @@ /** - #define PIOS_INCLUDE_DELTATIME ****************************************************************************** * * @file pios_config.h @@ -33,7 +32,6 @@ /* Enable/Disable PiOS Modules */ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 0738b6e44..f3b494e91 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -41,7 +41,6 @@ /* Enable/Disable PiOS Modules */ // #define PIOS_INCLUDE_ADC #define PIOS_INCLUDE_DELAY -#define PIOS_INCLUDE_DELTATIME // #define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED