From db9042757da5f302455965eb8c54b8803bf0bc43 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 22 Apr 2014 20:14:52 +0100 Subject: [PATCH] Remove sensor_gyro.c's dependency on mw.h/board.h. --- src/flight_imu.c | 2 +- src/sensors_gyro.c | 20 ++++++++++++++------ src/sensors_gyro.h | 3 +-- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/flight_imu.c b/src/flight_imu.c index 8862986405..a144a1428d 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -92,7 +92,7 @@ void computeIMU(void) uint32_t axis; static int16_t gyroYawSmooth = 0; - gyroGetADC(); + gyroGetADC(masterConfig.moron_threshold); if (sensors(SENSOR_ACC)) { updateAccelerationReadings(¤tProfile.accelerometerTrims); getEstimatedAttitude(); diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index ba8903466b..f2c79b3226 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -1,10 +1,18 @@ -#include "board.h" -#include "mw.h" +#include +#include +#include "platform.h" + +#include "common/axis.h" #include "common/maths.h" +#include "drivers/accgyro_common.h" #include "flight_common.h" +#include "sensors_common.h" #include "statusindicator.h" +#include "boardalignment.h" + +#include "sensors_gyro.h" uint16_t calibratingG = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration. @@ -16,7 +24,7 @@ void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired) calibratingG = calibrationCyclesRequired; } -void gyroCommon(void) +static void gyroCommon(uint8_t gyroMovementCalibrationThreshold) { int axis; static int32_t g[3]; @@ -38,7 +46,7 @@ void gyroCommon(void) if (calibratingG == 1) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover if idiot was moving the model - if (masterConfig.moron_threshold && dev > masterConfig.moron_threshold) { + if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { calibratingG = CALIBRATING_GYRO_CYCLES; devClear(&var[0]); devClear(&var[1]); @@ -56,10 +64,10 @@ void gyroCommon(void) gyroADC[axis] -= gyroZero[axis]; } -void gyroGetADC(void) +void gyroGetADC(uint8_t gyroMovementCalibrationThreshold) { // range: +/- 8192; +/- 2000 deg/sec gyro.read(gyroADC); alignSensors(gyroADC, gyroADC, gyroAlign); - gyroCommon(); + gyroCommon(gyroMovementCalibrationThreshold); } diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h index 4e55f1a0cd..ee5683d99e 100644 --- a/src/sensors_gyro.h +++ b/src/sensors_gyro.h @@ -5,6 +5,5 @@ extern gyro_t gyro; extern sensor_align_e gyroAlign; void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); -void gyroCommon(void); -void gyroGetADC(void); +void gyroGetADC(uint8_t gyroMovementCalibrationThreshold);