From c46be0304794437cca63fdb6db941e80dbe788b2 Mon Sep 17 00:00:00 2001 From: Diego Basch Date: Tue, 24 Apr 2018 11:11:35 -0700 Subject: [PATCH] added gps for altitude estimation, remove most unused code, rename altitude.c to position.c, add hdop to nmea --- make/source.mk | 2 +- src/main/fc/fc_core.c | 14 +- src/main/fc/fc_tasks.c | 54 +--- src/main/flight/altitude.c | 317 -------------------- src/main/flight/imu.c | 56 +--- src/main/flight/position.c | 117 ++++++++ src/main/flight/{altitude.h => position.h} | 14 +- src/main/interface/cli.c | 2 +- src/main/interface/msp.c | 61 +--- src/main/interface/settings.c | 23 +- src/main/io/gps.c | 7 +- src/main/io/gps.h | 2 +- src/main/io/osd.c | 2 +- src/main/scheduler/scheduler.h | 2 +- src/main/target/COLIBRI_RACE/i2c_bst.c | 2 +- src/main/target/common_fc_post.h | 5 - src/main/target/common_fc_pre.h | 1 - src/main/telemetry/frsky_hub.c | 2 +- src/main/telemetry/hott.c | 2 +- src/main/telemetry/ibus_shared.c | 2 +- src/main/telemetry/jetiexbus.c | 2 +- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/smartport.c | 2 +- src/test/Makefile | 7 +- src/test/unit/altitude_hold_unittest.cc.txt | 2 +- 26 files changed, 154 insertions(+), 550 deletions(-) delete mode 100644 src/main/flight/altitude.c create mode 100644 src/main/flight/position.c rename src/main/flight/{altitude.h => position.h} (69%) diff --git a/make/source.mk b/make/source.mk index 519fbe8865..f976b0380d 100644 --- a/make/source.mk +++ b/make/source.mk @@ -107,7 +107,7 @@ FC_SRC = \ fc/rc_adjustments.c \ fc/rc_controls.c \ fc/rc_modes.c \ - flight/altitude.c \ + flight/position.c \ flight/failsafe.c \ flight/imu.c \ flight/mixer.c \ diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 8460d95975..9324ca5d40 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -83,7 +83,7 @@ #include "telemetry/telemetry.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -866,17 +866,7 @@ static NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs) updateMagHold(); } #endif - -#if defined(USE_ALT_HOLD) - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState - updateRcCommands(); - if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) { - if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) { - applyAltHold(); - } - } -#endif - + // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index aad2d51c88..2f1e01a6fc 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -49,7 +49,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -81,8 +81,8 @@ #include "sensors/compass.h" #include "sensors/esc_sensor.h" #include "sensors/gyro.h" -#include "sensors/rangefinder.h" #include "sensors/sensors.h" +#include "sensors/rangefinder.h" #include "scheduler/scheduler.h" @@ -168,25 +168,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs) } #endif -#if !defined(USE_ALT_HOLD) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); -#endif updateArmingStatus(); - -#ifdef USE_ALT_HOLD -#ifdef USE_BARO - if (sensors(SENSOR_BARO)) { - updateAltHoldState(); - } -#endif - -#ifdef USE_RANGEFINDER - if (sensors(SENSOR_RANGEFINDER)) { - updateRangefinderAltHoldState(); - } -#endif -#endif // USE_ALT_HOLD } #endif @@ -204,20 +188,12 @@ static void taskUpdateBaro(timeUs_t currentTimeUs) } #endif -#if defined(USE_BARO) || defined(USE_RANGEFINDER) +#if defined(USE_BARO) || defined(USE_GPS) static void taskCalculateAltitude(timeUs_t currentTimeUs) { - if (false -#if defined(USE_BARO) - || (sensors(SENSOR_BARO) && isBaroReady()) -#endif -#if defined(USE_RANGEFINDER) - || sensors(SENSOR_RANGEFINDER) -#endif - ) { - calculateEstimatedAltitude(currentTimeUs); - }} -#endif // USE_BARO || USE_RANGEFINDER + calculateEstimatedAltitude(currentTimeUs); +} +#endif // USE_BARO || USE_GPS #ifdef USE_TELEMETRY static void taskTelemetry(timeUs_t currentTimeUs) @@ -296,11 +272,8 @@ void fcTasksInit(void) #ifdef USE_BARO setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); #endif -#ifdef USE_RANGEFINDER - setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER)); -#endif -#if defined(USE_BARO) || defined(USE_RANGEFINDER) - setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)); +#if defined(USE_BARO) || defined(USE_GPS) + setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_GPS)); #endif #ifdef USE_DASHBOARD setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); @@ -502,16 +475,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#ifdef USE_RANGEFINDER - [TASK_RANGEFINDER] = { - .taskName = "RANGEFINDER", - .taskFunc = rangefinderUpdate, - .desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value. - .staticPriority = TASK_PRIORITY_LOW, - }, -#endif - -#if defined(USE_BARO) || defined(USE_RANGEFINDER) +#if defined(USE_BARO) || defined(USE_GPS) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunc = taskCalculateAltitude, diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c deleted file mode 100644 index 260664b427..0000000000 --- a/src/main/flight/altitude.c +++ /dev/null @@ -1,317 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include -#include -#include -#include - -#include "platform.h" - -#include "build/debug.h" - -#include "common/axis.h" -#include "common/maths.h" -#include "common/utils.h" - -#include "pg/pg.h" -#include "pg/pg_ids.h" - -#include "fc/config.h" -#include "fc/rc_controls.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.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/rangefinder.h" - - -int32_t AltHold; -static int32_t estimatedVario = 0; // variometer in cm/s -static int32_t estimatedAltitude = 0; // in cm - - - -enum { - DEBUG_ALTITUDE_ACC, - DEBUG_ALTITUDE_VEL, - DEBUG_ALTITUDE_HEIGHT -}; -// 40hz update rate (20hz LPF on acc) -#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) - -#if defined(USE_ALT_HOLD) - -PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); - -PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, - .fixedwing_althold_reversed = false -); - -static int32_t setVelocity = 0; -static uint8_t velocityControl = 0; -static int32_t errorVelocityI = 0; -static int32_t altHoldThrottleAdjustment = 0; -static int16_t initialThrottleHold; - - -#define DEGREES_80_IN_DECIDEGREES 800 - -static void applyMultirotorAltHold(void) -{ - static uint8_t isAltHoldChanged = 0; - // multirotor alt hold - if (rcControlsConfig()->alt_hold_fast_change) { - // rapid alt changes - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { - errorVelocityI = 0; - isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband; - } else { - if (isAltHoldChanged) { - AltHold = estimatedAltitude; - isAltHoldChanged = 0; - } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX); - } - } else { - // slow alt changes, mostly used for aerial photography - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { - // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s - setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2; - velocityControl = 1; - isAltHoldChanged = 1; - } else if (isAltHoldChanged) { - AltHold = estimatedAltitude; - velocityControl = 0; - isAltHoldChanged = 0; - } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX); - } -} - -static void applyFixedWingAltHold(void) -{ - // handle fixedwing-related althold. UNTESTED! and probably wrong - // most likely need to check changes on pitch channel and 'reset' althold similar to - // how throttle does it on multirotor - - rcCommand[PITCH] += altHoldThrottleAdjustment * GET_DIRECTION(airplaneConfig()->fixedwing_althold_reversed); -} - -void applyAltHold(void) -{ - if (STATE(FIXED_WING)) { - applyFixedWingAltHold(); - } else { - applyMultirotorAltHold(); - } -} - -void updateAltHoldState(void) -{ - // Baro alt hold activate - if (!IS_RC_MODE_ACTIVE(BOXBARO)) { - DISABLE_FLIGHT_MODE(BARO_MODE); - return; - } - - if (!FLIGHT_MODE(BARO_MODE)) { - ENABLE_FLIGHT_MODE(BARO_MODE); - AltHold = estimatedAltitude; - initialThrottleHold = rcData[THROTTLE]; - errorVelocityI = 0; - altHoldThrottleAdjustment = 0; - } -} - -void updateRangefinderAltHoldState(void) -{ - // Sonar alt hold activate - if (!IS_RC_MODE_ACTIVE(BOXRANGEFINDER)) { - DISABLE_FLIGHT_MODE(RANGEFINDER_MODE); - return; - } - - if (!FLIGHT_MODE(RANGEFINDER_MODE)) { - ENABLE_FLIGHT_MODE(RANGEFINDER_MODE); - AltHold = estimatedAltitude; - initialThrottleHold = rcData[THROTTLE]; - errorVelocityI = 0; - altHoldThrottleAdjustment = 0; - } -} - -bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude) -{ - return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES; -} - -int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old) -{ - int32_t result = 0; - int32_t error; - int32_t setVel; - - if (!isThrustFacingDownwards(&attitude)) { - return result; - } - - // Altitude P-Controller - - if (!velocityControl) { - error = constrain(AltHold - estimatedAltitude, -500, 500); - error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position - setVel = constrain((currentPidProfile->pid[PID_ALT].P * error / 128), -300, +300); // limit velocity to +/- 3 m/s - } else { - setVel = setVelocity; - } - // Velocity PID-Controller - - // P - error = setVel - vel_tmp; - result = constrain((currentPidProfile->pid[PID_VEL].P * error / 32), -300, +300); - - // I - errorVelocityI += (currentPidProfile->pid[PID_VEL].I * error); - errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200)); - result += errorVelocityI / 8192; // I in range +/-200 - - // D - result -= constrain(currentPidProfile->pid[PID_VEL].D * (accZ_tmp + accZ_old) / 512, -150, 150); - - return result; -} -#endif // USE_ALT_HOLD - -#if defined(USE_BARO) || defined(USE_RANGEFINDER) -void calculateEstimatedAltitude(timeUs_t currentTimeUs) -{ - 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 USE_BARO - if (sensors(SENSOR_BARO)) { - if (!isBaroCalibrationComplete()) { - performBaroCalibrationCycle(); - vel = 0; - accAlt = 0; - } else { - baroAlt = baroCalculateAltitude(); - estimatedAltitude = baroAlt; - } - } -#endif - -#ifdef USE_RANGEFINDER - if (sensors(SENSOR_RANGEFINDER) && rangefinderProcess(getCosTiltAngle())) { - int32_t rangefinderAlt = rangefinderGetLatestAltitude(); - if (rangefinderAlt > 0 && rangefinderAlt >= rangefinderCfAltCm && rangefinderAlt <= rangefinderMaxAltWithTiltCm) { - // RANGEFINDER in range, so use complementary filter - float rangefinderTransition = (float)(rangefinderMaxAltWithTiltCm - rangefinderAlt) / (rangefinderMaxAltWithTiltCm - rangefinderCfAltCm); - rangefinderAlt = (float)rangefinderAlt * rangefinderTransition + baroAlt * (1.0f - rangefinderTransition); - estimatedAltitude = rangefinderAlt; - } - } -#endif - - float accZ_tmp = 0; -#ifdef USE_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] / 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 * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(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 USE_BARO - 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 // USE_BARO - - // 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 * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel)); - int32_t vel_tmp = lrintf(vel); - - // set vario - estimatedVario = applyDeadband(vel_tmp, 5); - -#ifdef USE_ALT_HOLD - static float accZ_old = 0.0f; - altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); - accZ_old = accZ_tmp; -#else - UNUSED(accZ_tmp); -#endif -} -#endif // USE_BARO || USE_RANGEFINDER - -int32_t getEstimatedAltitude(void) -{ - return estimatedAltitude; -} - -int32_t getEstimatedVario(void) -{ - return estimatedVario; -} diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 66e62ebd10..8a579ec45d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -193,58 +193,6 @@ void imuResetAccelerationSum(void) accTimeSum = 0; } -#if defined(USE_ALT_HOLD) -static void imuTransformVectorBodyToEarth(t_fp_vector * v) -{ - // From body frame to earth frame - const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; - const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; - const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; - - v->V.X = x; - v->V.Y = -y; - v->V.Z = z; -} - -// rotate acc into Earth frame and calculate acceleration in it -static void imuCalculateAcceleration(timeDelta_t deltaT) -{ - static int32_t accZoffset = 0; - static float accz_smooth = 0; - - // deltaT is measured in us ticks - const float dT = (float)deltaT * 1e-6f; - - t_fp_vector accel_ned; - accel_ned.V.X = acc.accADC[X]; - accel_ned.V.Y = acc.accADC[Y]; - accel_ned.V.Z = acc.accADC[Z]; - - imuTransformVectorBodyToEarth(&accel_ned); - - if (imuRuntimeConfig.acc_unarmedcal == 1) { - if (!ARMING_FLAG(ARMED)) { - accZoffset -= accZoffset / 64; - accZoffset += accel_ned.V.Z; - } - accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis - } else { - accel_ned.V.Z -= acc.dev.acc_1G; - } - - accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter - - // apply Deadband to reduce integration drift and vibration influence - accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy); - accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy); - accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z); - - // sum up Values for later integration to get velocity and distance - accTimeSum += deltaT; - accSumCount++; -} -#endif // USE_ALT_HOLD - static float invSqrt(float x) { return 1.0f / sqrtf(x); @@ -531,6 +479,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) UNUSED(useCOG); UNUSED(canUseGPSHeading); UNUSED(courseOverGround); + UNUSED(deltaT); #else #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) @@ -552,9 +501,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) imuUpdateEulerAngles(); #endif -#if defined(USE_ALT_HOLD) - imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame -#endif } void imuUpdateAttitude(timeUs_t currentTimeUs) diff --git a/src/main/flight/position.c b/src/main/flight/position.c new file mode 100644 index 0000000000..fb1963adf5 --- /dev/null +++ b/src/main/flight/position.c @@ -0,0 +1,117 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software: you can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "build/debug.h" + +#include "common/maths.h" + +#include "fc/runtime_config.h" + +#include "flight/position.h" +#include "flight/imu.h" +#include "flight/pid.h" + +#include "io/gps.h" + +#include "sensors/sensors.h" +#include "sensors/barometer.h" + +static int32_t estimatedAltitude = 0; // in cm + +#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) + + +#if defined(USE_BARO) || defined(USE_GPS) +void calculateEstimatedAltitude(timeUs_t currentTimeUs) +{ + static timeUs_t previousTimeUs = 0; + const uint32_t dTime = currentTimeUs - previousTimeUs; + if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { + return; + } + previousTimeUs = currentTimeUs; + + int32_t baroAlt = 0; + static int32_t baroAltOffset = 0; + static int32_t gpsAltOffset = 0; + + int32_t gpsAlt = 0; + float gpsTrust = 0.3; //conservative default + bool haveBaroAlt = false; + bool haveGPSAlt = false; +#ifdef USE_BARO + if (sensors(SENSOR_BARO)) { + if (!isBaroCalibrationComplete()) { + performBaroCalibrationCycle(); + } else { + baroAlt = baroCalculateAltitude(); + haveBaroAlt = true; + } + } +#endif + +#ifdef USE_GPS +if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { + gpsAlt = gpsSol.llh.alt; + haveGPSAlt = true; + + if (gpsSol.hdop != 0) { + gpsTrust = 100.0 / gpsSol.hdop; + } + // always use at least 10% of other sources besides gps if available + gpsTrust = MIN(gpsTrust, 0.9f); +} +#endif + + if (!ARMING_FLAG(ARMED)) { + baroAltOffset = baroAlt; + gpsAltOffset = gpsAlt; + } + baroAlt -= baroAltOffset; + gpsAlt -= gpsAltOffset; + + if (haveGPSAlt && haveBaroAlt) { + estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1-gpsTrust); + } else if (haveGPSAlt && !haveBaroAlt) { + estimatedAltitude = gpsAlt; + } + + DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust)); + DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt); + DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt); +} +#endif + + +int32_t getEstimatedAltitude(void) +{ + return estimatedAltitude; +} + +// This should be removed or fixed, but it would require changing a lot of other things to get rid of. +int16_t getEstimatedVario(void) +{ + return 0; +} diff --git a/src/main/flight/altitude.h b/src/main/flight/position.h similarity index 69% rename from src/main/flight/altitude.h rename to src/main/flight/position.h index 0534f4c446..af7a3b3eca 100644 --- a/src/main/flight/altitude.h +++ b/src/main/flight/position.h @@ -22,18 +22,6 @@ #include "common/time.h" -extern int32_t AltHold; - -typedef struct airplaneConfig_s { - bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign -} airplaneConfig_t; - -PG_DECLARE(airplaneConfig_t, airplaneConfig); - void calculateEstimatedAltitude(timeUs_t currentTimeUs); int32_t getEstimatedAltitude(void); -int32_t getEstimatedVario(void); - -void applyAltHold(void); -void updateAltHoldState(void); -void updateRangefinderAltHoldState(void); +int16_t getEstimatedVario(void); \ No newline at end of file diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index b2728bac89..4cea40eca4 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -91,7 +91,7 @@ extern uint8_t __config_end; #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index bcee60565c..4ebee9bbc9 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -69,7 +69,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -1324,29 +1324,6 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb } #endif // USE_OSD_SLAVE -#ifdef USE_NAV -static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src) -{ - uint8_t wp_no; - int32_t lat = 0, lon = 0; - wp_no = sbufReadU8(src); // get the wp number - if (wp_no == 0) { - lat = GPS_home[LAT]; - lon = GPS_home[LON]; - } else if (wp_no == 16) { - lat = GPS_hold[LAT]; - lon = GPS_hold[LON]; - } - sbufWriteU8(dst, wp_no); - sbufWriteU32(dst, lat); - sbufWriteU32(dst, lon); - sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps - sbufWriteU16(dst, 0); // heading will come here (deg) - sbufWriteU16(dst, 0); // time to stay (ms) will come here - sbufWriteU8(dst, 0); // nav flag will come here -} -#endif - #ifdef USE_FLASHFS static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) { @@ -1399,10 +1376,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) uint32_t i; uint8_t value; const unsigned int dataSize = sbufBytesRemaining(src); -#ifdef USE_NAV - uint8_t wp_no; - int32_t lat = 0, lon = 0, alt = 0; -#endif switch (cmdMSP) { case MSP_SELECT_SETTING: value = sbufReadU8(src); @@ -1869,33 +1842,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; #endif // USE_GPS -#ifdef USE_NAV - case MSP_SET_WP: - wp_no = sbufReadU8(src); //get the wp number - lat = sbufReadU32(src); - lon = sbufReadU32(src); - alt = sbufReadU32(src); // to set altitude (cm) - sbufReadU16(src); // future: to set heading (deg) - sbufReadU16(src); // future: to set time to stay (ms) - sbufReadU8(src); // future: to set nav flag - if (wp_no == 0) { - GPS_home[LAT] = lat; - GPS_home[LON] = lon; - DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS - ENABLE_STATE(GPS_FIX_HOME); - if (alt != 0) - AltHold = alt; // temporary implementation to test feature with apps - } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS - GPS_hold[LAT] = lat; - GPS_hold[LON] = lon; - if (alt != 0) - AltHold = alt; // temporary implementation to test feature with apps - nav_mode = NAV_MODE_WP; - GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); - } - break; -#endif // USE_NAV - case MSP_SET_FEATURE_CONFIG: featureClearAll(); featureSet(sbufReadU32(src)); // features bitmap @@ -2307,11 +2253,6 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro mspFc4waySerialCommand(dst, src, mspPostProcessFn); ret = MSP_RESULT_ACK; #endif -#ifdef USE_NAV - } else if (cmdMSP == MSP_WP) { - mspFcWpCommand(dst, src); - ret = MSP_RESULT_ACK; -#endif #ifdef USE_FLASHFS } else if (cmdMSP == MSP_DATAFLASH_READ) { mspFcDataFlashReadCommand(dst, src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 8f7d920818..154ee7cb86 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -45,7 +45,7 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -691,17 +691,7 @@ const clivalue_t valueTable[] = { { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_max) }, { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) }, #endif - -// PG_AIRPLANE_CONFIG -#if defined(USE_ALT_HOLD) - { "fixedwing_althold_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_reversed) }, -#endif - -// PG_RC_CONTROLS_CONFIG -#if defined(USE_ALT_HOLD) - { "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) }, - { "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_fast_change) }, -#endif + { "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, @@ -758,15 +748,6 @@ const clivalue_t valueTable[] = { { "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, -#ifdef USE_ALT_HOLD - { "p_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) }, - { "i_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) }, - { "d_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) }, - { "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) }, - { "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) }, - { "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) }, -#endif - { "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, { "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, diff --git a/src/main/io/gps.c b/src/main/io/gps.c index c06e399038..667c46a71d 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -636,6 +636,7 @@ typedef struct gpsDataNmea_s { uint8_t numSat; uint16_t altitude; uint16_t speed; + uint16_t hdop; uint16_t ground_course; uint32_t time; uint32_t date; @@ -700,6 +701,9 @@ static bool gpsNewFrameNMEA(char c) case 7: gps_Msg.numSat = grab_fields(string, 0); break; + case 8: + gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop + break; case 9: gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis break; @@ -793,6 +797,7 @@ static bool gpsNewFrameNMEA(char c) gpsSol.llh.lon = gps_Msg.longitude; gpsSol.numSat = gps_Msg.numSat; gpsSol.llh.alt = gps_Msg.altitude; + gpsSol.hdop = gps_Msg.hdop; } break; case FRAME_RMC: @@ -1015,7 +1020,7 @@ static bool UBLOX_parse_gps(void) //i2c_dataset.time = _buffer.posllh.time; gpsSol.llh.lon = _buffer.posllh.longitude; gpsSol.llh.lat = _buffer.posllh.latitude; - gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m + gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm if (next_fix) { ENABLE_STATE(GPS_FIX); } else { diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 4b8cf045fb..f7c8a226b5 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -85,7 +85,7 @@ typedef struct gpsCoordinateDDDMMmmmm_s { typedef struct gpsLocation_s { int32_t lat; // latitude * 1e+7 int32_t lon; // longitude * 1e+7 - uint16_t alt; // altitude in 0.1m + int32_t alt; // altitude in 0.1m } gpsLocation_t; typedef struct gpsSolutionData_s { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index bed34cc9f3..c007a80d73 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -65,7 +65,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/imu.h" #ifdef USE_ESC_SENSOR #include "flight/mixer.h" diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 274192b3a0..e55cfae51a 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -82,7 +82,7 @@ typedef enum { #ifdef USE_RANGEFINDER TASK_RANGEFINDER, #endif -#if defined(USE_BARO) || defined(USE_RANGEFINDER) +#if defined(USE_BARO) || defined(USE_GPS) TASK_ALTITUDE, #endif #ifdef USE_DASHBOARD diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index ca6540b869..8d9d4a331e 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -57,7 +57,7 @@ #include "telemetry/telemetry.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h index 4b35a610d1..3c550aa1f8 100644 --- a/src/main/target/common_fc_post.h +++ b/src/main/target/common_fc_post.h @@ -74,11 +74,6 @@ #undef USE_SPEKTRUM_CMS_TELEMETRY #endif -// undefine USE_ALT_HOLD if there is no baro or rangefinder to support it -#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_RANGEFINDER) -#undef USE_ALT_HOLD -#endif - /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ #if !defined(USE_VTX_COMMON) || !defined(USE_VTX_CONTROL) #undef USE_VTX_COMMON diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 432c1752af..6e8c8694bd 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -198,7 +198,6 @@ #endif #if (FLASH_SIZE > 256) -#define USE_ALT_HOLD #define USE_DASHBOARD #define USE_GPS #define USE_GPS_NMEA diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 04cd3c2190..79c8f8f3a5 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -60,7 +60,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "rx/rx.h" diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 4e4516a190..f756d3462b 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -75,7 +75,7 @@ #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/pid.h" #include "io/gps.h" diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 8ccb3461ea..420bc2384c 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -54,7 +54,7 @@ static uint16_t calculateChecksum(const uint8_t *ibusPacket); #include "sensors/sensors.h" #include "sensors/barometer.h" #include "flight/imu.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/navigation.h" #include "io/gps.h" diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 0924154fed..731fe6fe19 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/src/main/telemetry/jetiexbus.c @@ -38,7 +38,7 @@ #include "drivers/serial_uart.h" #include "drivers/time.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/imu.h" #include "io/serial.h" diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c9f193689a..ee4920ebc0 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -72,7 +72,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "telemetry/telemetry.h" #include "telemetry/ltm.h" diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 84f8cf2bc5..b07ff2a4a0 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -52,7 +52,7 @@ #include "flight/imu.h" #include "flight/failsafe.h" #include "flight/navigation.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "io/serial.h" #include "io/gimbal.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index f8b286a9d6..690cd4bea2 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -51,7 +51,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/altitude.h" +#include "flight/position.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" diff --git a/src/test/Makefile b/src/test/Makefile index c132f170d5..a1c34c0188 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -26,11 +26,6 @@ alignsensor_unittest_SRC := \ $(USER_DIR)/sensors/boardalignment.c \ $(USER_DIR)/common/maths.c - -altitude_hold_unittest_SRC := \ - $(USER_DIR)/flight/altitude.c - - arming_prevention_unittest_SRC := \ $(USER_DIR)/fc/fc_core.c \ $(USER_DIR)/fc/rc_controls.c \ @@ -120,7 +115,7 @@ flight_imu_unittest_SRC := \ $(USER_DIR)/common/maths.c \ $(USER_DIR)/config/feature.c \ $(USER_DIR)/fc/rc_modes.c \ - $(USER_DIR)/flight/altitude.c \ + $(USER_DIR)/flight/position.c \ $(USER_DIR)/flight/imu.c diff --git a/src/test/unit/altitude_hold_unittest.cc.txt b/src/test/unit/altitude_hold_unittest.cc.txt index 089d5c7a78..6bfd6c7388 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/altitude.h" + #include "flight/position.h" #include "config/runtime_config.h"