mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 17:55:30 +03:00
Improve landing, update OSD, BB names, smooth Pitch, sanity bugfix
Bugfix potential sanity check failure on negative ascent or slow descent Bugfix sanity failure on low climb authority builds Update GPS Rescue OSD, CLI, Blackbox names and sequence Allow zero climb altitude and return speed Change some GPS Rescue altitude names max rescue yaw rate = 180 deg/s check yaw error angle faster update rate Yaw faster Initate yaw smoothly. Increase throttle D during faster descents climb without rotating smoother transition to fly home fix maxAlt bug by using float update heading debug to include groundSpeed etc altitude lowpass default 3Hz not 4Hz increase disarm threshold from 1.0g to 1.5g PT3 on pitch at 4Hz.
This commit is contained in:
ctzsnooze
parent
b88e73d137
commit
47460f55e1
6 changed files with 313 additions and 226 deletions
|
|
@ -1528,12 +1528,27 @@ 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_ANGLE, "%d", gpsRescueConfig()->angle)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_BUFFER, "%d", gpsRescueConfig()->rescueAltitudeBufferM)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_ALT, "%d", gpsRescueConfig()->initialAltitudeM)
|
||||
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_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_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)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
|
||||
|
|
@ -1541,17 +1556,8 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_DTH, "%d", gpsRescueConfig()->minRescueDth)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode)
|
||||
|
||||
#ifdef USE_MAG
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag)
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1015,12 +1015,27 @@ const clivalue_t valueTable[] = {
|
|||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
// PG_GPS_RESCUE
|
||||
{ PARAM_NAME_GPS_RESCUE_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ALT_BUFFER, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueAltitudeBufferM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_INITIAL_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_MIN_START_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
|
||||
{ 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_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_PITCH_ANGLE_MAX, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
|
||||
{ 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_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
|
||||
{ PARAM_NAME_GPS_RESCUE_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
|
||||
{ PARAM_NAME_GPS_RESCUE_GROUND_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 250, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
||||
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) },
|
||||
|
|
@ -1028,17 +1043,7 @@ const clivalue_t valueTable[] = {
|
|||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_I, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
|
||||
{ PARAM_NAME_GPS_RESCUE_VELOCITY_D, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
|
||||
{ PARAM_NAME_GPS_RESCUE_YAW_P, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
|
||||
{ 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_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
|
||||
{ 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_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
|
||||
{ PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
|
||||
{ PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
||||
{ PARAM_NAME_GPS_RESCUE_MIN_DTH, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
|
||||
{ PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
||||
{ 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) },
|
||||
#ifdef USE_MAG
|
||||
{ PARAM_NAME_GPS_RESCUE_USE_MAG, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -40,23 +40,30 @@
|
|||
#include "flight/gps_rescue.h"
|
||||
#include "io/gps.h"
|
||||
|
||||
static uint16_t gpsRescueConfig_angle; //degrees
|
||||
static uint16_t gpsRescueConfig_initialAltitudeM; //meters
|
||||
static uint16_t gpsRescueConfig_descentDistanceM; //meters
|
||||
static uint16_t gpsRescueConfig_minRescueDth; //meters
|
||||
static uint8_t gpsRescueConfig_altitudeMode;
|
||||
static uint8_t gpsRescueConfig_rescueAltitudeBufferM; // meters
|
||||
static uint16_t gpsRescueConfig_ascendRate;
|
||||
|
||||
static uint8_t gpsRescueConfig_initialAltitudeM; //meters
|
||||
static uint16_t gpsRescueConfig_rescueGroundspeed; // centimeters per second
|
||||
static uint8_t gpsRescueConfig_angle; //degrees
|
||||
|
||||
static uint16_t gpsRescueConfig_descentDistanceM; //meters
|
||||
static uint16_t gpsRescueConfig_descendRate;
|
||||
static uint8_t gpsRescueConfig_targetLandingAltitudeM;
|
||||
|
||||
static uint16_t gpsRescueConfig_throttleMin;
|
||||
static uint16_t gpsRescueConfig_throttleMax;
|
||||
static uint16_t gpsRescueConfig_throttleHover;
|
||||
static uint8_t gpsConfig_gpsMinimumSats;
|
||||
static uint16_t gpsRescueConfig_minRescueDth; //meters
|
||||
|
||||
static uint8_t gpsConfig_gpsRequiredSats;
|
||||
static uint8_t gpsRescueConfig_allowArmingWithoutFix;
|
||||
static uint16_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
|
||||
static uint16_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
|
||||
static uint16_t gpsRescueConfig_yawP;
|
||||
static uint8_t gpsRescueConfig_targetLandingAltitudeM;
|
||||
static uint8_t gpsRescueConfig_altitudeMode;
|
||||
static uint16_t gpsRescueConfig_ascendRate;
|
||||
static uint16_t gpsRescueConfig_descendRate;
|
||||
|
||||
static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueConfig_throttleD;
|
||||
static uint8_t gpsRescueConfig_yawP;
|
||||
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
|
||||
|
||||
|
||||
static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
|
||||
{
|
||||
|
|
@ -97,15 +104,15 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
|
|||
{
|
||||
{"--- GPS RESCUE PID---", OME_Label, NULL, NULL},
|
||||
|
||||
{ "THROTTLE P", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleP, 0, 500, 1 } },
|
||||
{ "THROTTLE I", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleI, 0, 500, 1 } },
|
||||
{ "THROTTLE D", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleD, 0, 500, 1 } },
|
||||
{ "THROTTLE P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleP, 0, 255, 1 } },
|
||||
{ "THROTTLE I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleI, 0, 255, 1 } },
|
||||
{ "THROTTLE D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_throttleD, 0, 255, 1 } },
|
||||
|
||||
{ "YAW P", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_yawP, 0, 500, 1 } },
|
||||
{ "YAW P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_yawP, 0, 255, 1 } },
|
||||
|
||||
{ "VELOCITY P", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velP, 0, 500, 1 } },
|
||||
{ "VELOCITY I", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velI, 0, 500, 1 } },
|
||||
{ "VELOCITY D", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_velD, 0, 500, 1 } },
|
||||
{ "VELOCITY P", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velP, 0, 255, 1 } },
|
||||
{ "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } },
|
||||
{ "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } },
|
||||
|
||||
{"BACK", OME_Back, NULL, NULL},
|
||||
{NULL, OME_END, NULL, NULL}
|
||||
|
|
@ -126,20 +133,25 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
|
|||
{
|
||||
UNUSED(pDisp);
|
||||
|
||||
gpsRescueConfig_angle = gpsRescueConfig()->angle;
|
||||
gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth;
|
||||
gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode;
|
||||
gpsRescueConfig_rescueAltitudeBufferM = gpsRescueConfig()->rescueAltitudeBufferM;
|
||||
gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate;
|
||||
|
||||
gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
|
||||
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
|
||||
gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
|
||||
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin ;
|
||||
gpsRescueConfig_angle = gpsRescueConfig()->angle;
|
||||
|
||||
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
|
||||
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
|
||||
gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM;
|
||||
|
||||
gpsRescueConfig_throttleMin = gpsRescueConfig()->throttleMin;
|
||||
gpsRescueConfig_throttleMax = gpsRescueConfig()->throttleMax;
|
||||
gpsRescueConfig_throttleHover = gpsRescueConfig()->throttleHover;
|
||||
gpsConfig_gpsMinimumSats = gpsConfig()->gpsMinimumSats;
|
||||
gpsRescueConfig_minRescueDth = gpsRescueConfig()->minRescueDth;
|
||||
|
||||
gpsConfig_gpsRequiredSats = gpsConfig()->gpsRequiredSats;
|
||||
gpsRescueConfig_allowArmingWithoutFix = gpsRescueConfig()->allowArmingWithoutFix;
|
||||
gpsRescueConfig_targetLandingAltitudeM = gpsRescueConfig()->targetLandingAltitudeM;
|
||||
gpsRescueConfig_altitudeMode = gpsRescueConfig()->altitudeMode;
|
||||
gpsRescueConfig_ascendRate = gpsRescueConfig()->ascendRate;
|
||||
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
|
@ -149,20 +161,25 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
|
|||
UNUSED(pDisp);
|
||||
UNUSED(self);
|
||||
|
||||
gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
|
||||
gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth;
|
||||
gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode;
|
||||
gpsRescueConfigMutable()->rescueAltitudeBufferM = gpsRescueConfig_rescueAltitudeBufferM;
|
||||
gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate;
|
||||
|
||||
gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
|
||||
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
|
||||
gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
|
||||
gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
|
||||
|
||||
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
|
||||
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
|
||||
gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM;
|
||||
|
||||
gpsRescueConfigMutable()->throttleMin = gpsRescueConfig_throttleMin;
|
||||
gpsRescueConfigMutable()->throttleMax = gpsRescueConfig_throttleMax;
|
||||
gpsRescueConfigMutable()->throttleHover = gpsRescueConfig_throttleHover;
|
||||
gpsConfigMutable()->gpsMinimumSats = gpsConfig_gpsMinimumSats;
|
||||
gpsRescueConfigMutable()->minRescueDth = gpsRescueConfig_minRescueDth;
|
||||
|
||||
gpsConfigMutable()->gpsRequiredSats = gpsConfig_gpsRequiredSats;
|
||||
gpsRescueConfigMutable()->allowArmingWithoutFix = gpsRescueConfig_allowArmingWithoutFix;
|
||||
gpsRescueConfigMutable()->targetLandingAltitudeM = gpsRescueConfig_targetLandingAltitudeM;
|
||||
gpsRescueConfigMutable()->altitudeMode = gpsRescueConfig_altitudeMode;
|
||||
gpsRescueConfigMutable()->ascendRate = gpsRescueConfig_ascendRate;
|
||||
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
|
@ -171,20 +188,26 @@ const OSD_Entry cmsx_menuGpsRescueEntries[] =
|
|||
{
|
||||
{"--- GPS RESCUE ---", OME_Label, NULL, NULL},
|
||||
|
||||
{ "ANGLE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_angle, 0, 60, 1 } },
|
||||
{ "MIN DIST HOME M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 20, 1000, 1 } },
|
||||
{ "INITAL ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_initialAltitudeM, 2, 100, 1 } },
|
||||
{ "MIN START DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_minRescueDth, 20, 1000, 1 } },
|
||||
{ "ALTITUDE MODE" , OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gpsRescueConfig_altitudeMode, 2, lookupTableRescueAltitudeMode} },
|
||||
{ "DESCENT DIST M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descentDistanceM, 10, 500, 1 } },
|
||||
{ "LANDING ALT M", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 15, 1 } },
|
||||
{ "GROUND SPEED CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_rescueGroundspeed, 250, 3000, 1 } },
|
||||
{ "INITAL CLIMB M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_rescueAltitudeBufferM, 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 } },
|
||||
{ "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 } },
|
||||
{ "DESCENT RATE CM/S", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } },
|
||||
{ "LANDING ALT M", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_targetLandingAltitudeM, 3, 15, 1 } },
|
||||
|
||||
{ "THROTTLE MIN", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMin, 1000, 2000, 1 } },
|
||||
{ "THROTTLE MAX", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleMax, 1000, 2000, 1 } },
|
||||
{ "THROTTLE HOV", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_throttleHover, 1000, 2000, 1 } },
|
||||
{ "ASCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_ascendRate, 50, 2500, 1 } },
|
||||
{ "DESCEND RATE", OME_UINT16 | REBOOT_REQUIRED, NULL, &(OSD_UINT16_t){ &gpsRescueConfig_descendRate, 25, 500, 1 } },
|
||||
|
||||
{ "SATS REQUIRED", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsConfig_gpsRequiredSats, 4, 50, 1 } },
|
||||
{ "ARM WITHOUT FIX", OME_Bool | REBOOT_REQUIRED, NULL, &gpsRescueConfig_allowArmingWithoutFix },
|
||||
{ "MIN SATELITES", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsConfig_gpsMinimumSats, 4, 50, 1 } },
|
||||
|
||||
{ "GPS RESCUE PID", OME_Submenu, cmsMenuChange, &cms_menuGpsRescuePid},
|
||||
|
||||
{"BACK", OME_Back, NULL, NULL},
|
||||
|
|
|
|||
|
|
@ -136,12 +136,27 @@
|
|||
#define PARAM_NAME_GPS_MINIMUM_SATS "gps_minimum_sats"
|
||||
|
||||
#ifdef USE_GPS_RESCUE
|
||||
#define PARAM_NAME_GPS_RESCUE_ANGLE "gps_rescue_angle"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALT_BUFFER "gps_rescue_alt_buffer"
|
||||
#define PARAM_NAME_GPS_RESCUE_INITIAL_ALT "gps_rescue_initial_alt"
|
||||
#define PARAM_NAME_GPS_RESCUE_MIN_START_DIST "gps_rescue_min_start_dist"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALT_MODE "gps_rescue_alt_mode"
|
||||
#define PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB "gps_rescue_initial_climb"
|
||||
#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_PITCH_ANGLE_MAX "gps_rescue_pitch_angle_max"
|
||||
#define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist"
|
||||
#define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
|
||||
#define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt"
|
||||
#define PARAM_NAME_GPS_RESCUE_GROUND_SPEED "gps_rescue_ground_speed"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
|
||||
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_P "gps_rescue_throttle_p"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_I "gps_rescue_throttle_i"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_D "gps_rescue_throttle_d"
|
||||
|
|
@ -149,16 +164,7 @@
|
|||
#define PARAM_NAME_GPS_RESCUE_VELOCITY_I "gps_rescue_velocity_i"
|
||||
#define PARAM_NAME_GPS_RESCUE_VELOCITY_D "gps_rescue_velocity_d"
|
||||
#define PARAM_NAME_GPS_RESCUE_YAW_P "gps_rescue_yaw_p"
|
||||
#define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"
|
||||
#define PARAM_NAME_GPS_RESCUE_ASCEND_RATE "gps_rescue_ascend_rate"
|
||||
#define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
|
||||
#define PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER "gps_rescue_throttle_hover"
|
||||
#define PARAM_NAME_GPS_RESCUE_SANITY_CHECKS "gps_rescue_sanity_checks"
|
||||
#define PARAM_NAME_GPS_RESCUE_MIN_DTH "gps_rescue_min_dth"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX "gps_rescue_allow_arming_without_fix"
|
||||
#define PARAM_NAME_GPS_RESCUE_ALT_MODE "gps_rescue_alt_mode"
|
||||
|
||||
#ifdef USE_MAG
|
||||
#define PARAM_NAME_GPS_RESCUE_USE_MAG "gps_rescue_use_mag"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h" // remove this later, only here temporarily for the cutoff for D
|
||||
|
||||
#include "gps_rescue.h"
|
||||
|
||||
|
|
@ -86,6 +85,7 @@ typedef enum {
|
|||
typedef struct {
|
||||
float returnAltitudeCm;
|
||||
float targetAltitudeCm;
|
||||
float targetLandingAltitudeCm;
|
||||
uint16_t targetVelocityCmS;
|
||||
uint8_t pitchAngleLimitDeg;
|
||||
int8_t rollAngleLimitDeg; // must have a negative to positive range
|
||||
|
|
@ -93,10 +93,12 @@ typedef struct {
|
|||
float descentDistanceM;
|
||||
int8_t secondsFailing;
|
||||
float altitudeStep;
|
||||
float descentSpeedModifier;
|
||||
float yawAttenuator;
|
||||
} rescueIntent_s;
|
||||
|
||||
typedef struct {
|
||||
int32_t maxAltitudeCm;
|
||||
float maxAltitudeCm;
|
||||
float currentAltitudeCm;
|
||||
float distanceToHomeCm;
|
||||
float distanceToHomeM;
|
||||
|
|
@ -128,8 +130,8 @@ typedef enum {
|
|||
CURRENT_ALT
|
||||
} altitudeMode_e;
|
||||
|
||||
#define GPS_RESCUE_MAX_YAW_RATE 90 // deg/sec max yaw rate
|
||||
#define GPS_RESCUE_MIN_DESCENT_DIST_M 10 // minimum descent distance allowed
|
||||
#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
|
||||
|
|
@ -140,13 +142,30 @@ typedef enum {
|
|||
#define GPS_RESCUE_USE_MAG false
|
||||
#endif
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 2);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
|
||||
|
||||
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||
.angle = 40,
|
||||
.minRescueDth = 30,
|
||||
.altitudeMode = MAX_ALT,
|
||||
.rescueAltitudeBufferM = 10,
|
||||
.ascendRate = 500, // cm/s, for altitude corrections on ascent
|
||||
|
||||
.initialAltitudeM = 30,
|
||||
.descentDistanceM = 20,
|
||||
.rescueGroundspeed = 500,
|
||||
.angle = 40,
|
||||
.rollMix = 100,
|
||||
|
||||
.descentDistanceM = 20,
|
||||
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
|
||||
.targetLandingAltitudeM = 5,
|
||||
|
||||
.throttleMin = 1100,
|
||||
.throttleMax = 1600,
|
||||
.throttleHover = 1275,
|
||||
|
||||
.allowArmingWithoutFix = false,
|
||||
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
||||
|
||||
.throttleP = 15,
|
||||
.throttleI = 15,
|
||||
.throttleD = 15,
|
||||
|
|
@ -154,19 +173,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
|||
.velI = 20,
|
||||
.velD = 50,
|
||||
.yawP = 25,
|
||||
.throttleMin = 1100,
|
||||
.throttleMax = 1600,
|
||||
.throttleHover = 1275,
|
||||
.sanityChecks = RESCUE_SANITY_FS_ONLY,
|
||||
.minRescueDth = 30,
|
||||
.allowArmingWithoutFix = false,
|
||||
.useMag = GPS_RESCUE_USE_MAG,
|
||||
.targetLandingAltitudeM = 5,
|
||||
.altitudeMode = MAX_ALT,
|
||||
.ascendRate = 500, // cm/s, for altitude corrections on ascent
|
||||
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
|
||||
.rescueAltitudeBufferM = 10,
|
||||
.rollMix = 100
|
||||
|
||||
.useMag = GPS_RESCUE_USE_MAG
|
||||
);
|
||||
|
||||
static float rescueThrottle;
|
||||
|
|
@ -175,6 +183,7 @@ float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
|||
bool magForceDisable = false;
|
||||
static bool newGPSData = false;
|
||||
static pt2Filter_t throttleDLpf;
|
||||
static pt3Filter_t pitchLpf;
|
||||
|
||||
rescueState_s rescueState;
|
||||
|
||||
|
|
@ -185,6 +194,10 @@ void gpsRescueInit(void)
|
|||
const float throttleDCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
|
||||
const float throttleDCutoffGain = pt2FilterGain(throttleDCutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&throttleDLpf, throttleDCutoffGain);
|
||||
|
||||
const float pitchCutoffHz = 4.0f;
|
||||
const float pitchCutoffGain = pt3FilterGain(pitchCutoffHz, sampleTimeS);
|
||||
pt3FilterInit(&pitchLpf, pitchCutoffGain);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -299,16 +312,16 @@ static void rescueAttainPosition()
|
|||
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
|
||||
// up to 20% increase in throttle from I alone
|
||||
|
||||
// D component from position.c
|
||||
// float throttleD = rescueState.sensor.currentAltitudeDerivative;
|
||||
|
||||
// is error based, so includes positive boost when climbing and negative boost on descent
|
||||
const float throttleDRaw = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
|
||||
// D component is error based, so includes positive boost when climbing and negative boost on descent
|
||||
float throttleDRaw = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
|
||||
previousAltitudeError = altitudeError;
|
||||
throttleDRaw += rescueState.intent.descentSpeedModifier * throttleDRaw;
|
||||
// add up to 2x D when descent rate is faster
|
||||
|
||||
float throttleD = pt2FilterApply(&throttleDLpf, throttleDRaw);
|
||||
throttleD = gpsRescueConfig()->throttleD * throttleD;
|
||||
|
||||
// acceleration component not implemented - was needed previously due to GPS lag, maybe not needed now.
|
||||
// acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
|
||||
|
||||
float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
|
||||
tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000);
|
||||
|
|
@ -322,14 +335,6 @@ static void rescueAttainPosition()
|
|||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
|
||||
|
||||
|
||||
// *** other updates occur only with new GPS data ***
|
||||
if (!newGPSData) {
|
||||
return;
|
||||
}
|
||||
|
||||
const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
|
||||
|
||||
/**
|
||||
Heading / yaw controller
|
||||
*/
|
||||
|
|
@ -343,7 +348,9 @@ static void rescueAttainPosition()
|
|||
// 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
|
||||
|
||||
rescueYaw = constrainf(rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * 0.1f, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
|
||||
if (rescueState.intent.updateYaw) {
|
||||
rescueYaw = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator * 0.1f;
|
||||
rescueYaw = constrainf(rescueYaw, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
|
||||
// rescueYaw is the yaw rate in deg/s to correct the heading error
|
||||
|
||||
const float rollMixAttenuator = constrainf(1.0f - ABS(rescueYaw) * 0.01f, 0.0f, 1.0f);
|
||||
|
|
@ -357,16 +364,19 @@ static void rescueAttainPosition()
|
|||
// gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c
|
||||
|
||||
rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
||||
if (!rescueState.intent.updateYaw) {
|
||||
|
||||
} else {
|
||||
rescueYaw = 0.0f;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueYaw * 10.0f); // Rescue yaw rate in degrees/sec * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsRescueAngle[AI_ROLL]); // roll correction degrees * 100
|
||||
|
||||
/**
|
||||
Pitch / velocity controller
|
||||
*/
|
||||
static float pitchAdjustment = 0.0f;
|
||||
static float pitchAdjustmentFiltered = 0.0f;
|
||||
if (newGPSData) {
|
||||
|
||||
const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
|
||||
const float velocityTargetLimiter = constrainf((60.0f -rescueState.sensor.absErrorAngle) / 60.0f, 0.0f, 1.0f);
|
||||
// attenuate velocity target when quad is not pointing towards home
|
||||
// stops attempting to gain velocity when pointing the wrong way, eg overshooting home point (sudden heading error)
|
||||
|
|
@ -397,7 +407,7 @@ static void rescueAttainPosition()
|
|||
velocityD *= gpsRescueConfig()->velD;
|
||||
|
||||
// Pitch PIDsum and smoothing
|
||||
float pitchAdjustment = velocityP + velocityD + velocityI;
|
||||
pitchAdjustment = velocityP + velocityD + velocityI;
|
||||
// simple rate of change limiter - not more than 25 deg/s of pitch change to keep pitch smooth
|
||||
float pitchAdjustmentDelta = pitchAdjustment - previousPitchAdjustment;
|
||||
if (pitchAdjustmentDelta > rescueState.sensor.maxPitchStep) {
|
||||
|
|
@ -412,20 +422,27 @@ static void rescueAttainPosition()
|
|||
// pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100
|
||||
// it gets added to the normal level mode Pitch adjustments in pid.c
|
||||
|
||||
gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustment, -rescueState.intent.pitchAngleLimitDeg * 100.0f, rescueState.intent.pitchAngleLimitDeg * 100.0f);
|
||||
// this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
|
||||
|
||||
DEBUG_SET(DEBUG_RTH, 0, gpsRescueAngle[AI_PITCH]);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, velocityP);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, velocityD);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, rescueState.intent.targetVelocityCmS);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, rescueState.intent.targetVelocityCmS);
|
||||
}
|
||||
|
||||
pitchAdjustmentFiltered = pt3FilterApply(&pitchLpf, pitchAdjustment);
|
||||
// upsampling smoothing of steps in pitch angle
|
||||
|
||||
gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustmentFiltered, -rescueState.intent.pitchAngleLimitDeg * 100.0f, rescueState.intent.pitchAngleLimitDeg * 100.0f);
|
||||
// this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
|
||||
|
||||
DEBUG_SET(DEBUG_RTH, 0, gpsRescueAngle[AI_PITCH]);
|
||||
|
||||
}
|
||||
|
||||
static void performSanityChecks()
|
||||
{
|
||||
static timeUs_t previousTimeUs = 0; // Last time Stalled/LowSat was checked
|
||||
static int32_t prevAltitudeCm = 0.0f; // to calculate ascent or descent change
|
||||
static float prevAltitudeCm = 0.0f; // to calculate ascent or descent change
|
||||
static float prevTargetAltitudeCm = 0.0f; // to calculate ascent or descent target change
|
||||
static int8_t secondsLowSats = 0; // Minimum sat detection
|
||||
static int8_t secondsDoingNothing = 0; // Limit on doing nothing
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
|
@ -437,6 +454,7 @@ static void performSanityChecks()
|
|||
// Initialize internal variables each time GPS Rescue is started
|
||||
previousTimeUs = currentTimeUs;
|
||||
prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
|
||||
secondsLowSats = 5; // Start the count at 5 to be less forgiving at the beginning
|
||||
secondsDoingNothing = 0;
|
||||
return;
|
||||
|
|
@ -492,16 +510,20 @@ static void performSanityChecks()
|
|||
}
|
||||
|
||||
// These conditions are 'special', in that even with sanity checks off, they should still apply
|
||||
const float actualAltitudeChange = rescueState.sensor.currentAltitudeCm - prevAltitudeCm;
|
||||
const float targetAltitudeChange = rescueState.intent.targetAltitudeCm - prevTargetAltitudeCm;
|
||||
const float ratio = actualAltitudeChange / targetAltitudeChange;
|
||||
if (rescueState.phase == RESCUE_ATTAIN_ALT) {
|
||||
rescueState.intent.secondsFailing += (rescueState.sensor.currentAltitudeCm - prevAltitudeCm) > (0.5f * gpsRescueConfig()->ascendRate) ? -1 : 1;
|
||||
rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 10);if (rescueState.intent.secondsFailing == 10) {
|
||||
rescueState.intent.secondsFailing += ratio > 0.5f ? -1 : 1;
|
||||
rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 10);
|
||||
if (rescueState.intent.secondsFailing == 10) {
|
||||
{
|
||||
rescueState.phase = RESCUE_ABORT;
|
||||
// if stuck in a tree while climbing, or otherwise can't climb, stop motors and disarm
|
||||
}
|
||||
}
|
||||
} else if (rescueState.phase == RESCUE_LANDING || rescueState.phase == RESCUE_DESCENT) {
|
||||
rescueState.intent.secondsFailing += (prevAltitudeCm - rescueState.sensor.currentAltitudeCm) > (0.5f * gpsRescueConfig()->descendRate) ? -1 : 1;
|
||||
rescueState.intent.secondsFailing += ratio > 0.5f ? -1 : 1;
|
||||
rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 10);
|
||||
if (rescueState.intent.secondsFailing == 10) {
|
||||
{
|
||||
|
|
@ -519,6 +541,7 @@ static void performSanityChecks()
|
|||
}
|
||||
}
|
||||
prevAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||
prevTargetAltitudeCm = rescueState.intent.targetAltitudeCm;
|
||||
|
||||
secondsLowSats += gpsSol.numSat < (gpsConfig()->gpsMinimumSats) ? 1 : -1;
|
||||
secondsLowSats = constrain(secondsLowSats, 0, 10);
|
||||
|
|
@ -544,7 +567,11 @@ static void sensorUpdate()
|
|||
rescueState.sensor.currentAltitudeCm = getAltitude();
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm);
|
||||
// may be updated more frequently than GPS data
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 0, rescueState.sensor.groundSpeedCmS); // groundspeed cm/s
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 1, gpsSol.groundCourse); // degrees * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 3, rescueState.sensor.directionToHome); // degrees * 10
|
||||
|
||||
rescueState.sensor.healthy = gpsIsHealthy();
|
||||
|
||||
|
|
@ -553,13 +580,6 @@ static void sensorUpdate()
|
|||
rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
|
||||
}
|
||||
|
||||
if (!newGPSData) {
|
||||
return;
|
||||
}
|
||||
|
||||
rescueState.sensor.distanceToHomeCm = GPS_distanceToHomeCm;
|
||||
rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
|
||||
rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
|
||||
rescueState.sensor.directionToHome = GPS_directionToHome;
|
||||
rescueState.sensor.errorAngle = (attitude.values.yaw - rescueState.sensor.directionToHome) * 0.1f;
|
||||
// both attitude and direction are in degrees * 10, errorAngle is degrees
|
||||
|
|
@ -570,6 +590,15 @@ static void sensorUpdate()
|
|||
}
|
||||
rescueState.sensor.absErrorAngle = fabsf(rescueState.sensor.errorAngle);
|
||||
|
||||
if (!newGPSData) {
|
||||
return;
|
||||
}
|
||||
|
||||
rescueState.sensor.distanceToHomeCm = GPS_distanceToHomeCm;
|
||||
rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
|
||||
rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
|
||||
|
||||
|
||||
static timeUs_t previousGPSDataTimeUs = 0;
|
||||
const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
|
||||
rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
|
||||
|
|
@ -585,10 +614,9 @@ static void sensorUpdate()
|
|||
|
||||
rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 3, rescueState.sensor.directionToHome); // degrees * 10
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, rescueState.sensor.velocityToHomeCmS);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, rescueState.sensor.velocityToHomeCmS);
|
||||
|
||||
}
|
||||
|
||||
// This function flashes "RESCUE N/A" in the OSD if:
|
||||
|
|
@ -643,17 +671,34 @@ static bool checkGPSRescueIsAvailable(void)
|
|||
|
||||
void disarmOnImpact(void)
|
||||
{
|
||||
if (rescueState.sensor.accMagnitude > 2.0f) {
|
||||
if (rescueState.sensor.accMagnitude > 3.0f) {
|
||||
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
|
||||
disarm(DISARM_REASON_GPS_RESCUE);
|
||||
rescueStop();
|
||||
}
|
||||
}
|
||||
|
||||
void descend(void)
|
||||
{
|
||||
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
// adjust altitude step for interval between altitude readings
|
||||
|
||||
const float descentAttenuator = rescueState.intent.returnAltitudeCm / 2000.0f;
|
||||
// descend more slowly if return altitude is lower than 20m
|
||||
if (descentAttenuator < 1.0f) {
|
||||
rescueState.intent.altitudeStep *= descentAttenuator;
|
||||
}
|
||||
|
||||
rescueState.intent.descentSpeedModifier = constrainf(rescueState.sensor.currentAltitudeCm / 4000.0f, 0.0f, 1.0f);
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + (2.0f * rescueState.intent.descentSpeedModifier));
|
||||
// increase descent rate to max of 3x default above 40m, 2x above 20m, 1.166 at 5m, default at ground level.
|
||||
}
|
||||
|
||||
void altitudeAchieved(void)
|
||||
{
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
rescueState.intent.altitudeStep = 0;
|
||||
rescueState.intent.updateYaw = true; // allow yaw updating
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
}
|
||||
|
||||
|
|
@ -673,7 +718,6 @@ void updateGPSRescueState(void)
|
|||
sensorUpdate(); // always get latest GPS / Altitude data; update ascend and descend rates
|
||||
|
||||
uint8_t halfAngle = gpsRescueConfig()->angle / 2;
|
||||
const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
|
||||
float proximityToLandingArea = 0.0f;
|
||||
bool startedLow = true;
|
||||
rescueState.isAvailable = checkGPSRescueIsAvailable();
|
||||
|
|
@ -700,18 +744,24 @@ void updateGPSRescueState(void)
|
|||
} else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
|
||||
// Attempt to initiate inside minimum activation distance -> landing mode
|
||||
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
|
||||
rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
|
||||
rescueState.intent.rollAngleLimitDeg = 0; // flat on roll also
|
||||
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
|
||||
rescueState.phase = RESCUE_LANDING;
|
||||
// start landing from current altitude
|
||||
} else {
|
||||
rescueState.phase = RESCUE_ATTAIN_ALT;
|
||||
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 = true; // point the nose to home at all times during the rescue
|
||||
rescueState.intent.updateYaw = false; // point the nose to home at all times during the rescue
|
||||
rescueState.intent.targetVelocityCmS = 0; // zero forward velocity while climbing
|
||||
rescueState.intent.pitchAngleLimitDeg = halfAngle; // only half pitch authority
|
||||
rescueState.intent.rollAngleLimitDeg = 0; // don't roll yet
|
||||
rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
|
||||
rescueState.intent.rollAngleLimitDeg = 0; // flat on roll also
|
||||
rescueState.intent.altitudeStep = 0;
|
||||
rescueState.intent.descentSpeedModifier = 0.0f;
|
||||
rescueState.intent.yawAttenuator = 0.0f;
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -723,36 +773,39 @@ void updateGPSRescueState(void)
|
|||
if (startedLow) {
|
||||
if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
} else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
|
||||
altitudeAchieved();
|
||||
}
|
||||
} else {
|
||||
if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
} else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
|
||||
altitudeAchieved();
|
||||
}
|
||||
}
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
break;
|
||||
|
||||
case RESCUE_ROTATE:
|
||||
// we may bypass attain_alt so this must stand alone.
|
||||
// complete the rotation, allowing pitch when pointing towards home
|
||||
// keep in mind that rotation may already be complete
|
||||
if (newGPSData) {
|
||||
if (rescueState.sensor.absErrorAngle < 60.0f) {
|
||||
// give the craft a forward velocity target (will be attenuated by heading) and full pitch authority
|
||||
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed;
|
||||
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle;
|
||||
if (rescueState.sensor.absErrorAngle < 15.0f) {
|
||||
// enable roll, enter full fly home phase
|
||||
// gradually acquire yaw and roll authority over 50 iterations, or about half a second
|
||||
if (rescueState.intent.yawAttenuator < 1.0f) {
|
||||
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;
|
||||
rescueState.intent.rollAngleLimitDeg = 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;
|
||||
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle;
|
||||
// enable roll with yaw
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
@ -762,7 +815,6 @@ void updateGPSRescueState(void)
|
|||
if (newGPSData) {
|
||||
if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
|
||||
rescueState.phase = RESCUE_DESCENT;
|
||||
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; // negative step dsescending
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
|
||||
}
|
||||
}
|
||||
|
|
@ -771,10 +823,9 @@ void updateGPSRescueState(void)
|
|||
case RESCUE_DESCENT:
|
||||
// attenuate velocity and altitude targets while updating the heading to home
|
||||
// once inside the landing box, stop rotating, just descend
|
||||
if (rescueState.sensor.currentAltitudeCm < targetLandingAltitudeCm) {
|
||||
if (rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) {
|
||||
// enter landing mode once below landing altitude
|
||||
rescueState.phase = RESCUE_LANDING;
|
||||
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
|
||||
rescueState.intent.targetVelocityCmS = 0; // zero velocity to home
|
||||
rescueState.intent.pitchAngleLimitDeg = halfAngle; // reduced pitch angles
|
||||
|
|
@ -790,15 +841,12 @@ void updateGPSRescueState(void)
|
|||
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
|
||||
// reduce roll capability when closer to home, none within final 2m
|
||||
}
|
||||
const float proximityToGround = constrainf(rescueState.intent.targetAltitudeCm / 3000.0f, 0.0f, 2.0f);
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + proximityToGround);
|
||||
// reduce current altitude inexorably, by not less than descendStepCm and not more than 5 * altitudeStep
|
||||
descend();
|
||||
break;
|
||||
|
||||
case RESCUE_LANDING:
|
||||
// keep reducing target altitude, keep nose to home, zero velocity target with limited pitch control, no roll
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
// take one step off target altitude every time we get new GPS data
|
||||
// control yaw angle, throttle and pitch. no roll. descend steadily until impact, then disarm.
|
||||
descend();
|
||||
disarmOnImpact();
|
||||
break;
|
||||
|
||||
|
|
@ -821,7 +869,6 @@ void updateGPSRescueState(void)
|
|||
}
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, rescueState.intent.targetAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.intent.targetAltitudeCm);
|
||||
DEBUG_SET(DEBUG_RTH, 1, rescueState.phase);
|
||||
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4
|
|||
PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
|
||||
.altitude_source = DEFAULT,
|
||||
.altitude_prefer_baro = 100,
|
||||
.altitude_lpf = 400,
|
||||
.altitude_lpf = 300,
|
||||
.altitude_d_lpf = 100,
|
||||
);
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue