OP-1535 - Add board specific sensor update rate constant

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;

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
+// Attitude module loop interval (defined by sensor rate in pios_config.h)
-#define SENSOR_PERIOD       2
+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 UPDATE_EXPECTED     (1.0f / PIOS_SENSOR_RATE)
-#define TEMP_ALPHA          0.999504f
-
-#define UPDATE_EXPECTED     (1.0f / 500.0f)
 #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[0]  *= gyro_scale.X * invcount;
-    gyros[1]   *= gyro_scale.Y * invcount;
+    gyros[1]  *= gyro_scale.Y * invcount;
-    gyros[2]   *= gyro_scale.Z * invcount;
+    gyros[2]  *= gyro_scale.Z * invcount;
 
-    accels[0]  *= accel_scale.X * invcount;
+    accels[0] *= accel_scale.X * invcount;
-    accels[1]  *= accel_scale.Y * invcount;
+    accels[1] *= accel_scale.Y * invcount;
-    accels[2]  *= accel_scale.Z * 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;

flight/modules/Sensors/sensors.c

@@ -58,7 +58,7 @@
 #include <accelgyrosettings.h>
 #include <flightstatus.h>
 #include <taskinfo.h>
-
+#include <pios_math.h>
 #include <CoordinateConversions.h>
 
 #include <pios_board_info.h>
@@ -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;
+        accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature;
-        gyro_temperature  = TEMP_ALPHA * gyro_temperature + (1 - TEMP_ALPHA) * gyroSensorData.temperature;
+        gyro_temperature  = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature;
-        gyroSensorData.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);
     }
 }
 

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

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

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

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)) { \

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 {

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

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 */

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

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

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