#102 Use a tick in the stabiliser to reduce the rate for sub tasks in…

…stead of the system tick
bitcraze · Apr 27, 2016 · d54d929 · d54d929
1 parent e3ac9f3
commit d54d929
Show file tree

Hide file tree

Showing 8 changed files with 24 additions and 20 deletions.
diff --git a/src/modules/interface/controller.h b/src/modules/interface/controller.h
@@ -32,6 +32,7 @@ void stateControllerInit(void);
 bool stateControllerTest(void);
 void stateController(control_t *control, const sensorData_t *sensors,
                                          const state_t *state,
-                                         const setpoint_t *setpoint);
+                                         const setpoint_t *setpoint,
+                                         const uint32_t tick);
 
 #endif //__CONTROLLER_H__
diff --git a/src/modules/interface/estimator.h b/src/modules/interface/estimator.h
@@ -30,6 +30,6 @@
 
 void stateEstimatorInit(void);
 bool stateEstimatorTest(void);
-void stateEstimator(state_t *state, const sensorData_t *sensorData);
+void stateEstimator(state_t *state, const sensorData_t *sensorData, const uint32_t tick);
 
 #endif //__ESTIMATOR_H__
diff --git a/src/modules/interface/sensors.h b/src/modules/interface/sensors.h
@@ -30,6 +30,6 @@
 
 void sensorsInit();
 bool sensorsTest();
-bool sensorsAcquire(sensorData_t *sensors);
+bool sensorsAcquire(sensorData_t *sensors, const uint32_t tick);
 
 #endif //__SENSORS_H__
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
@@ -143,11 +143,11 @@ typedef struct setpointZ_s {
 #include "FreeRTOS.h"
 #include "task.h"
 
-#define _RATE_SKIP_HZ(X) ((xTaskGetTickCount() % (1000/X)) != 0)
+#define _RATE_SKIP_HZ(T, X) ((T % (1000/X)) != 0)
 
-#define RATE_SKIP_500HZ() _RATE_SKIP_HZ(500)
-#define RATE_SKIP_250HZ() _RATE_SKIP_HZ(250)
-#define RATE_SKIP_100HZ() _RATE_SKIP_HZ(100)
+#define RATE_SKIP_500HZ(tick) _RATE_SKIP_HZ(tick, 500)
+#define RATE_SKIP_250HZ(tick) _RATE_SKIP_HZ(tick, 250)
+#define RATE_SKIP_100HZ(tick) _RATE_SKIP_HZ(tick, 100)
 
 
 #endif
diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
@@ -28,9 +28,10 @@ bool stateControllerTest(void)
 
 void stateController(control_t *control, const sensorData_t *sensors,
                                          const state_t *state,
-                                         const setpoint_t *setpoint)
+                                         const setpoint_t *setpoint,
+                                         const uint32_t tick)
 {
-  if (!RATE_SKIP_500HZ()) {
+  if (!RATE_SKIP_500HZ(tick)) {
     // Rate-controled YAW is moving YAW angle setpoint
     if (setpoint->mode.yaw == modeVelocity) {
        attitudeDesired.yaw -= setpoint->attitudeRate.yaw/500.0;
@@ -43,7 +44,7 @@ void stateController(control_t *control, const sensorData_t *sensors,
     }
   }
 
-  if (!RATE_SKIP_100HZ()) {
+  if (!RATE_SKIP_100HZ(tick)) {
     positionController(&actuatorThrust, &attitudeDesired, state, setpoint);
   }
 
@@ -56,13 +57,13 @@ void stateController(control_t *control, const sensorData_t *sensors,
     attitudeDesired.pitch = setpoint->attitude.pitch;
   }
 
-  if (!RATE_SKIP_500HZ()) {
+  if (!RATE_SKIP_500HZ(tick)) {
     attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
                                 setpoint->attitude.roll, setpoint->attitude.pitch, attitudeDesired.yaw,
                                 &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
   }
 
-  if (!RATE_SKIP_500HZ()) {
+  if (!RATE_SKIP_500HZ(tick)) {
     if (setpoint->mode.roll == modeVelocity) {
       rateDesired.roll = setpoint->attitudeRate.roll;
     }

diff --git a/src/modules/src/estimator_complementary.c b/src/modules/src/estimator_complementary.c
@@ -25,9 +25,9 @@ bool stateEstimatorTest(void)
   return pass;
 }
 
-void stateEstimator(state_t *state, const sensorData_t *sensorData)
+void stateEstimator(state_t *state, const sensorData_t *sensorData, const uint32_t tick)
 {
-  if (!RATE_SKIP_250HZ()) {
+  if (!RATE_SKIP_250HZ(tick)) {
     sensfusion6UpdateQ(sensorData->gyro.x, sensorData->gyro.y, sensorData->gyro.z,
                        sensorData->acc.x, sensorData->acc.y, sensorData->acc.z,
                        ATTITUDE_UPDATE_DT);
@@ -40,7 +40,7 @@ void stateEstimator(state_t *state, const sensorData_t *sensorData)
     positionUpdateVelocity(state->acc.z, ATTITUDE_UPDATE_DT);
   }
 
-  if (!RATE_SKIP_100HZ()) {
+  if (!RATE_SKIP_100HZ(tick)) {
     positionEstimate(state, sensorData->baro.asl, POS_UPDATE_DT);
   }
 }
diff --git a/src/modules/src/sensors_stock.c b/src/modules/src/sensors_stock.c
@@ -46,9 +46,9 @@ bool sensorsTest(void)
  return pass;
 }
 
-bool sensorsAcquire(sensorData_t *sensors)
+bool sensorsAcquire(sensorData_t *sensors, const uint32_t tick)
 {
- if (RATE_SKIP_500HZ()) {
+ if (RATE_SKIP_500HZ(tick)) {
    return imu6IsCalibrated();
  }
 

diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
@@ -88,6 +88,7 @@ bool stabilizerTest(void)
  */
 static void stabilizerTask(void* param)
 {
+  uint32_t tick;
   uint32_t lastWakeTime;
   vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);
 
@@ -100,16 +101,17 @@ static void stabilizerTask(void* param)
   {
     vTaskDelayUntil(&lastWakeTime, F2T(1000));
 
-    if (sensorsAcquire(&sensorData))  // Test if the sensors are calibrated
+    if (sensorsAcquire(&sensorData, tick))  // Test if the sensors are calibrated
     {
-      stateEstimator(&state, &sensorData);
+      stateEstimator(&state, &sensorData, tick);
       commanderGetSetpoint(&setpoint, &state);
 
       sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
 
-      stateController(&control, &sensorData, &state, &setpoint);
+      stateController(&control, &sensorData, &state, &setpoint, tick);
       powerDistribution(&control);
     }
+    tick++;
   }
 }