From 5d074063d1d667162735026baff3fef3b67ef0fa Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 14 Oct 2014 05:22:36 +0200 Subject: [PATCH] OP-1535 - Add board specific sensor update rate constant --- flight/modules/Altitude/revolution/altitude.c | 9 +++-- flight/modules/Attitude/attitude.c | 33 ++++++++++--------- flight/modules/Sensors/sensors.c | 24 ++++++++------ flight/modules/Stabilization/altitudeloop.c | 2 +- flight/modules/Stabilization/innerloop.c | 2 +- flight/modules/Stabilization/outerloop.c | 2 +- flight/modules/StateEstimation/filterekf.c | 2 +- .../modules/StateEstimation/filtervelocity.c | 2 +- .../coptercontrol/firmware/inc/pios_config.h | 2 ++ .../firmware/inc/pios_config.h | 3 ++ .../revolution/firmware/inc/pios_config.h | 5 +-- .../revoproto/firmware/inc/pios_config.h | 2 ++ .../simposix/firmware/inc/pios_config.h | 3 ++ 13 files changed, 55 insertions(+), 36 deletions(-) diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c index eb3fe1b75..6cd1e5750 100644 --- a/flight/modules/Altitude/revolution/altitude.c +++ b/flight/modules/Altitude/revolution/altitude.c @@ -53,8 +53,11 @@ // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 10 -// 5Hz @120Hz update -#define TEMP_ALPHA 0.9979f + +// LPF +#define TEMP_DT (1.0f / 120.0f) +#define TEMP_LPF_FC 5.0f +static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); // Private types @@ -181,7 +184,7 @@ static void altitudeTask(__attribute__((unused)) void *parameters) baro_temperature = temp; } - baro_temperature = TEMP_ALPHA * baro_temperature + (1 - TEMP_ALPHA) * temp; + baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature; if (tempCorrectionEnabled && !temp_calibration_count) { temp_calibration_count = TEMP_CALIB_INTERVAL; diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index d79699d65..84a09b397 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -82,18 +82,19 @@ PERF_DEFINE_COUNTER(counterAtt); #define STACK_SIZE_BYTES 540 #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -// Attitude module runs at 500Hz -#define SENSOR_PERIOD 2 +// Attitude module loop interval (defined by sensor rate in pios_config.h) +static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); #define UPDATE_RATE 25.0f // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 30 +// LPF +#define TEMP_DT (1.0f / PIOS_SENSOR_RATE) +#define TEMP_LPF_FC 5.0f +static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); -// LPF 5Hz at 500Hz rate -#define TEMP_ALPHA 0.999504f - -#define UPDATE_EXPECTED (1.0f / 500.0f) +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f @@ -327,7 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) PERF_MEASURE_PERIOD(counterPeriod); AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); } - vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_PERIOD_MS); + vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS); } } @@ -465,7 +466,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData * #if defined(PIOS_INCLUDE_MPU6000) xQueueHandle queue = PIOS_MPU6000_GetQueue(); - BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD); + BaseType_t ret = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms); while (ret == pdTRUE) { gyros[0] += mpu6000_data.gyro_x; gyros[1] += mpu6000_data.gyro_y; @@ -492,19 +493,19 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData * } float invcount = 1.0f / count; PERF_TIMED_SECTION_START(counterUpd); - gyros[0] *= gyro_scale.X * invcount; - gyros[1] *= gyro_scale.Y * invcount; - gyros[2] *= gyro_scale.Z * invcount; + gyros[0] *= gyro_scale.X * invcount; + gyros[1] *= gyro_scale.Y * invcount; + gyros[2] *= gyro_scale.Z * invcount; - accels[0] *= accel_scale.X * invcount; - accels[1] *= accel_scale.Y * invcount; - accels[2] *= accel_scale.Z * invcount; + accels[0] *= accel_scale.X * invcount; + accels[1] *= accel_scale.Y * invcount; + accels[2] *= accel_scale.Z * invcount; temp *= invcount; - if(isnan(temperature)){ + if (isnan(temperature)) { temperature = temp; } - temperature = TEMP_ALPHA * temperature + (1 - TEMP_ALPHA) * temp; + temperature = temp_alpha * (temp - temperature) + temperature; if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) { temp_calibration_count = TEMP_CALIB_INTERVAL; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index 67b063b24..764999928 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -58,7 +58,7 @@ #include #include #include - +#include #include #include @@ -66,12 +66,16 @@ // Private constants #define STACK_SIZE_BYTES 1000 #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define SENSOR_PERIOD 2 + +static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE); // Interval in number of sample to recalculate temp bias #define TEMP_CALIB_INTERVAL 30 -// LPF 5Hz at 500Hz rate -#define TEMP_ALPHA 0.999504f + +// LPF +#define TEMP_DT (1.0f / PIOS_SENSOR_RATE) +#define TEMP_LPF_FC 5.0f +static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC)); #define ZERO_ROT_ANGLE 0.00001f // Private types @@ -255,7 +259,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); #endif lastSysTime = xTaskGetTickCount(); - vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); + vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); error = false; } else { @@ -284,7 +288,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) count = 0; while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) { - error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; + error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error; } if (error) { // Unfortunately if the BMA180 ever misses getting read, then it will not @@ -379,9 +383,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters) gyro_temperature = gyroSensorData.temperature; } - accel_temperature = TEMP_ALPHA * accel_temperature + (1 - TEMP_ALPHA) * accelSensorData.temperature; - gyro_temperature = TEMP_ALPHA * gyro_temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature; - gyroSensorData.temperature = gyro_temperature; + accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature; + gyro_temperature = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature; + if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) { temp_calibration_count = TEMP_CALIB_INTERVAL; if (accel_temp_calibrated) { @@ -472,7 +476,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); #endif - vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); + vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS); } } diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.c index f54ed5740..489e6e0d1 100644 --- a/flight/modules/Stabilization/altitudeloop.c +++ b/flight/modules/Stabilization/altitudeloop.c @@ -42,7 +42,7 @@ #ifdef REVOLUTION -#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 1f152c382..816006b7c 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -54,7 +54,7 @@ #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL -#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index a1f8d37c1..995003208 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -52,7 +52,7 @@ #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR -#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE) #define UPDATE_MIN 1.0e-6f #define UPDATE_MAX 1.0f #define UPDATE_ALPHA 1.0e-2f diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 3931226e4..1fd036afd 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -47,7 +47,7 @@ #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 DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \ diff --git a/flight/modules/StateEstimation/filtervelocity.c b/flight/modules/StateEstimation/filtervelocity.c index 493f0f3d7..ec6e29867 100644 --- a/flight/modules/StateEstimation/filtervelocity.c +++ b/flight/modules/StateEstimation/filtervelocity.c @@ -40,7 +40,7 @@ #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 DT_INIT (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate // Private types struct data { diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 95332df45..9450a6781 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -92,6 +92,8 @@ /* #define PIOS_INCLUDE_ETASV3 */ /* #define PIOS_INCLUDE_HCSR04 */ +#define PIOS_SENSOR_RATE 500.0f + /* PIOS receiver drivers */ #define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PPM diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index 44065be33..11ec1415a 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -91,6 +91,9 @@ // #define PIOS_INCLUDE_MPXV // #define PIOS_INCLUDE_ETASV3 /* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f + #define PIOS_INCLUDE_WS2811 /* PIOS receiver drivers */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 5ee827783..15281a88b 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -91,11 +91,12 @@ #define PIOS_INCLUDE_MPXV #define PIOS_INCLUDE_ETASV3 #define PIOS_INCLUDE_MS4525DO +/* #define PIOS_INCLUDE_HCSR04 */ + +#define PIOS_SENSOR_RATE 500.0f #define PIOS_INCLUDE_WS2811 -/* #define PIOS_INCLUDE_HCSR04 */ - /* PIOS receiver drivers */ #define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PPM diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 897f9b360..b7aed8375 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -89,6 +89,8 @@ #define PIOS_INCLUDE_ETASV3 /* #define PIOS_INCLUDE_HCSR04 */ +#define PIOS_SENSOR_RATE 500.0f + /* PIOS receiver drivers */ #define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PPM diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 0dc6b7aaa..a592b487c 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -70,6 +70,9 @@ // #define PIOS_INCLUDE_MS5611 // #define PIOS_INCLUDE_HCSR04 #define PIOS_FLASH_ON_ACCEL /* true for second revo */ + +#define PIOS_SENSOR_RATE 500.0f + #define FLASH_FREERTOS /* Com systems to include */ #define PIOS_INCLUDE_COM