mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Fix GPS Rescue parameters confusion (#13047)
* Fix GPS Rescue parameters * Fixes per review * more renames * Refactor setReturnAltitude * Change return altitude to 5 - 1000 * Rename return speed * Change groundSpeed to groundSpeedCmS
This commit is contained in:
Mark Haslinghuis
GitHub
parent
b7f98b08e6
commit
7780880139
9 changed files with 69 additions and 65 deletions
|
|
@ -1610,13 +1610,13 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
|
||||
|
||||
#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()->minStartDistM)
|
||||
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()->initialClimbM)
|
||||
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_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz)
|
||||
|
|
|
|||
|
|
@ -1032,13 +1032,13 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
// PG_GPS_RESCUE
|
||||
{ PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
|
||||
{ PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 30 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minStartDistM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ALT_MODE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) },
|
||||
{ PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialClimbM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ASCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50, 2500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, ascendRate) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_RETURN_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_RETURN_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
|
||||
{ PARAM_NAME_GPS_RESCUE_RETURN_ALT, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, returnAltitudeM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_GROUND_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, groundSpeedCmS) },
|
||||
{ PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, maxRescueAngle) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ROLL_MIX, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rollMix) },
|
||||
{ PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, pitchCutoffHz) },
|
||||
|
|
|
|||
|
|
@ -39,13 +39,13 @@
|
|||
|
||||
#include "flight/gps_rescue.h"
|
||||
|
||||
static uint16_t gpsRescueConfig_minRescueDth; //meters
|
||||
static uint16_t gpsRescueConfig_minStartDistM; //meters
|
||||
static uint8_t gpsRescueConfig_altitudeMode;
|
||||
static uint8_t gpsRescueConfig_rescueAltitudeBufferM; // meters
|
||||
static uint16_t gpsRescueConfig_initialClimbM; // meters
|
||||
static uint16_t gpsRescueConfig_ascendRate;
|
||||
|
||||
static uint8_t gpsRescueConfig_initialAltitudeM; //meters
|
||||
static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second
|
||||
static uint16_t gpsRescueConfig_returnAltitudeM; //meters
|
||||
static uint16_t gpsRescueConfig_groundSpeedCmS; // centimeters per second
|
||||
static uint8_t gpsRescueConfig_angle; //degrees
|
||||
|
||||
static uint16_t gpsRescueConfig_descentDistanceM; //meters
|
||||
|
|
@ -143,13 +143,13 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
|
|||
{
|
||||
UNUSED(pDisp);
|
||||
|
||||
gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth;
|
||||
gpsRescueConfig_minStartDistM = gpsRescueConfig()->minStartDistM;
|
||||
gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode;
|
||||
gpsRescueConfig_rescueAltitudeBufferM = gpsRescueConfig()->rescueAltitudeBufferM;
|
||||
gpsRescueConfig_initialClimbM = gpsRescueConfig()->initialClimbM;
|
||||
gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate;
|
||||
|
||||
gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
|
||||
gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
|
||||
gpsRescueConfig_returnAltitudeM = gpsRescueConfig()->returnAltitudeM;
|
||||
gpsRescueConfig_groundSpeedCmS = gpsRescueConfig()->groundSpeedCmS;
|
||||
gpsRescueConfig_angle = gpsRescueConfig()->maxRescueAngle;
|
||||
|
||||
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
|
||||
|
|
@ -171,13 +171,13 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth;
|
||||
gpsRescueConfigMutable()->minStartDistM = gpsRescueConfig_minStartDistM;
|
||||
gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode;
|
||||
gpsRescueConfigMutable()->rescueAltitudeBufferM = gpsRescueConfig_rescueAltitudeBufferM;
|
||||
gpsRescueConfigMutable()->initialClimbM = gpsRescueConfig_initialClimbM;
|
||||
gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate;
|
||||
|
||||
gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
|
||||
gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
|
||||
gpsRescueConfigMutable()->returnAltitudeM = gpsRescueConfig_returnAltitudeM;
|
||||
gpsRescueConfigMutable()->groundSpeedCmS = gpsRescueConfig_groundSpeedCmS;
|
||||
gpsRescueConfigMutable()->maxRescueAngle = gpsRescueConfig_angle;
|
||||
|
||||
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
|
||||
|
|
@ -198,13 +198,13 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
|
|||
{
|
||||
{"--- GPS RESCUE ---", OME_Label, NULL, NULL},
|
||||
|
||||
{ "MIN START DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 20, 1000, 1 } },
|
||||
{ "MIN START DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minStartDistM, 20, 1000, 1 } },
|
||||
{ "ALTITUDE MODE" , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} },
|
||||
{ "INITAL CLIMB M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_rescueAltitudeBufferM, 0, 100, 1 } },
|
||||
{ "INITAL CLIMB M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialClimbM, 0, 100, 1 } },
|
||||
{ "ASCEND RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 50, 2500, 1 } },
|
||||
|
||||
{ "RETURN ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_initialAltitudeM, 2, 255, 1 } },
|
||||
{ "RETURN SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 0, 3000, 1 } },
|
||||
{ "RETURN ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_returnAltitudeM, 2, 255, 1 } },
|
||||
{ "RETURN SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_groundSpeedCmS, 0, 3000, 1 } },
|
||||
{ "PITCH ANGLE MAX", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_angle, 0, 60, 1 } },
|
||||
|
||||
{ "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 5, 500, 1 } },
|
||||
|
|
|
|||
|
|
@ -158,7 +158,7 @@
|
|||
#define PARAM_NAME_GPS_RESCUE_ASCEND_RATE "gps_rescue_ascend_rate"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_RETURN_ALT "gps_rescue_return_alt"
|
||||
#define PARAM_NAME_GPS_RESCUE_RETURN_SPEED "gps_rescue_ground_speed"
|
||||
#define PARAM_NAME_GPS_RESCUE_GROUND_SPEED "gps_rescue_ground_speed"
|
||||
#define PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE "gps_rescue_max_angle"
|
||||
#define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
|
||||
#define PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF "gps_rescue_pitch_cutoff"
|
||||
|
|
|
|||
|
|
@ -190,22 +190,20 @@ static void setReturnAltitude(void)
|
|||
// set the target altitude to current values, so there will be no D kick on first run
|
||||
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
|
||||
// Intended descent distance for rescues that start outside the minRescueDth distance
|
||||
// Intended descent distance for rescues that start outside the minStartDistM distance
|
||||
// Set this to the user's intended descent distance, but not more than half the distance to home to ensure some fly home time
|
||||
rescueState.intent.descentDistanceM = fminf(0.5f * rescueState.sensor.distanceToHomeM, gpsRescueConfig()->descentDistanceM);
|
||||
|
||||
const float initialAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100.0f;
|
||||
const float rescueAltitudeBufferCm = gpsRescueConfig()->rescueAltitudeBufferM * 100.0f;
|
||||
switch (gpsRescueConfig()->altitudeMode) {
|
||||
case GPS_RESCUE_ALT_MODE_FIXED:
|
||||
rescueState.intent.returnAltitudeCm = initialAltitudeCm;
|
||||
rescueState.intent.returnAltitudeCm = gpsRescueConfig()->returnAltitudeM * 100.0f;
|
||||
break;
|
||||
case GPS_RESCUE_ALT_MODE_CURRENT:
|
||||
rescueState.intent.returnAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueAltitudeBufferCm;
|
||||
rescueState.intent.returnAltitudeCm = rescueState.sensor.currentAltitudeCm + gpsRescueConfig()->initialClimbM * 100.0f;
|
||||
break;
|
||||
case GPS_RESCUE_ALT_MODE_MAX:
|
||||
default:
|
||||
rescueState.intent.returnAltitudeCm = rescueState.intent.maxAltitudeCm + rescueAltitudeBufferCm;
|
||||
rescueState.intent.returnAltitudeCm = rescueState.intent.maxAltitudeCm + gpsRescueConfig()->initialClimbM * 100.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -372,7 +370,7 @@ static void rescueAttainPosition(void)
|
|||
|
||||
// if initiated too close, and in the climb phase, pitch forward in whatever direction the nose is oriented until min distance away
|
||||
// intent is to get far enough away that, with an IMU error, the quad will have enough distance to home to correct that error in the fly home phase
|
||||
if ((rescueState.phase == RESCUE_ATTAIN_ALT) && (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth)) {
|
||||
if (rescueState.phase == RESCUE_ATTAIN_ALT && rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minStartDistM) {
|
||||
pitchAdjustment = 1500.0f; // 15 degree pitch forward
|
||||
}
|
||||
|
||||
|
|
@ -585,7 +583,7 @@ static void sensorUpdate(void)
|
|||
DEBUG_SET(DEBUG_ATTITUDE, 4, rescueState.sensor.velocityToHomeCmS); // velocity to home
|
||||
|
||||
// when there is a flyaway due to IMU disorientation, increase IMU yaw CoG gain, and reduce max pitch angle
|
||||
if (gpsRescueConfig()->rescueGroundspeed) {
|
||||
if (gpsRescueConfig()->groundSpeedCmS) {
|
||||
// calculate a factor that can reduce pitch angle when flying away
|
||||
const float rescueGroundspeed = gpsRescueConfig()->imuYawGain * 100.0f; // in cm/s, imuYawGain is m/s groundspeed
|
||||
// rescueGroundspeed is effectively a normalising gain factor for the magnitude of the groundspeed error
|
||||
|
|
@ -705,7 +703,7 @@ void descend(void)
|
|||
|
||||
// reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
|
||||
// if it does overshoot, allow pitch angle limit to build up to correct the overshoot once rotated
|
||||
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
|
||||
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->groundSpeedCmS * proximityToLandingArea;
|
||||
|
||||
// attenuate velocity iterm towards zero as we get closer to the landing area
|
||||
rescueState.intent.velocityItermAttenuator = fminf(proximityToLandingArea, rescueState.intent.velocityItermAttenuator);
|
||||
|
|
@ -770,7 +768,7 @@ void gpsRescueUpdate(void)
|
|||
|
||||
sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
|
||||
|
||||
static bool initialAltitudeLow = true;
|
||||
static bool returnAltitudeLow = true;
|
||||
static bool initialVelocityLow = true;
|
||||
rescueState.isAvailable = checkGPSRescueIsAvailable();
|
||||
|
||||
|
|
@ -796,16 +794,16 @@ void gpsRescueUpdate(void)
|
|||
rescueState.phase = RESCUE_ABORT;
|
||||
} else {
|
||||
// attempted initiation within minimum rescue distance requires us to fly out to at least that distance
|
||||
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
|
||||
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minStartDistM) {
|
||||
// climb above current height by buffer height, to at least 10m altitude
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->rescueAltitudeBufferM * 100.0f));
|
||||
// note that the pitch controller will pitch forward to fly out to minRescueDth
|
||||
rescueState.intent.returnAltitudeCm = fmaxf(1000.0f, rescueState.sensor.currentAltitudeCm + (gpsRescueConfig()->initialClimbM * 100.0f));
|
||||
// note that the pitch controller will pitch forward to fly out to minStartDistM
|
||||
// set the descent distance to half the minimum rescue distance. which we should have moved out to in the climb phase
|
||||
rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minRescueDth;
|
||||
rescueState.intent.descentDistanceM = 0.5f * gpsRescueConfig()->minStartDistM;
|
||||
}
|
||||
// otherwise behave as for a normal rescue
|
||||
initialiseRescueValues ();
|
||||
initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
|
||||
returnAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
|
||||
rescueState.phase = RESCUE_ATTAIN_ALT;
|
||||
}
|
||||
}
|
||||
|
|
@ -815,15 +813,15 @@ void gpsRescueUpdate(void)
|
|||
// 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
|
||||
if (initialAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) {
|
||||
if (returnAltitudeLow == (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm)) {
|
||||
// we started low, and still are low; also true if we started high, and still are too high
|
||||
const float altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
|
||||
const float altitudeStep = (returnAltitudeLow ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
|
||||
rescueState.intent.targetAltitudeCm += altitudeStep;
|
||||
} else {
|
||||
// target altitude achieved - move on to ROTATE phase, returning at target altitude
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
// if initiated too close, do not rotate or do anything else until sufficiently far away that a 'normal' rescue can happen
|
||||
if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minRescueDth) {
|
||||
if (rescueState.sensor.distanceToHomeM > gpsRescueConfig()->minStartDistM) {
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
}
|
||||
}
|
||||
|
|
@ -842,7 +840,7 @@ void gpsRescueUpdate(void)
|
|||
rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home
|
||||
}
|
||||
initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->rescueGroundspeed; // used to set direction of velocity target change
|
||||
initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->groundSpeedCmS; // used to set direction of velocity target change
|
||||
rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
|
||||
break;
|
||||
|
||||
|
|
@ -852,10 +850,10 @@ void gpsRescueUpdate(void)
|
|||
}
|
||||
// velocity PIDs are now active
|
||||
// update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
|
||||
const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
|
||||
const float targetVelocityError = gpsRescueConfig()->groundSpeedCmS - rescueState.intent.targetVelocityCmS;
|
||||
const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
|
||||
// velocityTargetStep is positive when starting low, negative when starting high
|
||||
const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed;
|
||||
const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->groundSpeedCmS;
|
||||
if (initialVelocityLow == targetVelocityIsLow) {
|
||||
// also true if started faster than target velocity and target is still high
|
||||
rescueState.intent.targetVelocityCmS += velocityTargetStep;
|
||||
|
|
|
|||
|
|
@ -1545,9 +1545,9 @@ case MSP_NAME:
|
|||
#ifdef USE_GPS_RESCUE
|
||||
case MSP_GPS_RESCUE:
|
||||
sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->returnAltitudeM);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->groundSpeedCmS);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMin);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleMax);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->throttleHover);
|
||||
|
|
@ -1560,7 +1560,9 @@ case MSP_NAME:
|
|||
sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
|
||||
// Added in API version 1.44
|
||||
sbufWriteU16(dst, gpsRescueConfig()->minRescueDth);
|
||||
sbufWriteU16(dst, gpsRescueConfig()->minStartDistM);
|
||||
// Added in API version 1.46
|
||||
sbufWriteU16(dst, gpsRescueConfig()->initialClimbM);
|
||||
break;
|
||||
|
||||
case MSP_GPS_RESCUE_PIDS:
|
||||
|
|
@ -2825,9 +2827,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
#ifdef USE_GPS_RESCUE
|
||||
case MSP_SET_GPS_RESCUE:
|
||||
gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->returnAltitudeM = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->groundSpeedCmS = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMin = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleMax = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->throttleHover = sbufReadU16(src);
|
||||
|
|
@ -2842,7 +2844,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
}
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
// Added in API version 1.44
|
||||
gpsRescueConfigMutable()->minRescueDth = sbufReadU16(src);
|
||||
gpsRescueConfigMutable()->minStartDistM = sbufReadU16(src);
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
// Added in API version 1.46
|
||||
gpsRescueConfigMutable()->initialClimbM = sbufReadU16(src);
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -282,8 +282,8 @@
|
|||
#define MSP_GPS_CONFIG 132 //out message GPS configuration
|
||||
//DEPRECATED - #define MSP_COMPASS_CONFIG 133 //out message Compass configuration
|
||||
#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM)
|
||||
#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats
|
||||
#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P
|
||||
#define MSP_GPS_RESCUE 135 //out message GPS Rescue angle, returnAltitude, descentDistance, groundSpeed, sanityChecks and minSats
|
||||
#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescue throttleP and velocity PIDS + yaw P
|
||||
#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data
|
||||
#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data
|
||||
#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.)
|
||||
|
|
@ -320,8 +320,8 @@
|
|||
#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
|
||||
#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
|
||||
//DEPRECATED - #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
|
||||
#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed and sanityChecks
|
||||
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P
|
||||
#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescue angle, returnAltitude, descentDistance, groundSpeed and sanityChecks
|
||||
#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescue throttleP and velocity PIDS + yaw P
|
||||
#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time)
|
||||
#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time)
|
||||
|
||||
|
|
|
|||
|
|
@ -33,13 +33,13 @@ PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCU
|
|||
|
||||
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||
|
||||
.minRescueDth = 15,
|
||||
.minStartDistM = 15,
|
||||
.altitudeMode = GPS_RESCUE_ALT_MODE_MAX,
|
||||
.rescueAltitudeBufferM = 10,
|
||||
.initialClimbM = 10,
|
||||
.ascendRate = 750, // cm/s, for altitude corrections on ascent
|
||||
|
||||
.initialAltitudeM = 30,
|
||||
.rescueGroundspeed = 750,
|
||||
.returnAltitudeM = 30,
|
||||
.groundSpeedCmS = 750,
|
||||
.maxRescueAngle = 45,
|
||||
.rollMix = 150,
|
||||
.pitchCutoffHz = 75,
|
||||
|
|
|
|||
|
|
@ -27,9 +27,9 @@
|
|||
typedef struct gpsRescue_s {
|
||||
|
||||
uint16_t maxRescueAngle; // degrees
|
||||
uint16_t initialAltitudeM; // meters
|
||||
uint16_t returnAltitudeM; // meters
|
||||
uint16_t descentDistanceM; // meters
|
||||
uint16_t rescueGroundspeed; // centimeters per second
|
||||
uint16_t groundSpeedCmS; // centimeters per second
|
||||
uint8_t throttleP, throttleI, throttleD;
|
||||
uint8_t yawP;
|
||||
uint16_t throttleMin;
|
||||
|
|
@ -37,7 +37,7 @@ typedef struct gpsRescue_s {
|
|||
uint16_t throttleHover;
|
||||
uint8_t minSats;
|
||||
uint8_t velP, velI, velD;
|
||||
uint16_t minRescueDth; // meters
|
||||
uint16_t minStartDistM; // meters
|
||||
uint8_t sanityChecks;
|
||||
uint8_t allowArmingWithoutFix;
|
||||
uint8_t useMag;
|
||||
|
|
@ -45,7 +45,7 @@ typedef struct gpsRescue_s {
|
|||
uint8_t altitudeMode;
|
||||
uint16_t ascendRate;
|
||||
uint16_t descendRate;
|
||||
uint16_t rescueAltitudeBufferM; // meters
|
||||
uint16_t initialClimbM; // meters
|
||||
uint8_t rollMix;
|
||||
uint8_t disarmThreshold;
|
||||
uint8_t pitchCutoffHz;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue