From 1cd28a5eea2ee285e73c9ec058f34430da61b692 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Thu, 15 Sep 2022 15:06:40 +1000 Subject: [PATCH] refactoring and smoother transition to return, thanks Karatebrot --- src/main/blackbox/blackbox.c | 8 ++-- src/main/fc/init.c | 3 ++ src/main/flight/gps_rescue.c | 61 ++++++++++++++----------------- src/main/flight/position.c | 2 +- src/main/flight/position.h | 4 +- src/main/sensors/initialisation.c | 5 --- 6 files changed, 38 insertions(+), 45 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 06b1822410..f75f538f22 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1528,14 +1528,14 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_MINIMUM_SATS, "%d", gpsConfig()->gpsMinimumSats) #ifdef USE_GPS_RESCUE - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->rescueAltitudeBufferM) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->rescueAltitudeBufferM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, "%d", gpsRescueConfig()->angle) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, "%d", gpsRescueConfig()->angle) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index d06312d6e8..37902f642f 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -95,6 +95,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/gps_rescue.h" #include "flight/pid.h" #include "flight/pid_init.h" #include "flight/servos.h" @@ -766,6 +767,7 @@ void init(void) #ifdef USE_GPS if (featureIsEnabled(FEATURE_GPS)) { gpsInit(); + gpsRescueInit(); } #endif @@ -828,6 +830,7 @@ void init(void) #ifdef USE_BARO baroStartCalibration(); #endif + positionInit(); #if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL) vtxTableInit(); diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 34c96c3f6e..0f8c76225f 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -131,10 +131,10 @@ typedef enum { } 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 allowed -#define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max allowed iterm value for velocity -#define GPS_RESCUE_MAX_ITERM_THROTTLE 200 // max allowed iterm value for throttle -#define GPS_RESCUE_MAX_PITCH_RATE 3000 // max allowed change in pitch per second in degrees * 100 +#define GPS_RESCUE_MIN_DESCENT_DIST_M 5 // minimum descent distance +#define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max iterm value for velocity +#define GPS_RESCUE_MAX_ITERM_THROTTLE 200 // max iterm value for throttle +#define GPS_RESCUE_MAX_PITCH_RATE 3000 // max change in pitch per second in degrees * 100 #ifdef USE_MAG #define GPS_RESCUE_USE_MAG true @@ -344,7 +344,7 @@ static void rescueAttainPosition() // the faster the return speed, the more accurate the IMU will be, but the consequences of IMU error at the start are greater // A compass (magnetometer) is vital for accurate GPS rescue at slow speeds - // if the quad is pointing 180 degrees wrong at failsafe time, it will take 2s to rotate fully at 90 deg/s max rate + // if the quad is pointing 180 degrees wrong at failsafe time, it will take 1s to rotate fully at 180 deg/s max rate // this gives the level mode controller time to adjust pitch and roll during the yaw // we need a relatively gradual trajectory change for attitude.values.yaw to update effectively @@ -356,7 +356,7 @@ static void rescueAttainPosition() const float rollMixAttenuator = constrainf(1.0f - ABS(rescueYaw) * 0.01f, 0.0f, 1.0f); // attenuate roll as yaw rate increases, no roll at 100 deg/s of yaw const float rollAdjustment = - rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator; - // mix in the desired amount of roll; 1:1 yaw:roll when rollMix = 100 and yaw angles are small + // add the desired amount of roll; 1:1 yaw:roll when rollMix = 100 and yaw angles are small // when gpsRescueConfig()->rollMix is zero, there is no roll adjustment // rollAdjustment is degrees * 100 // note that the roll element has the same sign as the yaw element *before* GET_DIRECTION @@ -455,7 +455,7 @@ static void performSanityChecks() previousTimeUs = currentTimeUs; prevAltitudeCm = rescueState.sensor.currentAltitudeCm; prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm; - secondsLowSats = 5; // Start the count at 5 to be less forgiving at the beginning + secondsLowSats = 0; secondsDoingNothing = 0; return; } @@ -698,8 +698,8 @@ void altitudeAchieved(void) { rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm; rescueState.intent.altitudeStep = 0; - rescueState.intent.updateYaw = true; // allow yaw updating rescueState.phase = RESCUE_ROTATE; + rescueState.intent.updateYaw = true; } void updateGPSRescueState(void) @@ -755,21 +755,21 @@ void updateGPSRescueState(void) rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM; startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm); - rescueState.intent.updateYaw = false; // point the nose to home at all times during the rescue + rescueState.intent.updateYaw = false; // don't rotate until climb complete + rescueState.intent.yawAttenuator = 0.0f; rescueState.intent.targetVelocityCmS = 0; // zero forward velocity while climbing - rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch - rescueState.intent.rollAngleLimitDeg = 0; // flat on roll also + rescueState.intent.pitchAngleLimitDeg = 0; // no pitch + rescueState.intent.rollAngleLimitDeg = 0; // no roll until flying home rescueState.intent.altitudeStep = 0; rescueState.intent.descentSpeedModifier = 0.0f; - rescueState.intent.yawAttenuator = 0.0f; + } break; case RESCUE_ATTAIN_ALT: - // gradually increment the target altitude until the final target altitude value is set - // also require that the final target altitude has been achieved before moving on + // gradually increment the target altitude until the craft reaches target altitude + // note that this can mean the target altitude may increase above returnAltitude if the craft lags target // sanity check will abort if altitude gain is blocked for a cumulative period - // TO DO: if overshoots are a problem after craft achieves target altitude changes, adjust termination threshold with current vertical velocity if (startedLow) { if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) { rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate; @@ -787,33 +787,28 @@ void updateGPSRescueState(void) break; case RESCUE_ROTATE: - // gradually acquire yaw and roll authority over 50 iterations, or about half a second - if (rescueState.intent.yawAttenuator < 1.0f) { + if (rescueState.intent.yawAttenuator < 1.0f) { // gradually acquire yaw authority over 50 iterations, or about half a second rescueState.intent.yawAttenuator += 0.02f; } // - if (rescueState.sensor.absErrorAngle < 90.0f) { - const float angleToHome = 1.0f - (rescueState.sensor.absErrorAngle / 90.0f); - // once , gradually increase target velocity and roll angle - rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * angleToHome; - rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle * angleToHome; - if (rescueState.sensor.absErrorAngle < 10.0f) { - // enter fly home phase with full forward velocity target and full angle values - rescueState.phase = RESCUE_FLY_HOME; - rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home - rescueState.intent.yawAttenuator = 1.0f; - rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; - rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle; - } + if (rescueState.sensor.absErrorAngle < 10.0f) { + // enter fly home phase + rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed; + rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home + rescueState.phase = RESCUE_FLY_HOME; } break; case RESCUE_FLY_HOME: - // fly home with full control on all axes, pitching forward to gain speed - // gradually open up the roll limits up as we get underway - if (rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) { + if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to get full yaw authority + rescueState.intent.yawAttenuator += 0.02f; + } + if (rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) { // gradually acquire full roll authority rescueState.intent.rollAngleLimitDeg += 0.05f; } + if (rescueState.intent.pitchAngleLimitDeg < gpsRescueConfig()->angle) { // gradually acquire full pitch authority + rescueState.intent.pitchAngleLimitDeg += 0.05f; + } if (newGPSData) { if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) { rescueState.phase = RESCUE_DESCENT; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 2f5e1e65f9..453b5a7581 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -74,7 +74,7 @@ typedef enum { DEFAULT = 0, BARO_ONLY, GPS_ONLY -} altitude_source_e; +} altitudeSource_e; PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4); diff --git a/src/main/flight/position.h b/src/main/flight/position.h index a9733192fe..ac409a6c5d 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -26,8 +26,8 @@ typedef struct positionConfig_s { uint8_t altitude_source; uint8_t altitude_prefer_baro; - uint16_t altitude_lpf; // lowpass cutoff (value / 100) Hz for baro smoothing - uint16_t altitude_d_lpf; // lowpass for (value / 100) Hz baro derivative smoothing + uint16_t altitude_lpf; // lowpass cutoff (value / 100) Hz for altitude smoothing + uint16_t altitude_d_lpf; // lowpass for (value / 100) Hz for altitude derivative smoothing } positionConfig_t; PG_DECLARE(positionConfig_t, positionConfig); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 88840e8881..89352f74f5 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -32,8 +32,6 @@ #include "fc/runtime_config.h" #include "flight/pid.h" -#include "flight/position.h" -#include "flight/gps_rescue.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -87,9 +85,6 @@ bool sensorsAutodetect(void) baroInit(); #endif -positionInit(); -gpsRescueInit(); - #ifdef USE_RANGEFINDER rangefinderInit(); #endif