diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index c95924f600..6acd33502b 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -26,6 +26,7 @@ #include "build/debug.h" #include "common/axis.h" +#include "common/filter.h" #include "common/maths.h" #include "common/utils.h" @@ -44,21 +45,12 @@ #include "flight/pid.h" #include "flight/position.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" - #include "rx/rx.h" #include "sensors/acceleration.h" #include "gps_rescue.h" -typedef enum { - RESCUE_SANITY_OFF = 0, - RESCUE_SANITY_ON, - RESCUE_SANITY_FS_ONLY -} gpsRescueSanity_e; - typedef enum { RESCUE_IDLE, RESCUE_INITIALIZE, @@ -113,7 +105,6 @@ typedef struct { float velocityToHomeCmS; float alitutudeStepCm; float maxPitchStep; - float filterK; float absErrorAngle; } rescueSensorData_s; @@ -125,12 +116,6 @@ typedef struct { bool isAvailable; } rescueState_s; -typedef enum { - MAX_ALT, - FIXED_ALT, - CURRENT_ALT -} altitudeMode_e; - #define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate #define GPS_RESCUE_MIN_DESCENT_DIST_M 5 // minimum descent distance #define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max iterm value for velocity @@ -138,69 +123,33 @@ typedef enum { #define GPS_RESCUE_MAX_PITCH_RATE 3000 // max change in pitch per second in degrees * 100 #define GPS_RESCUE_DISARM_THRESHOLD 2.0f // disarm threshold in G's -#ifdef USE_MAG -#define GPS_RESCUE_USE_MAG true -#else -#define GPS_RESCUE_USE_MAG false -#endif - -PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3); - -PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, - .minRescueDth = 30, - .altitudeMode = MAX_ALT, - .rescueAltitudeBufferM = 10, - .ascendRate = 500, // cm/s, for altitude corrections on ascent - - .initialAltitudeM = 30, - .rescueGroundspeed = 500, - .angle = 40, - .rollMix = 150, - - .descentDistanceM = 20, - .descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent - .targetLandingAltitudeM = 4, - - .throttleMin = 1100, - .throttleMax = 1600, - .throttleHover = 1275, - - .allowArmingWithoutFix = false, - .sanityChecks = RESCUE_SANITY_FS_ONLY, - .minSats = 8, - - .throttleP = 15, - .throttleI = 15, - .throttleD = 15, - .velP = 8, - .velI = 30, - .velD = 20, - .yawP = 20, - - .useMag = GPS_RESCUE_USE_MAG -); - static float rescueThrottle; static float rescueYaw; float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 }; bool magForceDisable = false; static bool newGPSData = false; static pt2Filter_t throttleDLpf; +static pt2Filter_t velocityDLpf; static pt3Filter_t pitchLpf; rescueState_s rescueState; void gpsRescueInit(void) { - const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ); + const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ); + float cutoffHz, gain; - const float throttleDCutoffHz = positionConfig()->altitude_d_lpf / 100.0f; - const float throttleDCutoffGain = pt2FilterGain(throttleDCutoffHz, sampleTimeS); - pt2FilterInit(&throttleDLpf, throttleDCutoffGain); + cutoffHz = positionConfig()->altitude_d_lpf / 100.0f; + gain = pt2FilterGain(cutoffHz, sampleTimeS); + pt2FilterInit(&throttleDLpf, gain); - const float pitchCutoffHz = 4.0f; - const float pitchCutoffGain = pt3FilterGain(pitchCutoffHz, sampleTimeS); - pt3FilterInit(&pitchLpf, pitchCutoffGain); + cutoffHz = 0.8f; + gain = pt2FilterGain(cutoffHz, 1.0f); + pt2FilterInit(&velocityDLpf, gain); + + cutoffHz = 4.0f; + gain = pt3FilterGain(cutoffHz, sampleTimeS); + pt3FilterInit(&pitchLpf, gain); } /* @@ -242,13 +191,13 @@ static void setReturnAltitude(void) const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f; const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f; switch (gpsRescueConfig()->altitudeMode) { - case FIXED_ALT: + case GPS_RESCUE_ALT_MODE_FIXED: rescueState.intent.returnAltitudeCm = initialAltitudeCm; break; - case CURRENT_ALT: + case GPS_RESCUE_ALT_MODE_CURRENT: rescueState.intent.returnAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueAltitudeBufferCm; break; - case MAX_ALT: + case GPS_RESCUE_ALT_MODE_MAX: default: rescueState.intent.returnAltitudeCm = rescueState.intent.maxAltitudeCm + rescueAltitudeBufferCm; break; @@ -261,7 +210,6 @@ static void rescueAttainPosition(void) // runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase. static float previousVelocityError = 0.0f; static float velocityI = 0.0f; - static float previousVelocityD = 0.0f; // for smoothing static float previousPitchAdjustment = 0.0f; static float throttleI = 0.0f; static float previousAltitudeError = 0.0f; @@ -278,7 +226,6 @@ static void rescueAttainPosition(void) // Initialize internal variables each time GPS Rescue is started previousVelocityError = 0.0f; velocityI = 0.0f; - previousVelocityD = 0.0f; previousPitchAdjustment = 0.0f; throttleI = 0.0f; previousAltitudeError = 0.0f; @@ -391,9 +338,9 @@ static void rescueAttainPosition(void) // D component float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor); previousVelocityError = velocityError; - // simple first order filter on derivative with k = 0.5 for 200ms steps - velocityD = previousVelocityD + rescueState.sensor.filterK * (velocityD - previousVelocityD); - previousVelocityD = velocityD; + const float gain = pt2FilterGain(0.8f, HZ_TO_INTERVAL(gpsGetSampleRateHz())); + pt2FilterUpdateCutoff(&velocityDLpf, gain); + velocityD = pt2FilterApply(&velocityDLpf, velocityD); velocityD *= gpsRescueConfig()->velD; const float velocityIAttenuator = rescueState.intent.targetVelocityCmS / gpsRescueConfig()->rescueGroundspeed; @@ -602,9 +549,6 @@ static void sensorUpdate(void) // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values. previousGPSDataTimeUs = currentTimeUs; - rescueState.sensor.filterK = pt1FilterGain(0.8, rescueState.sensor.gpsDataIntervalSeconds); - // 0.8341 for 1hz, 0.5013 for 5hz, 0.3345 for 10hz, 0.1674 for 25Hz, etc - rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds; // positive = towards home. First value is useless since prevDistanceToHomeCm was zero. prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm; diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h index 53c639b8a6..7e77e7121c 100644 --- a/src/main/flight/gps_rescue.h +++ b/src/main/flight/gps_rescue.h @@ -17,37 +17,33 @@ #pragma once +#include + #include "common/axis.h" -#include "pg/pg.h" +#include "pg/gps_rescue.h" -#define TASK_GPS_RESCUE_RATE_HZ 100 // synced to altitude task rate +#define TASK_GPS_RESCUE_RATE_HZ 100 // in sync with altitude task rate -typedef struct gpsRescue_s { - uint16_t angle; // degrees - uint16_t initialAltitudeM; // meters - uint16_t descentDistanceM; // meters - uint16_t rescueGroundspeed; // centimeters per second - uint8_t throttleP, throttleI, throttleD; - uint8_t yawP; - uint16_t throttleMin; - uint16_t throttleMax; - uint16_t throttleHover; - uint8_t minSats; - uint8_t velP, velI, velD; - uint16_t minRescueDth; // meters - uint8_t sanityChecks; - uint8_t allowArmingWithoutFix; - uint8_t useMag; - uint8_t targetLandingAltitudeM; // meters - uint8_t altitudeMode; - uint16_t ascendRate; - uint16_t descendRate; - uint16_t rescueAltitudeBufferM; // meters - uint8_t rollMix; -} gpsRescueConfig_t; +#ifdef USE_MAG +#define GPS_RESCUE_USE_MAG true +#else +#define GPS_RESCUE_USE_MAG false +#endif -PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); +typedef enum { + RESCUE_SANITY_OFF = 0, + RESCUE_SANITY_ON, + RESCUE_SANITY_FS_ONLY, + RESCUE_SANITY_COUNT +} gpsRescueSanity_e; + +typedef enum { + GPS_RESCUE_ALT_MODE_MAX = 0, + GPS_RESCUE_ALT_MODE_FIXED, + GPS_RESCUE_ALT_MODE_CURRENT, + GPS_RESCUE_ALT_MODE_COUNT +} gpsRescueAltitudeMode_e; extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 9e07c4a34b..30c831d5cf 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -107,6 +107,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; #define UBLOX_ACK_TIMEOUT_MAX_COUNT (25) static serialPort_t *gpsPort; +static float gpsSampleRateHz; typedef struct gpsInitData_s { uint8_t index; @@ -307,6 +308,8 @@ static void gpsSetState(gpsState_e state) void gpsInit(void) { + gpsSampleRateHz = 0.0f; + gpsData.baudrateIndex = 0; gpsData.errors = 0; gpsData.timeouts = 0; @@ -1904,6 +1907,13 @@ void GPS_calculateDistanceAndDirectionToHome(void) void onGpsNewData(void) { + static timeUs_t timeUs, lastTimeUs = 0; + + // Detect current sample rate of GPS solution + timeUs = micros(); + gpsSampleRateHz = 1e6f / cmpTimeUs(timeUs, lastTimeUs); + lastTimeUs = timeUs; + if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) { // if we don't have a 3D fix and the minimum sats, don't give data to GPS rescue return; @@ -1928,4 +1938,10 @@ void gpsSetFixState(bool state) DISABLE_STATE(GPS_FIX); } } -#endif + +float gpsGetSampleRateHz(void) +{ + return gpsSampleRateHz; +} + +#endif // USE_GPS diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 11553ba538..ae609a931e 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -214,3 +214,4 @@ void GPS_reset_home_position(void); void GPS_calc_longitude_scaling(int32_t lat); void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); void gpsSetFixState(bool state); +float gpsGetSampleRateHz(void); diff --git a/src/main/pg/gps_rescue.c b/src/main/pg/gps_rescue.c new file mode 100644 index 0000000000..d0ade463fe --- /dev/null +++ b/src/main/pg/gps_rescue.c @@ -0,0 +1,69 @@ +/* + * 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 "platform.h" + +#ifdef USE_GPS_RESCUE + +#include "flight/gps_rescue.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "gps_rescue.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3); + +PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, + + .minRescueDth = 30, + .altitudeMode = GPS_RESCUE_ALT_MODE_MAX, + .rescueAltitudeBufferM = 10, + .ascendRate = 500, // cm/s, for altitude corrections on ascent + + .initialAltitudeM = 30, + .rescueGroundspeed = 500, + .angle = 40, + .rollMix = 150, + + .descentDistanceM = 20, + .descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent + .targetLandingAltitudeM = 4, + + .throttleMin = 1100, + .throttleMax = 1600, + .throttleHover = 1275, + + .allowArmingWithoutFix = false, + .sanityChecks = RESCUE_SANITY_FS_ONLY, + .minSats = 8, + + .throttleP = 15, + .throttleI = 15, + .throttleD = 15, + .velP = 8, + .velI = 30, + .velD = 20, + .yawP = 20, + + .useMag = GPS_RESCUE_USE_MAG +); + +#endif // USE_GPS_RESCUE diff --git a/src/main/pg/gps_rescue.h b/src/main/pg/gps_rescue.h new file mode 100644 index 0000000000..0a78dbbc89 --- /dev/null +++ b/src/main/pg/gps_rescue.h @@ -0,0 +1,53 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "pg/pg.h" + +typedef struct gpsRescue_s { + + uint16_t angle; // degrees + uint16_t initialAltitudeM; // meters + uint16_t descentDistanceM; // meters + uint16_t rescueGroundspeed; // centimeters per second + uint8_t throttleP, throttleI, throttleD; + uint8_t yawP; + uint16_t throttleMin; + uint16_t throttleMax; + uint16_t throttleHover; + uint8_t minSats; + uint8_t velP, velI, velD; + uint16_t minRescueDth; // meters + uint8_t sanityChecks; + uint8_t allowArmingWithoutFix; + uint8_t useMag; + uint8_t targetLandingAltitudeM; // meters + uint8_t altitudeMode; + uint16_t ascendRate; + uint16_t descendRate; + uint16_t rescueAltitudeBufferM; // meters + uint8_t rollMix; + +} gpsRescueConfig_t; + +PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); diff --git a/src/test/Makefile b/src/test/Makefile index 833330e462..6d287cb274 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -35,13 +35,13 @@ alignsensor_unittest_SRC := \ $(USER_DIR)/common/maths.c arming_prevention_unittest_SRC := \ + $(USER_DIR)/common/bitarray.c \ $(USER_DIR)/fc/core.c \ $(USER_DIR)/fc/dispatch.c \ $(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/runtime_config.c \ - $(USER_DIR)/flight/gps_rescue.c \ - $(USER_DIR)/common/bitarray.c + $(USER_DIR)/flight/gps_rescue.c arming_prevention_unittest_DEFINES := \ USE_GPS_RESCUE= diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 42ebb7e0f7..c36f38742e 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -18,34 +18,48 @@ #include extern "C" { + #include "blackbox/blackbox.h" + #include "build/debug.h" + + #include "common/filter.h" #include "common/maths.h" - #include "config/feature.h" - #include "pg/motor.h" - #include "pg/pg.h" - #include "pg/pg_ids.h" - #include "pg/rx.h" + #include "config/config.h" + #include "config/feature.h" + #include "fc/controlrate_profile.h" #include "fc/core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" + #include "flight/failsafe.h" + #include "flight/gps_rescue.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/position.h" #include "flight/servos.h" + #include "io/beeper.h" #include "io/gps.h" + + #include "pg/gps_rescue.h" + #include "pg/motor.h" + #include "pg/pg.h" + #include "pg/pg_ids.h" + #include "pg/rx.h" + #include "rx/rx.h" + #include "scheduler/scheduler.h" + #include "sensors/acceleration.h" #include "sensors/gyro.h" + #include "telemetry/telemetry.h" - #include "flight/gps_rescue.h" PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); @@ -60,6 +74,7 @@ extern "C" { PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); + PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0); float rcCommand[4]; @@ -81,6 +96,9 @@ extern "C" { acc_t acc = {}; bool mockIsUpright = false; uint8_t activePidLoopDenom = 1; + + float gpsGetSampleRateHz(void) { return 10.0f; } + void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; } } uint32_t simulationFeatureFlags = 0; diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index b8d509d006..21749ebec4 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -54,6 +54,7 @@ extern "C" { #include "osd/osd_elements.h" #include "osd/osd_warnings.h" + #include "pg/gps_rescue.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx.h" diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 1c24b3fa93..cfa0899de2 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -43,6 +43,7 @@ extern "C" { #include "io/gps.h" #include "io/vtx.h" + #include "pg/gps_rescue.h" #include "pg/motor.h" #include "pg/pg.h" #include "pg/pg_ids.h"