diff --git a/src/flight_common.c b/src/flight_common.c index e554e7a33f..0bcd0b525b 100644 --- a/src/flight_common.c +++ b/src/flight_common.c @@ -1,5 +1,7 @@ -#include "board.h" -#include "mw.h" +#include +#include + +#include "common/axis.h" #include "runtime_config.h" diff --git a/src/flight_imu.c b/src/flight_imu.c index 7889ba8dac..5c76e1c000 100644 --- a/src/flight_imu.c +++ b/src/flight_imu.c @@ -1,33 +1,63 @@ -#include "board.h" -#include "mw.h" +#include +#include +#include #include "common/maths.h" +#include "platform.h" +// +#include "common/axis.h" +#include "flight_common.h" +// +#include "drivers/system_common.h" +// +#include "sensors_common.h" #include "sensors_gyro.h" #include "sensors_compass.h" +#include "sensors_acceleration.h" +#include "sensors_barometer.h" + +#include "flight_mixer.h" + +#include "boardalignment.h" +#include "battery.h" +#include "rx_common.h" +#include "drivers/serial_common.h" +#include "serial_common.h" +#include "failsafe.h" +#include "runtime_config.h" +#include "config.h" +#include "config_storage.h" -#include "flight_common.h" int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT]; int32_t accSum[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; +float accVelScale; + int16_t smallAngle = 0; + +int32_t sonarAlt; // to think about the unit +int32_t EstAlt; // in cm +int32_t AltHold; +int32_t errorAltitudeI = 0; + +int32_t vario = 0; // variometer in cm/s + +int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, +float throttleAngleScale; + +uint16_t calibratingB = 0; // baro calibration = get new ground pressure value int32_t baroPressure = 0; int32_t baroTemperature = 0; uint32_t baroPressureSum = 0; -int32_t BaroAlt = 0; -int32_t sonarAlt; // to think about the unit -int32_t EstAlt; // in cm int32_t BaroPID = 0; -int32_t AltHold; -int32_t errorAltitudeI = 0; -int32_t vario = 0; // variometer in cm/s -int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind, +int32_t BaroAlt = 0; + float magneticDeclination = 0.0f; // calculated at startup from config -float accVelScale; -float throttleAngleScale; + // ************** // gyro+acc IMU @@ -350,7 +380,6 @@ int getEstimatedAltitude(void) int32_t error; int32_t baroVel; int32_t vel_tmp; - int32_t BaroAlt_tmp; int32_t setVel; float dt; float vel_acc; @@ -359,29 +388,18 @@ int getEstimatedAltitude(void) static float vel = 0.0f; static float accAlt = 0.0f; static int32_t lastBaroAlt; - static int32_t baroGroundAltitude = 0; - static int32_t baroGroundPressure = 0; dTime = currentT - previousT; if (dTime < UPDATE_INTERVAL) return 0; previousT = currentT; - if (calibratingB > 0) { - baroGroundPressure -= baroGroundPressure / 8; - baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); - baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; - + if (!isBaroCalibrationComplete()) { + performBaroCalibrationCycle(); vel = 0; accAlt = 0; - calibratingB--; } - - // calculates height from ground via baro readings - // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 - BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm - BaroAlt_tmp -= baroGroundAltitude; - BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise + BaroAlt = Baro_calculateAltitude(); dt = accTimeSum * 1e-6f; // delta acc reading time in seconds diff --git a/src/sensors_barometer.c b/src/sensors_barometer.c index 9a16d9d596..0799eea3d0 100644 --- a/src/sensors_barometer.c +++ b/src/sensors_barometer.c @@ -1,9 +1,14 @@ #include "board.h" #include "mw.h" +#include "drivers/barometer_common.h" + baro_t baro; // barometer access functions #ifdef BARO +static int32_t baroGroundAltitude = 0; +static int32_t baroGroundPressure = 0; + void Baro_Common(void) { static int32_t baroHistTab[BARO_TAB_SIZE_MAX]; @@ -46,4 +51,31 @@ int Baro_update(void) return 1; } } + +int32_t Baro_calculateAltitude(void) +{ + int32_t BaroAlt_tmp; + + // calculates height from ground via baro readings + // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140 + BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm + BaroAlt_tmp -= baroGroundAltitude; + BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise + + return BaroAlt; +} + +void performBaroCalibrationCycle(void) +{ + baroGroundPressure -= baroGroundPressure / 8; + baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1); + baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f; + + calibratingB--; +} + +bool isBaroCalibrationComplete(void) +{ + return calibratingB == 0; +} #endif /* BARO */ diff --git a/src/sensors_barometer.h b/src/sensors_barometer.h index bcbddb503f..c007560992 100644 --- a/src/sensors_barometer.h +++ b/src/sensors_barometer.h @@ -1,8 +1,9 @@ #pragma once -extern baro_t baro; - #ifdef BARO void Baro_Common(void); int Baro_update(void); +int32_t Baro_calculateAltitude(void); +bool isBaroCalibrationComplete(void); +void performBaroCalibrationCycle(void); #endif diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index 452540c157..4083ad9ffb 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -6,6 +6,8 @@ #include "flight_common.h" #include "statusindicator.h" +uint16_t calibratingG = 0; +uint16_t acc_1G = 256; // this is the 1G measured acceleration. sensor_t gyro; // gyro access functions void GYRO_Common(void) diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h index dbfe156801..66c903ac09 100644 --- a/src/sensors_gyro.h +++ b/src/sensors_gyro.h @@ -1,5 +1,6 @@ #pragma once +extern uint16_t acc_1G; extern sensor_t gyro; void GYRO_Common(void); diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index 14ad6de61e..68267046f9 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -7,15 +7,16 @@ #include "sensors_common.h" -uint16_t calibratingB = 0; // baro calibration = get new ground pressure value -uint16_t calibratingG = 0; -uint16_t acc_1G = 256; // this is the 1G measured acceleration. int16_t heading, magHold; extern uint16_t batteryWarningVoltage; extern uint8_t batteryCellCount; extern float magneticDeclination; +extern sensor_t gyro; +extern baro_t baro; +extern sensor_t acc; + #ifdef FY90Q // FY90Q analog gyro/acc void sensorsAutodetect(void)