From 6a03b48e7f65f7228c7566207144ca65f28d613c Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 29 Mar 2017 02:01:09 +1300 Subject: [PATCH] Reworked altitude estimation. Made it work when ACC is disabled. --- Makefile | 2 +- src/main/build/debug.h | 1 + src/main/fc/cli.c | 5 +- src/main/fc/config.c | 2 +- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_msp.c | 6 +- src/main/fc/fc_tasks.c | 2 +- .../flight/{altitudehold.c => altitude.c} | 183 ++++++++---------- .../flight/{altitudehold.h => altitude.h} | 5 +- src/main/flight/imu.h | 1 - src/main/io/osd.c | 8 +- src/main/rx/jetiexbus.c | 3 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/telemetry/frsky.c | 8 +- src/main/telemetry/hott.c | 7 +- src/main/telemetry/ltm.c | 4 +- src/main/telemetry/mavlink.c | 6 +- src/main/telemetry/smartport.c | 6 +- src/test/Makefile | 4 +- src/test/unit/altitude_hold_unittest.cc.txt | 2 +- src/test/unit/telemetry_hott_unittest.cc | 3 +- 21 files changed, 127 insertions(+), 135 deletions(-) rename src/main/flight/{altitudehold.c => altitude.c} (68%) rename src/main/flight/{altitudehold.h => altitude.h} (94%) diff --git a/Makefile b/Makefile index 2ab2c34f0..34f7e8436 100644 --- a/Makefile +++ b/Makefile @@ -694,7 +694,7 @@ COMMON_SRC = \ fc/rc_controls.c \ fc/runtime_config.c \ fc/cli.c \ - flight/altitudehold.c \ + flight/altitude.c \ flight/failsafe.c \ flight/imu.c \ flight/mixer.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index c7a0ea847..4882b9e96 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -64,5 +64,6 @@ typedef enum { DEBUG_STACK, DEBUG_ESC_SENSOR_RPM, DEBUG_ESC_SENSOR_TMP, + DEBUG_ALTITUDE, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index aade7d017..a5794dcb7 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -83,7 +83,7 @@ extern uint8_t __config_end; #include "fc/runtime_config.h" #include "fc/fc_msp.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -326,7 +326,8 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "SCHEDULER", "STACK", "ESC_SENSOR_RPM", - "ESC_SENSOR_TMP" + "ESC_SENSOR_TMP", + "ALTITUDE" }; #ifdef OSD diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b866382ab..b808ce704 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -67,7 +67,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 57b5c30f1..9ce039167 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -72,7 +72,7 @@ #include "telemetry/telemetry.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index cbcc376a4..5449f9262 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -62,7 +62,7 @@ #include "fc/rc_adjustments.h" #include "fc/runtime_config.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -700,11 +700,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_ALTITUDE: #if defined(BARO) || defined(SONAR) - sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude()); + sbufWriteU32(dst, getEstimatedAltitude()); #else sbufWriteU32(dst, 0); #endif - sbufWriteU16(dst, vario); + sbufWriteU16(dst, getEstimatedVario()); break; case MSP_SONAR_ALTITUDE: diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 72c57be18..d2181cc49 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -49,7 +49,7 @@ #include "fc/cli.h" #include "fc/fc_dispatch.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitude.c similarity index 68% rename from src/main/flight/altitudehold.c rename to src/main/flight/altitude.c index 7797bc498..ebd9d8453 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitude.c @@ -34,16 +34,23 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/imu.h" #include "flight/pid.h" #include "rx/rx.h" +#include "sensors/sensors.h" #include "sensors/barometer.h" #include "sensors/sonar.h" +enum { + DEBUG_ALTITUDE_ACC, + DEBUG_ALTITUDE_VEL, + DEBUG_ALTITUDE_HEIGHT +}; + PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, @@ -55,7 +62,8 @@ uint8_t velocityControl = 0; int32_t errorVelocityI = 0; int32_t altHoldThrottleAdjustment = 0; int32_t AltHold; -int32_t vario = 0; // variometer in cm/s +int32_t estimatedVario = 0; // variometer in cm/s +static int32_t estimatedAltitude = 0; // in cm static pidProfile_t *pidProfile; @@ -68,7 +76,6 @@ void configureAltitudeHold(pidProfile_t *initialPidProfile) #if defined(BARO) || defined(SONAR) static int16_t initialThrottleHold; -static int32_t EstAlt; // in cm // 40hz update rate (20hz LPF on acc) #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) @@ -87,7 +94,7 @@ static void applyMultirotorAltHold(void) rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband; } else { if (isAltHoldChanged) { - AltHold = EstAlt; + AltHold = estimatedAltitude; isAltHoldChanged = 0; } rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX); @@ -100,7 +107,7 @@ static void applyMultirotorAltHold(void) velocityControl = 1; isAltHoldChanged = 1; } else if (isAltHoldChanged) { - AltHold = EstAlt; + AltHold = estimatedAltitude; velocityControl = 0; isAltHoldChanged = 0; } @@ -136,7 +143,7 @@ void updateAltHoldState(void) if (!FLIGHT_MODE(BARO_MODE)) { ENABLE_FLIGHT_MODE(BARO_MODE); - AltHold = EstAlt; + AltHold = estimatedAltitude; initialThrottleHold = rcData[THROTTLE]; errorVelocityI = 0; altHoldThrottleAdjustment = 0; @@ -153,7 +160,7 @@ void updateSonarAltHoldState(void) if (!FLIGHT_MODE(SONAR_MODE)) { ENABLE_FLIGHT_MODE(SONAR_MODE); - AltHold = EstAlt; + AltHold = estimatedAltitude; initialThrottleHold = rcData[THROTTLE]; errorVelocityI = 0; altHoldThrottleAdjustment = 0; @@ -178,7 +185,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa // Altitude P-Controller if (!velocityControl) { - error = constrain(AltHold - EstAlt, -500, 500); + error = constrain(AltHold - estimatedAltitude, -500, 500); error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s } else { @@ -203,122 +210,104 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa void calculateEstimatedAltitude(timeUs_t currentTimeUs) { - static timeUs_t previousTimeUs; - uint32_t dTime; - int32_t baroVel; - float dt; - float vel_acc; - int32_t vel_tmp; - float accZ_tmp; - static float accZ_old = 0.0f; - static float vel = 0.0f; - static float accAlt = 0.0f; - static int32_t lastBaroAlt; - -#ifdef SONAR - int32_t sonarAlt = SONAR_OUT_OF_RANGE; - static int32_t baroAlt_offset = 0; - float sonarTransition; -#endif - - dTime = currentTimeUs - previousTimeUs; - if (dTime < BARO_UPDATE_FREQUENCY_40HZ) + static timeUs_t previousTimeUs = 0; + const uint32_t dTime = currentTimeUs - previousTimeUs; + if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { return; - + } previousTimeUs = currentTimeUs; + static float vel = 0.0f; + static float accAlt = 0.0f; + + int32_t baroAlt = 0; #ifdef BARO - if (!isBaroCalibrationComplete()) { - performBaroCalibrationCycle(); - vel = 0; - accAlt = 0; - } - - baro.BaroAlt = baroCalculateAltitude(); -#else - baro.BaroAlt = 0; -#endif - -#ifdef SONAR - sonarAlt = sonarRead(); - sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle()); - - if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) { - // just use the SONAR - baroAlt_offset = baro.BaroAlt - sonarAlt; - baro.BaroAlt = sonarAlt; - } else { - baro.BaroAlt -= baroAlt_offset; - if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) { - // SONAR in range, so use complementary filter - sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm); - baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition); + if (sensors(SENSOR_BARO)) { + if (!isBaroCalibrationComplete()) { + performBaroCalibrationCycle(); + vel = 0; + accAlt = 0; + } else { + baroAlt = baroCalculateAltitude(); + estimatedAltitude = baroAlt; } } #endif - dt = accTimeSum * 1e-6f; // delta acc reading time in seconds - - // Integrator - velocity, cm/sec - if (accSumCount) { - accZ_tmp = (float)accSum[2] / (float)accSumCount; - } else { - accZ_tmp = 0; +#ifdef SONAR + if (sensors(SENSOR_SONAR)) { + int32_t sonarAlt = sonarRead(); + sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle()); + if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) { + // SONAR in range, so use complementary filter + float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm); + sonarAlt = sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition); + estimatedAltitude = sonarAlt; + } } - vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; - - // Integrator - Altitude in cm - accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) - vel += vel_acc; - -#ifdef DEBUG_ALT_HOLD - debug[1] = accSum[2] / accSumCount; // acceleration - debug[2] = vel; // velocity - debug[3] = accAlt; // height #endif + float accZ_tmp = 0; +#ifdef ACC + if (sensors(SENSOR_ACC)) { + const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds + + // Integrator - velocity, cm/sec + if (accSumCount) { + accZ_tmp = (float)accSum[2] / (float)accSumCount; + } + const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum; + + // Integrator - Altitude in cm + accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) + accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) + vel += vel_acc; + estimatedAltitude = accAlt; + } +#endif + + DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount); + DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel); + DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt); + imuResetAccelerationSum(); + int32_t baroVel = 0; #ifdef BARO - if (!isBaroCalibrationComplete()) { - return; + if (sensors(SENSOR_BARO)) { + if (!isBaroCalibrationComplete()) { + return; + } + + static int32_t lastBaroAlt = 0; + baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime; + lastBaroAlt = baroAlt; + + baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s + baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero } #endif -#ifdef SONAR - if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) { - // the sonar has the best range - EstAlt = baro.BaroAlt; - } else { - EstAlt = accAlt; - } -#else - EstAlt = accAlt; -#endif - - baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime; - lastBaroAlt = baro.BaroAlt; - - baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s - baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero - // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel); - vel_tmp = lrintf(vel); + int32_t vel_tmp = lrintf(vel); // set vario - vario = applyDeadband(vel_tmp, 5); + estimatedVario = applyDeadband(vel_tmp, 5); + static float accZ_old = 0.0f; altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); - accZ_old = accZ_tmp; } +#endif -int32_t altitudeHoldGetEstimatedAltitude(void) +int32_t getEstimatedAltitude(void) { - return EstAlt; + return estimatedAltitude; } -#endif +int32_t getEstimatedVario(void) +{ + return estimatedVario; +} diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitude.h similarity index 94% rename from src/main/flight/altitudehold.h rename to src/main/flight/altitude.h index 14e48d205..be904dce9 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitude.h @@ -20,7 +20,6 @@ #include "common/time.h" extern int32_t AltHold; -extern int32_t vario; typedef struct airplaneConfig_s { bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign @@ -29,6 +28,8 @@ typedef struct airplaneConfig_s { PG_DECLARE(airplaneConfig_t, airplaneConfig); void calculateEstimatedAltitude(timeUs_t currentTimeUs); +int32_t getEstimatedAltitude(void); +int32_t getEstimatedVario(void); struct pidProfile_s; void configureAltitudeHold(struct pidProfile_s *initialPidProfile); @@ -36,5 +37,3 @@ void configureAltitudeHold(struct pidProfile_s *initialPidProfile); void applyAltHold(void); void updateAltHoldState(void); void updateSonarAltHoldState(void); - -int32_t altitudeHoldGetEstimatedAltitude(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 41aebab9e..e41b3cc76 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -67,7 +67,6 @@ typedef struct imuRuntimeConfig_s { void imuConfigure(uint16_t throttle_correction_angle); float getCosTiltAngle(void); -void calculateEstimatedAltitude(timeUs_t currentTimeUs); void imuUpdateAttitude(timeUs_t currentTimeUs); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 145676a67..038936161 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -256,7 +256,7 @@ static void osdDrawSingleElement(uint8_t item) case OSD_ALTITUDE: { - int32_t alt = osdGetAltitude(baro.BaroAlt); + int32_t alt = osdGetAltitude(getEstimatedAltitude()); sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); break; } @@ -619,7 +619,7 @@ void osdUpdateAlarms(void) // This is overdone? // uint16_t *itemPos = osdConfig()->item_pos; - int32_t alt = osdGetAltitude(baro.BaroAlt) / 100; + int32_t alt = osdGetAltitude(getEstimatedAltitude()) / 100; statRssi = rssi * 100 / 1024; if (statRssi < osdConfig()->rssi_alarm) @@ -699,8 +699,8 @@ static void osdUpdateStats(void) if (stats.min_rssi > statRssi) stats.min_rssi = statRssi; - if (stats.max_altitude < baro.BaroAlt) - stats.max_altitude = baro.BaroAlt; + if (stats.max_altitude < getEstimatedAltitude()) + stats.max_altitude = getEstimatedAltitude(); } static void osdGetBlackboxStatusString(char * buff, uint8_t len) diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index 4654cf793..2ddb1454b 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -57,6 +57,7 @@ #ifdef TELEMETRY #include +#include "flight/altitude.h" #include "sensors/sensors.h" #include "sensors/battery.h" #include "sensors/barometer.h" @@ -525,7 +526,7 @@ void handleJetiExBusTelemetry(void) if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) { jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage(); jetiExSensors[EX_CURRENT].value = getAmperage(); - jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt; + jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude(); jetiExSensors[EX_CAPACITY].value = getMAhDrawn(); jetiExSensors[EX_FRAMES_LOST].value = framesLost; jetiExSensors[EX_TIME_DIFF].value = timeDiff; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 32797d489..3039c5272 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -60,7 +60,7 @@ #include "telemetry/telemetry.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index daac014f6..c2c6fb25d 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -57,7 +57,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "rx/rx.h" @@ -175,9 +175,9 @@ static void sendAccel(void) static void sendBaro(void) { sendDataHead(ID_ALTITUDE_BP); - serialize16(baro.BaroAlt / 100); + serialize16(getEstimatedAltitude() / 100); sendDataHead(ID_ALTITUDE_AP); - serialize16(ABS(baro.BaroAlt % 100)); + serialize16(ABS(getEstimatedAltitude() % 100)); } #ifdef GPS @@ -356,7 +356,7 @@ static void sendGPSLatLong(void) static void sendVario(void) { sendDataHead(ID_VERT_SPEED); - serialize16(vario); + serialize16(getEstimatedVario()); } /* diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index eb6a4c1eb..009e53a6a 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -84,7 +84,7 @@ #include "flight/navigation.h" #include "io/gps.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "telemetry/telemetry.h" #include "telemetry/hott.h" @@ -210,7 +210,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) uint16_t altitude = GPS_altitude; if (!STATE(GPS_FIX)) { - altitude = baro.BaroAlt / 100; + altitude = getEstimatedAltitude() / 100; } const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m @@ -271,7 +271,7 @@ static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMess static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) { - const uint16_t hottEamAltitude = (baro.BaroAlt / 100) + HOTT_EAM_OFFSET_HEIGHT; + const uint16_t hottEamAltitude = (getEstimatedAltitude() / 100) + HOTT_EAM_OFFSET_HEIGHT; hottEAMMessage->altitude_L = hottEamAltitude & 0x00FF; hottEAMMessage->altitude_H = hottEamAltitude >> 8; @@ -279,6 +279,7 @@ static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage) { + int32_t vario = getEstimatedVario(); hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF; hottEAMMessage->climbrate_H = (30000 + vario) >> 8; hottEAMMessage->climbrate3s = 120 + (vario / 100); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index cb292e8cd..358084de4 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -69,7 +69,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/navigation.h" #include "telemetry/telemetry.h" @@ -144,7 +144,7 @@ static void ltm_gframe(void) ltm_serialise_8((uint8_t)(GPS_speed / 100)); #if defined(BARO) || defined(SONAR) - ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() : GPS_altitude * 100; + ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100; #else ltm_alt = GPS_altitude * 100; #endif diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 63a9fafb2..8210774ce 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -50,7 +50,7 @@ #include "flight/imu.h" #include "flight/failsafe.h" #include "flight/navigation.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "io/serial.h" #include "io/gimbal.h" @@ -331,7 +331,7 @@ void mavlinkSendPosition(void) GPS_altitude * 1000, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(BARO) || defined(SONAR) - (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000, + (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000, #else GPS_altitude * 1000, #endif @@ -400,7 +400,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) { // Baro or sonar generally is a better estimate of altitude than GPS MSL altitude - mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0; + mavAltitude = getEstimatedAltitude() / 100.0; } #if defined(GPS) else if (sensors(SENSOR_GPS)) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 1e8aa1661..9be06c9ff 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -32,7 +32,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitudehold.h" +#include "flight/altitude.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -638,7 +638,7 @@ void handleSmartPortTelemetry(void) //case FSSP_DATAID_RPM : case FSSP_DATAID_ALTITUDE : if (sensors(SENSOR_BARO)) { - smartPortSendPackage(id, baro.BaroAlt); // unknown given unit, requested 100 = 1 meter + smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter smartPortHasRequest = 0; } break; @@ -675,7 +675,7 @@ void handleSmartPortTelemetry(void) //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : if (sensors(SENSOR_BARO)) { - smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s + smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s smartPortHasRequest = 0; } break; diff --git a/src/test/Makefile b/src/test/Makefile index 9b8d053f0..3344a7cd3 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -25,7 +25,7 @@ alignsensor_unittest_SRC := \ altitude_hold_unittest_SRC := \ - $(USER_DIR)/flight/altitudehold.c + $(USER_DIR)/flight/altitude.c baro_bmp085_unittest_SRC := \ @@ -66,7 +66,7 @@ flight_failsafe_unittest_SRC := \ flight_imu_unittest_SRC := \ $(USER_DIR)/flight/imu.c \ - $(USER_DIR)/flight/altitudehold.c \ + $(USER_DIR)/flight/altitude.c \ $(USER_DIR)/common/maths.c diff --git a/src/test/unit/altitude_hold_unittest.cc.txt b/src/test/unit/altitude_hold_unittest.cc.txt index 02bf80b9f..d3bd1c856 100644 --- a/src/test/unit/altitude_hold_unittest.cc.txt +++ b/src/test/unit/altitude_hold_unittest.cc.txt @@ -45,7 +45,7 @@ extern "C" { #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" - #include "flight/altitudehold.h" + #include "flight/altitude.h" #include "config/runtime_config.h" diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 939fd7dc9..d4d427969 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -183,7 +183,8 @@ uint32_t fixedMillis = 0; baro_t baro; -int32_t vario = 0; +uint32_t getEstimatedAltitude() { return 0; } +uint32_t getEstimatedVario() { return 0; } uint32_t millis(void) { return fixedMillis;