rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Renamed 'gpsRescue' to 'gpsRescueConfig' and moved it into the appropriate location.

This commit is contained in:
mikeller 2018-05-26 15:28:08 +12:00
parent 98fb4d7c66
commit d535fd6180
7 changed files with 97 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/fc/fc_core.c
View File

@ -257,7 +257,7 @@ void updateArmingStatus(void)
#ifdef USE_GPS_RESCUE
if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
if (rescueState.sensor.numSat < gpsRescue()->minSats) {
if (rescueState.sensor.numSat < gpsRescueConfig()->minSats) {
setArmingDisabled(ARMING_DISABLED_GPS);
} else {
unsetArmingDisabled(ARMING_DISABLED_GPS);

77
src/main/flight/gps_rescue.c
View File

@ -18,32 +18,59 @@
#include <stdint.h>
#include <math.h>
#include "common/axis.h"
#include "common/maths.h"
#include "build/debug.h"
#include "drivers/time.h"
#include "platform.h"
#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 "fc/fc_core.h"
#include "fc/runtime_config.h"
#include "fc/config.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/position.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "gps_rescue.h"
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 };
uint16_t hoverThrottle = 0;
float averageThrottle = 0.0;
@ -86,7 +113,7 @@ void updateGPSRescueState(void)
break;
case RESCUE_INITIALIZE:
if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
hoverThrottle = gpsRescue()->throttleHover;
hoverThrottle = gpsRescueConfig()->throttleHover;
}
rescueState.phase = RESCUE_ATTAIN_ALT;
@ -98,23 +125,23 @@ void updateGPSRescueState(void)
}
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.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_CROSSTRACK:
if (rescueState.sensor.distanceToHome < gpsRescue()->descentDistance) {
if (rescueState.sensor.distanceToHome < gpsRescueConfig()->descentDistance) {
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
// Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT
rescueState.intent.targetGroundspeed = gpsRescue()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescue()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 15;
rescueState.intent.maxAngleDeg = gpsRescue()->angle;
rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
break;
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
@ -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)
int32_t newAlt = gpsRescue()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance;
int32_t newSpeed = gpsRescue()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescue()->descentDistance;
int32_t newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
rescueState.intent.targetAltitude = constrain(newAlt, 100, rescueState.intent.targetAltitude);
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
if (rescueState.failure != RESCUE_HEALTHY) {
if (gpsRescue()->sanityChecks == RESCUE_SANITY_ON
|| (gpsRescue()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON
|| (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
rescueState.phase = RESCUE_ABORT;
}
}
@ -241,7 +268,7 @@ void performSanityChecks()
// Minimum sat detection
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) {
rescueState.failure = RESCUE_FLYAWAY;
@ -317,7 +344,7 @@ void rescueAttainPosition()
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);
@ -341,10 +368,10 @@ void rescueAttainPosition()
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;
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, 1, gpsRescueAngle[AI_PITCH]);
@ -364,7 +391,7 @@ void setBearing(int16_t deg)
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
rcCommand[YAW] = - (dif * gpsRescue()->yawP / 20);
rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20);
}
#endif

26
src/main/flight/gps_rescue.h
View File

@ -16,7 +16,31 @@
*/
#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;

2
src/main/interface/cli.c
View File

@ -92,11 +92,11 @@ extern uint8_t __config_end;
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/position.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "flight/servos.h"
#include "interface/cli.h"

35
src/main/interface/settings.c
View File

@ -45,11 +45,12 @@
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "flight/position.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "flight/servos.h"
#include "interface/settings.h"
@ -695,23 +696,23 @@ const clivalue_t valueTable[] = {
#ifdef USE_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_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, initialAltitude) },
{ "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, descentDistance) },
{ "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, rescueGroundspeed) },
{ "gps_rescue_throttle_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleP) },
{ "gps_rescue_throttle_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleI) },
{ "gps_rescue_throttle_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleD) },
{ "gps_rescue_velocity_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velP) },
{ "gps_rescue_velocity_I", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velI) },
{ "gps_rescue_velocity_D", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, velD) },
{ "gps_rescue_yaw_P", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, yawP) },
{ "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(gpsRescueConfig_t, initialAltitude) },
{ "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(gpsRescueConfig_t, rescueGroundspeed) },
{ "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(gpsRescueConfig_t, throttleI) },
{ "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(gpsRescueConfig_t, velP) },
{ "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(gpsRescueConfig_t, velD) },
{ "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_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, throttleMax) },
{ "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescue_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_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescue_t, minSats) },
{ "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(gpsRescueConfig_t, throttleMax) },
{ "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(gpsRescueConfig_t, sanityChecks) },
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
#endif
#endif

25
src/main/io/gps.c
View File

@ -220,10 +220,6 @@ gpsData_t gpsData;
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,
.provider = GPS_NMEA,
.sbasMode = SBAS_AUTO,
@ -231,27 +227,6 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.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)
{
uint32_t i;

27
src/main/io/gps.h
View File

@ -65,12 +65,6 @@ typedef enum {
GPS_AUTOBAUD_ON
} gpsAutoBaud_e;
typedef enum {
RESCUE_SANITY_OFF = 0,
RESCUE_SANITY_ON,
RESCUE_SANITY_FS_ONLY
} gpsRescueSanity_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s {
@ -82,27 +76,6 @@ typedef struct gpsConfig_s {
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 {
int16_t dddmm;
int16_t mmmm;