Merge pull request #602 from martinbudden/bf_gyro_tidy

gyro sync and initialisation cleanup

src/main/blackbox/blackbox.c

@@ -325,9 +325,6 @@ extern uint32_t currentTime;
 //From rx.c:
 extern uint16_t rssi;
 
-//From gyro.c
-extern uint32_t targetLooptime;
-
 //From rc_controls.c
 extern uint32_t rcModeActivationMask;
 
@@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo()
             }
             );
 
-        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                         targetLooptime);
+        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                         gyro.targetLooptime);
         BLACKBOX_PRINT_HEADER_LINE("rcRate:%d",                           masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
         BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d",                           masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
         BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d",                        masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);

src/main/blackbox/blackbox_io.c

@@ -22,7 +22,6 @@
 #include "drivers/accgyro.h"
 #include "drivers/light_led.h"
 #include "drivers/sound_beeper.h"
-#include "drivers/gyro_sync.h"
 
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"

src/main/config/config.c

@@ -38,7 +38,6 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/serial.h"
-#include "drivers/gyro_sync.h"
 #include "drivers/pwm_output.h"
 #include "drivers/max7456.h"
 
@@ -751,7 +750,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
+    gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/drivers/accgyro.h

@@ -27,6 +27,7 @@ typedef struct gyro_s {
     sensorReadFuncPtr temperature;                          // read temperature if available
     sensorInterruptFuncPtr intStatus;
     float scale;                                            // scalefactor
+    uint32_t targetLooptime;
 } gyro_t;
 
 typedef struct acc_s {

src/main/drivers/accgyro_mpu.c

@@ -32,7 +32,6 @@
 #include "gpio.h"
 #include "exti.h"
 #include "bus_i2c.h"
-#include "gyro_sync.h"
 
 #include "sensor.h"
 #include "accgyro.h"
@@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC)
     return true;
 }
 
-void checkMPUDataReady(bool *mpuDataReadyPtr) {
+bool checkMPUDataReady(void)
+{
+    bool ret;
     if (mpuDataReady) {
-        *mpuDataReadyPtr = true;
+        ret = true;
         mpuDataReady= false;
     } else {
-        *mpuDataReadyPtr = false;
+        ret = false;
     }
+    return ret;
 }

src/main/drivers/accgyro_mpu.h

@@ -185,4 +185,4 @@ void mpuIntExtiInit(void);
 bool mpuAccRead(int16_t *accData);
 bool mpuGyroRead(int16_t *gyroADC);
 mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
-void checkMPUDataReady(bool *mpuDataReadyPtr);
+bool checkMPUDataReady(void);

src/main/drivers/accgyro_mpu3050.c

@@ -30,7 +30,6 @@
 #include "accgyro.h"
 #include "accgyro_mpu.h"
 #include "accgyro_mpu3050.h"
-#include "gyro_sync.h"
 
 // MPU3050, Standard address 0x68
 #define MPU3050_ADDRESS         0x68

src/main/drivers/accgyro_mpu6050.c

@@ -1,4 +1,4 @@
-/*
+/*
  * This file is part of Cleanflight.
  *
  * Cleanflight is free software: you can redistribute it and/or modify

src/main/drivers/gyro_sync.c

@@ -4,48 +4,40 @@
  *  Created on: 3 aug. 2015
  *      Author: borisb
  */
+
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
 
 #include "platform.h"
 #include "build_config.h"
 
-#include "common/axis.h"
-#include "common/maths.h"
-
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/gyro_sync.h"
 
-#include "sensors/sensors.h"
-#include "sensors/acceleration.h"
-
-#include "config/runtime_config.h"
-#include "config/config.h"
-
-extern gyro_t gyro;
-
-uint32_t targetLooptime;
 static uint8_t mpuDividerDrops;
 
-bool getMpuDataStatus(gyro_t *gyro)
+bool gyroSyncCheckUpdate(const gyro_t *gyro)
 {
-    bool mpuDataStatus;
     if (!gyro->intStatus)
         return false;
-    gyro->intStatus(&mpuDataStatus);
+    return gyro->intStatus();
-    return mpuDataStatus;
 }
 
-bool gyroSyncCheckUpdate(void) {
+#define GYRO_LPF_256HZ      0
-    return getMpuDataStatus(&gyro);
+#define GYRO_LPF_188HZ      1
-}
+#define GYRO_LPF_98HZ       2
+#define GYRO_LPF_42HZ       3
+#define GYRO_LPF_20HZ       4
+#define GYRO_LPF_10HZ       5
+#define GYRO_LPF_5HZ        6
+#define GYRO_LPF_NONE       7
 
-void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
+uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
+{
     int gyroSamplePeriod;
 
-    if (!lpf || lpf == 7) {
+    if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
         gyroSamplePeriod = 125;
     } else {
         gyroSamplePeriod = 1000;
@@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
 
     // calculate gyro divider and targetLooptime (expected cycleTime)
     mpuDividerDrops  = gyroSyncDenominator - 1;
-    targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
+    const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
+    return targetLooptime;
 }
 
-uint8_t gyroMPU6xxxGetDividerDrops(void) {
+uint8_t gyroMPU6xxxGetDividerDrops(void)
+{
     return mpuDividerDrops;
 }

src/main/drivers/gyro_sync.h

@@ -5,8 +5,7 @@
  *      Author: borisb
  */
 
-extern uint32_t targetLooptime;
+struct gyro_s;
-
+bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
-bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxGetDividerDrops(void);
-void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
+uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

src/main/drivers/sensor.h

@@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void);                    // sensor init proto
 typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
 typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc);    // sensor init prototype
 typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf);         // gyro sensor init prototype
-typedef void (*sensorInterruptFuncPtr)(bool *data);         // sensor Interrupt Data Ready
+typedef bool (*sensorInterruptFuncPtr)(void);               // sensor Interrupt Data Ready

src/main/flight/mixer.c

@@ -36,7 +36,6 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/system.h"
-#include "drivers/gyro_sync.h"
 
 #include "rx/rx.h"
 

src/main/flight/pid.c

@@ -29,7 +29,6 @@
 #include "common/filter.h"
 
 #include "drivers/sensor.h"
-#include "drivers/gyro_sync.h"
 
 #include "drivers/accgyro.h"
 #include "sensors/sensors.h"
@@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we
 
 void setTargetPidLooptime(uint8_t pidProcessDenom) 
 {
-    targetPidLooptime = targetLooptime * pidProcessDenom;
+    targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
 }
 
 uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate) 

src/main/io/osd.c

@@ -52,7 +52,6 @@
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
 #include "drivers/sonar_hcsr04.h"
-#include "drivers/gyro_sync.h"
 #include "drivers/usb_io.h"
 #include "drivers/transponder_ir.h"
 #include "drivers/sdcard.h"

src/main/io/rc_controls.c

@@ -34,7 +34,6 @@
 #include "drivers/system.h"
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "drivers/gyro_sync.h"
 
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
@@ -210,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
 
     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
         // GYRO calibration
-        gyroSetCalibrationCycles(calculateCalibratingCycles());
+        gyroSetCalibrationCycles();
 
 #ifdef GPS
         if (feature(FEATURE_GPS)) {

src/main/io/serial_cli.c

@@ -48,7 +48,6 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/sdcard.h"
-#include "drivers/gyro_sync.h"
 
 #include "drivers/buf_writer.h"
 

src/main/io/serial_msp.c

@@ -40,7 +40,6 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
-#include "drivers/gyro_sync.h"
 #include "drivers/sdcard.h"
 #include "drivers/buf_writer.h"
 #include "drivers/max7456.h"
@@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
     uint16_t gyroSampleRate = 1000;
     uint8_t maxDivider = 1;
 
-    if (looptime != targetLooptime || looptime == 0) {
+    if (looptime != gyro.targetLooptime || looptime == 0) {
-        if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes
+        if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
 #ifdef STM32F303xC
         if (looptime < 1000) {
             masterConfig.gyro_lpf = 0;
@@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_LOOP_TIME:
         headSerialReply(2);
-        serialize16((uint16_t)targetLooptime);
+        serialize16((uint16_t)gyro.targetLooptime);
         break;
     case MSP_RC_TUNING:
         headSerialReply(11);

src/main/main.c

@@ -49,7 +49,6 @@
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
 #include "drivers/sonar_hcsr04.h"
-#include "drivers/gyro_sync.h"
 #include "drivers/sdcard.h"
 #include "drivers/usb_io.h"
 #include "drivers/transponder_ir.h"
@@ -625,7 +624,7 @@ void init(void)
     if (masterConfig.mixerMode == MIXER_GIMBAL) {
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
     }
-    gyroSetCalibrationCycles(calculateCalibratingCycles());
+    gyroSetCalibrationCycles();
 #ifdef BARO
     baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
 #endif
@@ -693,12 +692,12 @@ void main_init(void)
 
     /* Setup scheduler */
     schedulerInit();
-    rescheduleTask(TASK_GYROPID, targetLooptime);
+    rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
     setTaskEnabled(TASK_GYROPID, true);
 
     if(sensors(SENSOR_ACC)) {
         setTaskEnabled(TASK_ACCEL, true);
-        switch(targetLooptime) {  // Switch statement kept in place to change acc rates in the future
+        switch(gyro.targetLooptime) {  // Switch statement kept in place to change acc rates in the future
              case(500):
              case(375):
              case(250):

src/main/mw.c

@@ -377,7 +377,7 @@ void mwArm(void)
     static bool firstArmingCalibrationWasCompleted;
 
     if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
-        gyroSetCalibrationCycles(calculateCalibratingCycles());
+        gyroSetCalibrationCycles();
         armingCalibrationWasInitialised = true;
         firstArmingCalibrationWasCompleted = true;
     }
@@ -802,7 +802,7 @@ void taskMainPidLoopCheck(void)
 
     const uint32_t startTime = micros();
     while (true) {
-        if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
+        if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
             static uint8_t pidUpdateCountdown;
 
             if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting

src/main/rx/rx.c

@@ -41,7 +41,6 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/system.h"
-#include "drivers/gyro_sync.h"
 #include "rx/pwm.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"

src/main/sensors/acceleration.c

@@ -28,7 +28,6 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/system.h"
-#include "drivers/gyro_sync.h"
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"

src/main/sensors/gyro.c

@@ -28,7 +28,6 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "drivers/gyro_sync.h"
 #include "sensors/sensors.h"
 #include "io/beeper.h"
 #include "io/statusindicator.h"
@@ -36,36 +35,31 @@
 
 #include "sensors/gyro.h"
 
-uint16_t calibratingG = 0;
-int32_t gyroADC[XYZ_AXIS_COUNT];
-float gyroADCf[XYZ_AXIS_COUNT];
-int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
-
-static gyroConfig_t *gyroConfig;
-static biquad_t gyroFilterState[3];
-static bool gyroFilterStateIsSet;
-static float gyroLpfCutFreq;
-
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
+int32_t gyroADC[XYZ_AXIS_COUNT];
+float gyroADCf[XYZ_AXIS_COUNT];
+
+static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
+static const gyroConfig_t *gyroConfig;
+static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
+static uint8_t gyroSoftLpfHz;
+static uint16_t calibratingG = 0;
+
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
 {
     gyroConfig = gyroConfigToUse;
-    gyroLpfCutFreq = gyro_lpf_hz;
+    gyroSoftLpfHz = gyro_soft_lpf_hz;
 }
 
-void initGyroFilterCoefficients(void) {
+void gyroInit(void)
-    int axis;
-    if (gyroLpfCutFreq && targetLooptime) {  /* Initialisation needs to happen once samplingrate is known */
-        for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
-        gyroFilterStateIsSet = true;
-    }
-}
-
-void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
-    calibratingG = calibrationCyclesRequired;
+    if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
+        for (int axis = 0; axis < 3; axis++) {
+            BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
+        }
+    }
 }
 
 bool isGyroCalibrationComplete(void)
@@ -73,27 +67,32 @@ bool isGyroCalibrationComplete(void)
     return calibratingG == 0;
 }
 
-bool isOnFinalGyroCalibrationCycle(void)
+static bool isOnFinalGyroCalibrationCycle(void)
 {
     return calibratingG == 1;
 }
 
-uint16_t calculateCalibratingCycles(void) {
+static uint16_t gyroCalculateCalibratingCycles(void)
-    return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
+{
+    return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
 }
 
-bool isOnFirstGyroCalibrationCycle(void)
+static bool isOnFirstGyroCalibrationCycle(void)
 {
-    return calibratingG == calculateCalibratingCycles();
+    return calibratingG == gyroCalculateCalibratingCycles();
+}
+
+void gyroSetCalibrationCycles(void)
+{
+    calibratingG = gyroCalculateCalibratingCycles();
 }
 
 static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
 {
-    int8_t axis;
     static int32_t g[3];
     static stdev_t var[3];
 
-    for (axis = 0; axis < 3; axis++) {
+    for (int axis = 0; axis < 3; axis++) {
 
         // Reset g[axis] at start of calibration
         if (isOnFirstGyroCalibrationCycle()) {
@@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
             float dev = devStandardDeviation(&var[axis]);
             // check deviation and startover in case the model was moved
             if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
-                gyroSetCalibrationCycles(calculateCalibratingCycles());
+                gyroSetCalibrationCycles();
                 return;
             }
-            gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles();
+            gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
         }
     }
 
@@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
 
 static void applyGyroZero(void)
 {
-    int8_t axis;
+    for (int axis = 0; axis < 3; axis++) {
-    for (axis = 0; axis < 3; axis++) {
         gyroADC[axis] -= gyroZero[axis];
     }
 }
@@ -138,14 +136,13 @@ static void applyGyroZero(void)
 void gyroUpdate(void)
 {
     int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-    int axis;
 
     // range: +/- 8192; +/- 2000 deg/sec
     if (!gyro.read(gyroADCRaw)) {
         return;
     }
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
         gyroADC[axis] = gyroADCRaw[axis];
     }
@@ -158,16 +155,10 @@ void gyroUpdate(void)
 
     applyGyroZero();
 
-    if (gyroLpfCutFreq) {
+    if (gyroSoftLpfHz) {
-        if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-
+            gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
-        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            if (gyroFilterStateIsSet) {
-                gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
             gyroADC[axis] = lrintf(gyroADCf[axis]);
-            } else {
-                gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled
-            }
         }
     }
 }

src/main/sensors/gyro.h

@@ -31,19 +31,17 @@ typedef enum {
 } gyroSensor_e;
 
 extern gyro_t gyro;
-extern sensor_align_e gyroAlign;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];
 extern float gyroADCf[XYZ_AXIS_COUNT];
-extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
 
 typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
+void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
-void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+void gyroSetCalibrationCycles(void);
+void gyroInit(void);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);
-uint16_t calculateCalibratingCycles(void);
 

src/main/sensors/initialisation.c

@@ -79,6 +79,7 @@ extern float magneticDeclination;
 extern gyro_t gyro;
 extern baro_t baro;
 extern acc_t acc;
+extern sensor_align_e gyroAlign;
 
 uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
 
@@ -632,8 +633,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
         acc.init(&acc);
     }
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator);    // Set gyro refresh rate before initialisation
+    gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator);    // Set gyro sample rate before initialisation
     gyro.init(gyroLpf);
+    gyroInit();
 
     detectMag(magHardwareToUse);