Moved gyro and acc detection code into init functions

src/main/sensors/acceleration.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 #include <math.h>
 
 #include "platform.h"
@@ -226,8 +227,12 @@ retry:
     return true;
 }
 
-void accInit(uint32_t gyroSamplingInverval)
+bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval)
 {
+    memset(&acc, 0, sizeof(acc));
+    if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
+        return false;
+    }
     acc.dev.acc_1G = 256; // set default
     acc.dev.init(&acc.dev); // driver initialisation
     // set the acc sampling interval according to the gyro sampling interval
@@ -251,6 +256,7 @@ void accInit(uint32_t gyroSamplingInverval)
             biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
         }
     }
+    return true;
 }
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)

src/main/sensors/acceleration.h

@@ -61,8 +61,7 @@ typedef struct accelerometerConfig_s {
     flightDynamicsTrims_t accZero;
 } accelerometerConfig_t;
 
-bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse);
+bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime);
-void accInit(uint32_t gyroTargetLooptime);
 bool isAccelerationCalibrationComplete(void);
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);

src/main/sensors/gyro.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 #include <math.h>
 
 #include "platform.h"
@@ -88,13 +89,8 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
 #endif
 }
 
-bool gyroDetect(gyroDev_t *dev)
+static bool gyroDetect(gyroDev_t *dev)
 {
-#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
-    const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
-    mpuDetect(extiConfig);
-#endif
-
     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 
     dev->gyroAlign = ALIGN_DEFAULT;
@@ -230,7 +226,7 @@ case GYRO_MPU9250:
     return true;
 }
 
-void gyroInit(const gyroConfig_t *gyroConfigToUse)
+bool gyroInit(const gyroConfig_t *gyroConfigToUse)
 {
     static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
     static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
@@ -239,6 +235,15 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse)
     static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
 
     gyroConfig = gyroConfigToUse;
+    memset(&gyro, 0, sizeof(gyro));
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
+    const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
+    mpuDetect(extiConfig);
+#endif
+
+    if (!gyroDetect(&gyro.dev)) {
+        return false;
+    }
     gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom);    // Set gyro sample rate before initialisation
     gyro.dev.lpf = gyroConfig->gyro_lpf;
     gyro.dev.init(&gyro.dev);
@@ -286,6 +291,7 @@ void gyroInit(const gyroConfig_t *gyroConfigToUse)
             biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
         }
     }
+    return true;
 }
 
 bool isGyroCalibrationComplete(void)

src/main/sensors/gyro.h

@@ -55,9 +55,7 @@ typedef struct gyroConfig_s {
     uint16_t gyro_soft_notch_cutoff_2;
 } gyroConfig_t;
 
-bool gyroDetect(gyroDev_t *dev);
 void gyroSetCalibrationCycles(void);
-void gyroInit(const gyroConfig_t *gyroConfigToUse);
+bool gyroInit(const gyroConfig_t *gyroConfigToUse);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);
-

src/main/sensors/initialisation.c

@@ -14,26 +14,17 @@
  * You should have received a copy of the GNU General Public License
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
+
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
 
 #include "platform.h"
 
-#include "common/axis.h"
 #include "common/utils.h"
 
 #include "config/feature.h"
 
-#include "drivers/accgyro_mpu.h"
-#include "drivers/system.h"
-#include "drivers/exti.h"
-#include "drivers/sensor.h"
-#include "drivers/accgyro.h"
-#include "drivers/barometer.h"
-#include "drivers/compass.h"
-#include "drivers/sonar_hcsr04.h"
-
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
@@ -45,11 +36,6 @@
 #include "sensors/sonar.h"
 #include "sensors/initialisation.h"
 
-#ifdef USE_HARDWARE_REVISION_DETECTION
-#include "hardware_revision.h"
-#endif
-
-
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
 
 
@@ -72,17 +58,12 @@ bool sensorsAutodetect(const gyroConfig_t *gyroConfig,
         const barometerConfig_t *barometerConfig,
         const sonarConfig_t *sonarConfig)
 {
-    memset(&gyro, 0, sizeof(gyro));
+    // gyro must be initialised before accelerometer
-    if (!gyroDetect(&gyro.dev)) {
+    if (!gyroInit(gyroConfig)) {
         return false;
     }
-    // gyro must be initialised before accelerometer
-    gyroInit(gyroConfig);
 
-    memset(&acc, 0, sizeof(acc));
+    accInit(accelerometerConfig, gyro.targetLooptime);
-    if (accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
-        accInit(gyro.targetLooptime);
-    }
 
     mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
 #ifdef MAG