Merge pull request cleanflight#2116 from martinbudden/tidy_gyro

Cleanup of gyro_sync.c

src/main/drivers/gyro_sync.c

@@ -34,31 +34,31 @@
 extern gyro_t gyro;
 
 uint32_t targetLooptime;
-uint8_t mpuDividerDrops;
+static uint8_t mpuDividerDrops;
 
-bool gyroSyncCheckUpdate(void) {
+bool gyroSyncCheckUpdate(void)
+{
     return gyro.isDataReady && gyro.isDataReady();
 }
 
-void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator) {
-    int gyroSamplePeriod;
-
+void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
+{
     if (gyroSync) {
-        if (!lpf) {
+        int gyroSamplePeriod;
+        if (lpf == 0) {
             gyroSamplePeriod = 125;
-
         } else {
             gyroSamplePeriod = 1000;
         }
-
         mpuDividerDrops = gyroSyncDenominator - 1;
-        targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
+        targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
     } else {
-    	mpuDividerDrops = 0;
-    	targetLooptime = looptime;
+        mpuDividerDrops = 0;
+        targetLooptime = looptime;
     }
 }
 
-uint8_t gyroMPU6xxxCalculateDivider(void) {
+uint8_t gyroMPU6xxxCalculateDivider(void)
+{
     return mpuDividerDrops;
 }

src/main/drivers/gyro_sync.h

@@ -21,4 +21,4 @@ extern uint32_t targetLooptime;
 
 bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxCalculateDivider(void);
-void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
+void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

src/main/io/serial_cli.c

@@ -592,7 +592,7 @@ const clivalue_t valueTable[] = {
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100,  900 } , PG_IMU_CONFIG, offsetof(imuConfig_t, max_angle_inclination) },
 
     { "gyro_lpf",                   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_LPF } , PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lpf)},
-    { "gyro_soft_lpf",              VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0,  500 } , PG_GYRO_CONFIG, offsetof(gyroConfig_t, soft_gyro_lpf_hz)},
+    { "gyro_soft_lpf",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0,  500 } , PG_GYRO_CONFIG, offsetof(gyroConfig_t, soft_gyro_lpf_hz)},
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  128 } , PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold)},
     { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0,  20000 } , PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp)},
     { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0,  20000 } , PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki)},

src/main/main.c

@@ -483,7 +483,7 @@ void init(void)
     }
 #endif
 
-    gyroUpdateSampleRate(imuConfig()->looptime, gyroConfig()->gyro_lpf, imuConfig()->gyroSync, imuConfig()->gyroSyncDenominator);   // Set gyro sampling rate divider before initialization
+    gyroSetSampleRate(imuConfig()->looptime, gyroConfig()->gyro_lpf, imuConfig()->gyroSync, imuConfig()->gyroSyncDenominator);   // Set gyro sampling rate divider before initialization
 
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, we give up now.

src/main/sensors/gyro.c

@@ -43,17 +43,18 @@
 
 #include "sensors/gyro.h"
 
-uint16_t calibratingG = 0;
-int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+gyro_t gyro;                      // gyro access functions
+sensor_align_e gyroAlign = 0;
+
 int32_t gyroADC[XYZ_AXIS_COUNT];
-int32_t gyroZero[FD_INDEX_COUNT] = { 0, 0, 0 };
+
+
+static uint16_t calibratingG = 0;
+static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
 
 static biquad_t gyroFilterState[3];
 static bool gyroFilterStateIsSet;
-int axis;
-
-gyro_t gyro;                      // gyro access functions
-sensor_align_e gyroAlign = 0;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
 
@@ -64,13 +65,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyroMovementCalibrationThreshold = 32,
 );
 
-void initGyroFilterCoefficients(void) {
+static void initGyroFilterCoefficients(void)
+{
     if (gyroConfig()->soft_gyro_lpf_hz) {
         // Initialisation needs to happen once sampling rate is known
-        for (axis = 0; axis < 3; axis++) {
+        for (int axis = 0; axis < 3; axis++) {
             BiQuadNewLpf(gyroConfig()->soft_gyro_lpf_hz, &gyroFilterState[axis], targetLooptime);
         }
-
         gyroFilterStateIsSet = true;
     }
 }
@@ -85,23 +86,22 @@ bool isGyroCalibrationComplete(void)
     return calibratingG == 0;
 }
 
-bool isOnFinalGyroCalibrationCycle(void)
+static bool isOnFinalGyroCalibrationCycle(void)
 {
     return calibratingG == 1;
 }
 
-bool isOnFirstGyroCalibrationCycle(void)
+static bool isOnFirstGyroCalibrationCycle(void)
 {
     return calibratingG == CALIBRATING_GYRO_CYCLES;
 }
 
 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()) {
@@ -132,13 +132,11 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
         beeper(BEEPER_GYRO_CALIBRATED);
     }
     calibratingG--;
-
 }
 
 static void applyGyroZero(void)
 {
-    int8_t axis;
-    for (axis = 0; axis < 3; axis++) {
+    for (int axis = 0; axis < 3; axis++) {
         gyroADC[axis] -= gyroZero[axis];
     }
 }
@@ -151,7 +149,7 @@ void gyroUpdate(void)
     }
 
     // Prepare a copy of int32_t gyroADC for mangling to prevent overflow
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         gyroADC[axis] = gyroADCRaw[axis];
     }
 
@@ -161,11 +159,8 @@ void gyroUpdate(void)
         if (!gyroFilterStateIsSet) {
             initGyroFilterCoefficients();
         }
-
-        if (gyroFilterStateIsSet) {
-            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
-            }
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            gyroADC[axis] = lrintf(applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]));
         }
     }
 

src/main/sensors/gyro.h

@@ -33,12 +33,11 @@ extern gyro_t gyro;
 extern sensor_align_e gyroAlign;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];
-extern int32_t gyroZero[FD_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.
-    uint8_t gyro_lpf;                       // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
-    float soft_gyro_lpf_hz;                 // Software based gyro filter in hz
+    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.
+    uint8_t gyro_lpf;                           // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
+    uint16_t soft_gyro_lpf_hz;                  // Software based gyro filter in hz
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);