Renamed 'gpsRescue' to 'gpsRescueConfig' and moved it into the appropriate location.
This commit is contained in:
parent
98fb4d7c66
commit
d535fd6180
|
@ -257,7 +257,7 @@ void updateArmingStatus(void)
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
|
if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
|
||||||
if (rescueState.sensor.numSat < gpsRescue()->minSats) {
|
if (rescueState.sensor.numSat < gpsRescueConfig()->minSats) {
|
||||||
setArmingDisabled(ARMING_DISABLED_GPS);
|
setArmingDisabled(ARMING_DISABLED_GPS);
|
||||||
} else {
|
} else {
|
||||||
unsetArmingDisabled(ARMING_DISABLED_GPS);
|
unsetArmingDisabled(ARMING_DISABLED_GPS);
|
||||||
|
|
|
@ -18,32 +18,59 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "platform.h"
|
||||||
#include "common/maths.h"
|
|
||||||
|
|
||||||
#include "build/debug.h"
|
|
||||||
|
|
||||||
#include "drivers/time.h"
|
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
|
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
|
#include "common/axis.h"
|
||||||
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
#include "drivers/time.h"
|
||||||
|
|
||||||
#include "io/gps.h"
|
#include "io/gps.h"
|
||||||
|
|
||||||
#include "fc/fc_core.h"
|
|
||||||
#include "fc/runtime_config.h"
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
|
#include "fc/fc_core.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/position.h"
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/gps_rescue.h"
|
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
#include "flight/position.h"
|
||||||
|
|
||||||
|
#include "pg/pg.h"
|
||||||
|
#include "pg/pg_ids.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
#include "sensors/acceleration.h"
|
#include "sensors/acceleration.h"
|
||||||
|
|
||||||
|
#include "gps_rescue.h"
|
||||||
|
|
||||||
|
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
|
||||||
|
|
||||||
|
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||||
|
.angle = 32,
|
||||||
|
.initialAltitude = 50,
|
||||||
|
.descentDistance = 200,
|
||||||
|
.rescueGroundspeed = 2000,
|
||||||
|
.throttleP = 150,
|
||||||
|
.throttleI = 20,
|
||||||
|
.throttleD = 50,
|
||||||
|
.velP = 80,
|
||||||
|
.velI = 20,
|
||||||
|
.velD = 15,
|
||||||
|
.yawP = 40,
|
||||||
|
.throttleMin = 1200,
|
||||||
|
.throttleMax = 1600,
|
||||||
|
.throttleHover = 1280,
|
||||||
|
.sanityChecks = 0,
|
||||||
|
.minSats = 8
|
||||||
|
);
|
||||||
|
|
||||||
int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
int32_t gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
||||||
uint16_t hoverThrottle = 0;
|
uint16_t hoverThrottle = 0;
|
||||||
float averageThrottle = 0.0;
|
float averageThrottle = 0.0;
|
||||||
|
@ -86,7 +113,7 @@ void updateGPSRescueState(void)
|
||||||
break;
|
break;
|
||||||
case RESCUE_INITIALIZE:
|
case RESCUE_INITIALIZE:
|
||||||
if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
|
if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
|
||||||
hoverThrottle = gpsRescue()->throttleHover;
|
hoverThrottle = gpsRescueConfig()->throttleHover;
|
||||||
}
|
}
|
||||||
|
|
||||||
rescueState.phase = RESCUE_ATTAIN_ALT;
|
rescueState.phase = RESCUE_ATTAIN_ALT;
|
||||||
|
@ -98,23 +125,23 @@ void updateGPSRescueState(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
rescueState.intent.targetGroundspeed = 500;
|
rescueState.intent.targetGroundspeed = 500;
|
||||||
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
|
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
|
||||||
rescueState.intent.crosstrack = true;
|
rescueState.intent.crosstrack = true;
|
||||||
rescueState.intent.minAngleDeg = 10;
|
rescueState.intent.minAngleDeg = 10;
|
||||||
rescueState.intent.maxAngleDeg = 15;
|
rescueState.intent.maxAngleDeg = 15;
|
||||||
break;
|
break;
|
||||||
case RESCUE_CROSSTRACK:
|
case RESCUE_CROSSTRACK:
|
||||||
if (rescueState.sensor.distanceToHome < gpsRescue()->descentDistance) {
|
if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) {
|
||||||
rescueState.phase = RESCUE_LANDING_APPROACH;
|
rescueState.phase = RESCUE_LANDING_APPROACH;
|
||||||
}
|
}
|
||||||
|
|
||||||
// We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
|
// We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
|
||||||
// Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT
|
// Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT
|
||||||
rescueState.intent.targetGroundspeed = gpsRescue()->rescueGroundspeed;
|
rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
|
||||||
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
|
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
|
||||||
rescueState.intent.crosstrack = true;
|
rescueState.intent.crosstrack = true;
|
||||||
rescueState.intent.minAngleDeg = 15;
|
rescueState.intent.minAngleDeg = 15;
|
||||||
rescueState.intent.maxAngleDeg = gpsRescue()->angle;
|
rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
|
||||||
break;
|
break;
|
||||||
case RESCUE_LANDING_APPROACH:
|
case RESCUE_LANDING_APPROACH:
|
||||||
// We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
|
// We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
|
||||||
|
@ -123,8 +150,8 @@ void updateGPSRescueState(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
|
// Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
|
||||||
int32_t newAlt = gpsRescue()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance;
|
int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
|
||||||
int32_t newSpeed = gpsRescue()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance;
|
int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
|
||||||
|
|
||||||
rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
|
rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
|
||||||
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
|
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
|
||||||
|
@ -204,8 +231,8 @@ void performSanityChecks()
|
||||||
|
|
||||||
// Do not abort until each of these items is fully tested
|
// Do not abort until each of these items is fully tested
|
||||||
if (rescueState.failure != RESCUE_HEALTHY) {
|
if (rescueState.failure != RESCUE_HEALTHY) {
|
||||||
if (gpsRescue()->sanityChecks == RESCUE_SANITY_ON
|
if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON
|
||||||
|| (gpsRescue()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
|
|| (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
|
||||||
rescueState.phase = RESCUE_ABORT;
|
rescueState.phase = RESCUE_ABORT;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -241,7 +268,7 @@ void performSanityChecks()
|
||||||
// Minimum sat detection
|
// Minimum sat detection
|
||||||
static int8_t msI = 0;
|
static int8_t msI = 0;
|
||||||
|
|
||||||
msI = constrain(msI + (rescueState.sensor.numSat < gpsRescue()->minSats) ? 1 : -1, -5, 5);
|
msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5);
|
||||||
|
|
||||||
if (msI == 5) {
|
if (msI == 5) {
|
||||||
rescueState.failure = RESCUE_FLYAWAY;
|
rescueState.failure = RESCUE_FLYAWAY;
|
||||||
|
@ -317,7 +344,7 @@ void rescueAttainPosition()
|
||||||
|
|
||||||
previousSpeedError = speedError;
|
previousSpeedError = speedError;
|
||||||
|
|
||||||
int16_t angleAdjustment = gpsRescue()->velP * speedError + (gpsRescue()->velI * speedIntegral) / 100 + gpsRescue()->velD * speedDerivative;
|
int16_t angleAdjustment = gpsRescueConfig()->velP * speedError + (gpsRescueConfig()->velI * speedIntegral) / 100 + gpsRescueConfig()->velD * speedDerivative;
|
||||||
|
|
||||||
gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100);
|
gpsRescueAngle[AI_PITCH] = constrain(gpsRescueAngle[AI_PITCH] + MIN(angleAdjustment, 80), rescueState.intent.minAngleDeg * 100, rescueState.intent.maxAngleDeg * 100);
|
||||||
|
|
||||||
|
@ -341,10 +368,10 @@ void rescueAttainPosition()
|
||||||
|
|
||||||
previousAltitudeError = altitudeError;
|
previousAltitudeError = altitudeError;
|
||||||
|
|
||||||
int16_t altitudeAdjustment = (gpsRescue()->throttleP * altitudeError + (gpsRescue()->throttleI * altitudeIntegral) / 10 * + gpsRescue()->throttleD * altitudeDerivative) / ct / 20;
|
int16_t altitudeAdjustment = (gpsRescueConfig()->throttleP * altitudeError + (gpsRescueConfig()->throttleI * altitudeIntegral) / 10 * + gpsRescueConfig()->throttleD * altitudeDerivative) / ct / 20;
|
||||||
int16_t hoverAdjustment = (hoverThrottle - 1000) / ct;
|
int16_t hoverAdjustment = (hoverThrottle - 1000) / ct;
|
||||||
|
|
||||||
rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescue()->throttleMin, gpsRescue()->throttleMax);
|
rescueThrottle = constrain(1000 + altitudeAdjustment + hoverAdjustment, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
||||||
|
|
||||||
DEBUG_SET(DEBUG_RTH, 0, rescueThrottle);
|
DEBUG_SET(DEBUG_RTH, 0, rescueThrottle);
|
||||||
DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]);
|
DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]);
|
||||||
|
@ -364,7 +391,7 @@ void setBearing(int16_t deg)
|
||||||
|
|
||||||
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
||||||
|
|
||||||
rcCommand[YAW] = - (dif * gpsRescue()->yawP / 20);
|
rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -16,7 +16,31 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "io/gps.h"
|
|
||||||
|
#include "pg/pg.h"
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
RESCUE_SANITY_OFF = 0,
|
||||||
|
RESCUE_SANITY_ON,
|
||||||
|
RESCUE_SANITY_FS_ONLY
|
||||||
|
} gpsRescueSanity_e;
|
||||||
|
|
||||||
|
typedef struct gpsRescue_s {
|
||||||
|
uint16_t angle; //degrees
|
||||||
|
uint16_t initialAltitude; //meters
|
||||||
|
uint16_t descentDistance; //meters
|
||||||
|
uint16_t rescueGroundspeed; // centimeters per second
|
||||||
|
uint16_t throttleP, throttleI, throttleD;
|
||||||
|
uint16_t yawP;
|
||||||
|
uint16_t throttleMin;
|
||||||
|
uint16_t throttleMax;
|
||||||
|
uint16_t throttleHover;
|
||||||
|
uint16_t velP, velI, velD;
|
||||||
|
uint8_t minSats;
|
||||||
|
gpsRescueSanity_e sanityChecks;
|
||||||
|
} gpsRescueConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
||||||
|
|
||||||
uint16_t rescueThrottle;
|
uint16_t rescueThrottle;
|
||||||
|
|
||||||
|
|
|
@ -92,11 +92,11 @@ extern uint8_t __config_end;
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/position.h"
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
#include "flight/position.h"
|
||||||
#include "flight/servos.h"
|
#include "flight/servos.h"
|
||||||
|
|
||||||
#include "interface/cli.h"
|
#include "interface/cli.h"
|
||||||
|
|
|
@ -45,11 +45,12 @@
|
||||||
#include "fc/rc_adjustments.h"
|
#include "fc/rc_adjustments.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
|
|
||||||
#include "flight/position.h"
|
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
|
#include "flight/gps_rescue.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
#include "flight/position.h"
|
||||||
#include "flight/servos.h"
|
#include "flight/servos.h"
|
||||||
|
|
||||||
#include "interface/settings.h"
|
#include "interface/settings.h"
|
||||||
|
@ -695,23 +696,23 @@ const clivalue_t valueTable[] = {
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
#ifdef USE_GPS_RESCUE
|
||||||
// PG_GPS_RESCUE
|
// PG_GPS_RESCUE
|
||||||
{ "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, angle) },
|
{ "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
|
||||||
{ "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, initialAltitude) },
|
{ "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitude) },
|
||||||
{ "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, descentDistance) },
|
{ "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistance) },
|
||||||
{ "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, rescueGroundspeed) },
|
{ "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
|
||||||
{ "gps_rescue_throttle_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleP) },
|
{ "gps_rescue_throttle_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
|
||||||
{ "gps_rescue_throttle_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleI) },
|
{ "gps_rescue_throttle_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
|
||||||
{ "gps_rescue_throttle_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleD) },
|
{ "gps_rescue_throttle_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleD) },
|
||||||
{ "gps_rescue_velocity_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velP) },
|
{ "gps_rescue_velocity_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velP) },
|
||||||
{ "gps_rescue_velocity_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velI) },
|
{ "gps_rescue_velocity_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velI) },
|
||||||
{ "gps_rescue_velocity_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velD) },
|
{ "gps_rescue_velocity_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, velD) },
|
||||||
{ "gps_rescue_yaw_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, yawP) },
|
{ "gps_rescue_yaw_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, yawP) },
|
||||||
|
|
||||||
{ "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleMin) },
|
{ "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
|
||||||
{ "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleMax) },
|
{ "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
|
||||||
{ "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleHover) },
|
{ "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
|
||||||
{ "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescue_t, sanityChecks) },
|
{ "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
|
||||||
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, minSats) },
|
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -220,10 +220,6 @@ gpsData_t gpsData;
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescue_t, gpsRescue, PG_GPS_RESCUE, 0);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
||||||
.provider = GPS_NMEA,
|
.provider = GPS_NMEA,
|
||||||
.sbasMode = SBAS_AUTO,
|
.sbasMode = SBAS_AUTO,
|
||||||
|
@ -231,27 +227,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
|
||||||
.autoBaud = GPS_AUTOBAUD_OFF
|
.autoBaud = GPS_AUTOBAUD_OFF
|
||||||
);
|
);
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
|
||||||
PG_RESET_TEMPLATE(gpsRescue_t, gpsRescue,
|
|
||||||
.angle = 32,
|
|
||||||
.initialAltitude = 50,
|
|
||||||
.descentDistance = 200,
|
|
||||||
.rescueGroundspeed = 2000,
|
|
||||||
.throttleP = 150,
|
|
||||||
.throttleI = 20,
|
|
||||||
.throttleD = 50,
|
|
||||||
.velP = 80,
|
|
||||||
.velI = 20,
|
|
||||||
.velD = 15,
|
|
||||||
.yawP = 40,
|
|
||||||
.throttleMin = 1200,
|
|
||||||
.throttleMax = 1600,
|
|
||||||
.throttleHover = 1280,
|
|
||||||
.sanityChecks = 0,
|
|
||||||
.minSats = 8
|
|
||||||
);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static void shiftPacketLog(void)
|
static void shiftPacketLog(void)
|
||||||
{
|
{
|
||||||
uint32_t i;
|
uint32_t i;
|
||||||
|
|
|
@ -65,12 +65,6 @@ typedef enum {
|
||||||
GPS_AUTOBAUD_ON
|
GPS_AUTOBAUD_ON
|
||||||
} gpsAutoBaud_e;
|
} gpsAutoBaud_e;
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
RESCUE_SANITY_OFF = 0,
|
|
||||||
RESCUE_SANITY_ON,
|
|
||||||
RESCUE_SANITY_FS_ONLY
|
|
||||||
} gpsRescueSanity_e;
|
|
||||||
|
|
||||||
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
|
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
|
||||||
|
|
||||||
typedef struct gpsConfig_s {
|
typedef struct gpsConfig_s {
|
||||||
|
@ -82,27 +76,6 @@ typedef struct gpsConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(gpsConfig_t, gpsConfig);
|
PG_DECLARE(gpsConfig_t, gpsConfig);
|
||||||
|
|
||||||
#ifdef USE_GPS_RESCUE
|
|
||||||
|
|
||||||
typedef struct gpsRescue_s {
|
|
||||||
uint16_t angle; //degrees
|
|
||||||
uint16_t initialAltitude; //meters
|
|
||||||
uint16_t descentDistance; //meters
|
|
||||||
uint16_t rescueGroundspeed; // centimeters per second
|
|
||||||
uint16_t throttleP, throttleI, throttleD;
|
|
||||||
uint16_t yawP;
|
|
||||||
uint16_t throttleMin;
|
|
||||||
uint16_t throttleMax;
|
|
||||||
uint16_t throttleHover;
|
|
||||||
uint16_t velP, velI, velD;
|
|
||||||
uint8_t minSats;
|
|
||||||
gpsRescueSanity_e sanityChecks;
|
|
||||||
} gpsRescue_t;
|
|
||||||
|
|
||||||
PG_DECLARE(gpsRescue_t, gpsRescue);
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
typedef struct gpsCoordinateDDDMMmmmm_s {
|
typedef struct gpsCoordinateDDDMMmmmm_s {
|
||||||
int16_t dddmm;
|
int16_t dddmm;
|
||||||
int16_t mmmm;
|
int16_t mmmm;
|
||||||
|
|
Loading…
Reference in New Issue