mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-21 07:15:18 +03:00
Merge pull request #6622 from mikeller/fix_gps_rescue_interface
Cleaned up the GPS rescue interface.
This commit is contained in:
Michael Keller
GitHub
commit
adc965327f
2 changed files with 275 additions and 286 deletions
|
|
@ -51,10 +51,69 @@
|
||||||
|
|
||||||
#include "gps_rescue.h"
|
#include "gps_rescue.h"
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
RESCUE_SANITY_OFF = 0,
|
||||||
|
RESCUE_SANITY_ON,
|
||||||
|
RESCUE_SANITY_FS_ONLY
|
||||||
|
} gpsRescueSanity_e;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
RESCUE_IDLE,
|
||||||
|
RESCUE_INITIALIZE,
|
||||||
|
RESCUE_ATTAIN_ALT,
|
||||||
|
RESCUE_CROSSTRACK,
|
||||||
|
RESCUE_LANDING_APPROACH,
|
||||||
|
RESCUE_LANDING,
|
||||||
|
RESCUE_ABORT,
|
||||||
|
RESCUE_COMPLETE
|
||||||
|
} rescuePhase_e;
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
RESCUE_HEALTHY,
|
||||||
|
RESCUE_FLYAWAY,
|
||||||
|
RESCUE_CRASH_DETECTED,
|
||||||
|
RESCUE_TOO_CLOSE
|
||||||
|
} rescueFailureState_e;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int32_t targetAltitudeCm;
|
||||||
|
int32_t targetGroundspeed;
|
||||||
|
uint8_t minAngleDeg;
|
||||||
|
uint8_t maxAngleDeg;
|
||||||
|
bool crosstrack;
|
||||||
|
} rescueIntent_s;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int32_t maxAltitudeCm;
|
||||||
|
int32_t currentAltitudeCm;
|
||||||
|
uint16_t distanceToHomeM;
|
||||||
|
uint16_t maxDistanceToHomeM;
|
||||||
|
int16_t directionToHome;
|
||||||
|
uint16_t groundSpeed;
|
||||||
|
uint8_t numSat;
|
||||||
|
float zVelocity; // Up/down movement in cm/s
|
||||||
|
float zVelocityAvg; // Up/down average in cm/s
|
||||||
|
float accMagnitude;
|
||||||
|
float accMagnitudeAvg;
|
||||||
|
} rescueSensorData_s;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
bool bumpDetection;
|
||||||
|
bool convergenceDetection;
|
||||||
|
} rescueSanityFlags;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
rescuePhase_e phase;
|
||||||
|
rescueFailureState_e failure;
|
||||||
|
rescueSensorData_s sensor;
|
||||||
|
rescueIntent_s intent;
|
||||||
|
bool isFailsafe;
|
||||||
|
} rescueState_s;
|
||||||
|
|
||||||
#define GPS_RESCUE_MAX_YAW_RATE 360 // deg/sec max yaw rate
|
#define GPS_RESCUE_MAX_YAW_RATE 360 // deg/sec max yaw rate
|
||||||
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
|
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||||
.angle = 32,
|
.angle = 32,
|
||||||
|
|
@ -100,6 +159,220 @@ void rescueNewGpsData(void)
|
||||||
newGPSData = true;
|
newGPSData = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void rescueStart()
|
||||||
|
{
|
||||||
|
rescueState.phase = RESCUE_INITIALIZE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void rescueStop()
|
||||||
|
{
|
||||||
|
rescueState.phase = RESCUE_IDLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Things that need to run regardless of GPS rescue mode being enabled or not
|
||||||
|
static void idleTasks()
|
||||||
|
{
|
||||||
|
// Do not calculate any of the idle task values when we are not flying
|
||||||
|
if (!ARMING_FLAG(ARMED)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet
|
||||||
|
if (!isAltitudeOffset()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
gpsRescueAngle[AI_PITCH] = 0;
|
||||||
|
gpsRescueAngle[AI_ROLL] = 0;
|
||||||
|
|
||||||
|
// Store the max altitude we see not during RTH so we know our fly-back minimum alt
|
||||||
|
rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm);
|
||||||
|
// Store the max distance to home during normal flight so we know if a flyaway is happening
|
||||||
|
rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM);
|
||||||
|
|
||||||
|
rescueThrottle = rcCommand[THROTTLE];
|
||||||
|
|
||||||
|
//to do: have a default value for hoverThrottle
|
||||||
|
|
||||||
|
// FIXME: GPS Rescue throttle handling should take into account min_check as the
|
||||||
|
// active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this
|
||||||
|
// in gpsRescueGetThrottle() but it would be better handled here.
|
||||||
|
|
||||||
|
float ct = getCosTiltAngle();
|
||||||
|
if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt
|
||||||
|
//TO DO: only sample when acceleration is low
|
||||||
|
uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct;
|
||||||
|
if (throttleSamples == 0) {
|
||||||
|
averageThrottle = adjustedThrottle;
|
||||||
|
} else {
|
||||||
|
averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1);
|
||||||
|
}
|
||||||
|
hoverThrottle = lrintf(averageThrottle);
|
||||||
|
throttleSamples++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Very similar to maghold function on betaflight/cleanflight
|
||||||
|
static void setBearing(int16_t desiredHeading)
|
||||||
|
{
|
||||||
|
float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading;
|
||||||
|
|
||||||
|
// Determine the most efficient direction to rotate
|
||||||
|
if (errorAngle <= -180) {
|
||||||
|
errorAngle += 360;
|
||||||
|
} else if (errorAngle > 180) {
|
||||||
|
errorAngle -= 360;
|
||||||
|
}
|
||||||
|
|
||||||
|
errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
||||||
|
|
||||||
|
// Calculate a desired yaw rate based on a maximum limit beyond
|
||||||
|
// an error window and then scale the requested rate down inside
|
||||||
|
// the window as error approaches 0.
|
||||||
|
rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void rescueAttainPosition()
|
||||||
|
{
|
||||||
|
// Point to home if that is in our intent
|
||||||
|
if (rescueState.intent.crosstrack) {
|
||||||
|
setBearing(rescueState.sensor.directionToHome);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!newGPSData) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
Speed controller
|
||||||
|
*/
|
||||||
|
static float previousSpeedError = 0;
|
||||||
|
static int16_t speedIntegral = 0;
|
||||||
|
|
||||||
|
const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100;
|
||||||
|
const int16_t speedDerivative = speedError - previousSpeedError;
|
||||||
|
|
||||||
|
speedIntegral = constrain(speedIntegral + speedError, -100, 100);
|
||||||
|
|
||||||
|
previousSpeedError = speedError;
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10));
|
||||||
|
|
||||||
|
/**
|
||||||
|
Altitude controller
|
||||||
|
*/
|
||||||
|
static float previousAltitudeError = 0;
|
||||||
|
static int16_t altitudeIntegral = 0;
|
||||||
|
|
||||||
|
const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters
|
||||||
|
const int16_t altitudeDerivative = altitudeError - previousAltitudeError;
|
||||||
|
|
||||||
|
// Only allow integral windup within +-15m absolute altitude error
|
||||||
|
if (ABS(altitudeError) < 25) {
|
||||||
|
altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250);
|
||||||
|
} else {
|
||||||
|
altitudeIntegral = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
previousAltitudeError = altitudeError;
|
||||||
|
|
||||||
|
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, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
||||||
|
|
||||||
|
DEBUG_SET(DEBUG_RTH, 0, rescueThrottle);
|
||||||
|
DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]);
|
||||||
|
DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment);
|
||||||
|
DEBUG_SET(DEBUG_RTH, 3, rescueState.failure);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void performSanityChecks()
|
||||||
|
{
|
||||||
|
if (rescueState.phase == RESCUE_IDLE) {
|
||||||
|
rescueState.failure = RESCUE_HEALTHY;
|
||||||
|
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Do not abort until each of these items is fully tested
|
||||||
|
if (rescueState.failure != RESCUE_HEALTHY) {
|
||||||
|
if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON
|
||||||
|
|| (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
|
||||||
|
rescueState.phase = RESCUE_ABORT;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if crash recovery mode is active, disarm if so.
|
||||||
|
if (crashRecoveryModeActive()) {
|
||||||
|
rescueState.failure = RESCUE_CRASH_DETECTED;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Things that should run at a low refresh rate (such as flyaway detection, etc)
|
||||||
|
// This runs at ~1hz
|
||||||
|
|
||||||
|
static uint32_t previousTimeUs;
|
||||||
|
|
||||||
|
const uint32_t currentTimeUs = micros();
|
||||||
|
const uint32_t dTime = currentTimeUs - previousTimeUs;
|
||||||
|
|
||||||
|
if (dTime < 1000000) { //1hz
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
previousTimeUs = currentTimeUs;
|
||||||
|
|
||||||
|
// Stalled movement detection
|
||||||
|
static int8_t gsI = 0;
|
||||||
|
|
||||||
|
gsI = constrain(gsI + (rescueState.sensor.groundSpeed < 150) ? 1 : -1, -10, 10);
|
||||||
|
|
||||||
|
if (gsI == 10) {
|
||||||
|
rescueState.failure = RESCUE_CRASH_DETECTED;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Minimum sat detection
|
||||||
|
static int8_t msI = 0;
|
||||||
|
|
||||||
|
msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5);
|
||||||
|
|
||||||
|
if (msI == 5) {
|
||||||
|
rescueState.failure = RESCUE_FLYAWAY;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void sensorUpdate()
|
||||||
|
{
|
||||||
|
rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
|
||||||
|
|
||||||
|
// Calculate altitude velocity
|
||||||
|
static uint32_t previousTimeUs;
|
||||||
|
static int32_t previousAltitudeCm;
|
||||||
|
|
||||||
|
const uint32_t currentTimeUs = micros();
|
||||||
|
const float dTime = currentTimeUs - previousTimeUs;
|
||||||
|
|
||||||
|
if (newGPSData) { // Calculate velocity at lowest common denominator
|
||||||
|
rescueState.sensor.distanceToHomeM = GPS_distanceToHome;
|
||||||
|
rescueState.sensor.directionToHome = GPS_directionToHome;
|
||||||
|
rescueState.sensor.numSat = gpsSol.numSat;
|
||||||
|
rescueState.sensor.groundSpeed = gpsSol.groundSpeed;
|
||||||
|
|
||||||
|
rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime;
|
||||||
|
rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f;
|
||||||
|
|
||||||
|
rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
|
||||||
|
rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f);
|
||||||
|
|
||||||
|
previousAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
||||||
|
previousTimeUs = currentTimeUs;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Determine what phase we are in, determine if all criteria are met to move to the next phase
|
Determine what phase we are in, determine if all criteria are met to move to the next phase
|
||||||
*/
|
*/
|
||||||
|
|
@ -218,222 +491,6 @@ void updateGPSRescueState(void)
|
||||||
newGPSData = false;
|
newGPSData = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void sensorUpdate()
|
|
||||||
{
|
|
||||||
rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
|
|
||||||
|
|
||||||
// Calculate altitude velocity
|
|
||||||
static uint32_t previousTimeUs;
|
|
||||||
static int32_t previousAltitudeCm;
|
|
||||||
|
|
||||||
const uint32_t currentTimeUs = micros();
|
|
||||||
const float dTime = currentTimeUs - previousTimeUs;
|
|
||||||
|
|
||||||
if (newGPSData) { // Calculate velocity at lowest common denominator
|
|
||||||
rescueState.sensor.distanceToHomeM = GPS_distanceToHome;
|
|
||||||
rescueState.sensor.directionToHome = GPS_directionToHome;
|
|
||||||
rescueState.sensor.numSat = gpsSol.numSat;
|
|
||||||
rescueState.sensor.groundSpeed = gpsSol.groundSpeed;
|
|
||||||
|
|
||||||
rescueState.sensor.zVelocity = (rescueState.sensor.currentAltitudeCm - previousAltitudeCm) * 1000000.0f / dTime;
|
|
||||||
rescueState.sensor.zVelocityAvg = 0.8f * rescueState.sensor.zVelocityAvg + rescueState.sensor.zVelocity * 0.2f;
|
|
||||||
|
|
||||||
rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
|
|
||||||
rescueState.sensor.accMagnitudeAvg = (rescueState.sensor.accMagnitudeAvg * 0.8f) + (rescueState.sensor.accMagnitude * 0.2f);
|
|
||||||
|
|
||||||
previousAltitudeCm = rescueState.sensor.currentAltitudeCm;
|
|
||||||
previousTimeUs = currentTimeUs;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void performSanityChecks()
|
|
||||||
{
|
|
||||||
if (rescueState.phase == RESCUE_IDLE) {
|
|
||||||
rescueState.failure = RESCUE_HEALTHY;
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Do not abort until each of these items is fully tested
|
|
||||||
if (rescueState.failure != RESCUE_HEALTHY) {
|
|
||||||
if (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_ON
|
|
||||||
|| (gpsRescueConfig()->sanityChecks == RESCUE_SANITY_FS_ONLY && rescueState.isFailsafe == true)) {
|
|
||||||
rescueState.phase = RESCUE_ABORT;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check if crash recovery mode is active, disarm if so.
|
|
||||||
if (crashRecoveryModeActive()) {
|
|
||||||
rescueState.failure = RESCUE_CRASH_DETECTED;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Things that should run at a low refresh rate (such as flyaway detection, etc)
|
|
||||||
// This runs at ~1hz
|
|
||||||
|
|
||||||
static uint32_t previousTimeUs;
|
|
||||||
|
|
||||||
const uint32_t currentTimeUs = micros();
|
|
||||||
const uint32_t dTime = currentTimeUs - previousTimeUs;
|
|
||||||
|
|
||||||
if (dTime < 1000000) { //1hz
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
previousTimeUs = currentTimeUs;
|
|
||||||
|
|
||||||
// Stalled movement detection
|
|
||||||
static int8_t gsI = 0;
|
|
||||||
|
|
||||||
gsI = constrain(gsI + (rescueState.sensor.groundSpeed < 150) ? 1 : -1, -10, 10);
|
|
||||||
|
|
||||||
if (gsI == 10) {
|
|
||||||
rescueState.failure = RESCUE_CRASH_DETECTED;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Minimum sat detection
|
|
||||||
static int8_t msI = 0;
|
|
||||||
|
|
||||||
msI = constrain(msI + (rescueState.sensor.numSat < gpsRescueConfig()->minSats) ? 1 : -1, -5, 5);
|
|
||||||
|
|
||||||
if (msI == 5) {
|
|
||||||
rescueState.failure = RESCUE_FLYAWAY;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void rescueStart()
|
|
||||||
{
|
|
||||||
rescueState.phase = RESCUE_INITIALIZE;
|
|
||||||
}
|
|
||||||
|
|
||||||
void rescueStop()
|
|
||||||
{
|
|
||||||
rescueState.phase = RESCUE_IDLE;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Things that need to run regardless of GPS rescue mode being enabled or not
|
|
||||||
void idleTasks()
|
|
||||||
{
|
|
||||||
// Do not calculate any of the idle task values when we are not flying
|
|
||||||
if (!ARMING_FLAG(ARMED)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet
|
|
||||||
if (!isAltitudeOffset()) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
gpsRescueAngle[AI_PITCH] = 0;
|
|
||||||
gpsRescueAngle[AI_ROLL] = 0;
|
|
||||||
|
|
||||||
// Store the max altitude we see not during RTH so we know our fly-back minimum alt
|
|
||||||
rescueState.sensor.maxAltitudeCm = MAX(rescueState.sensor.currentAltitudeCm, rescueState.sensor.maxAltitudeCm);
|
|
||||||
// Store the max distance to home during normal flight so we know if a flyaway is happening
|
|
||||||
rescueState.sensor.maxDistanceToHomeM = MAX(rescueState.sensor.distanceToHomeM, rescueState.sensor.maxDistanceToHomeM);
|
|
||||||
|
|
||||||
rescueThrottle = rcCommand[THROTTLE];
|
|
||||||
|
|
||||||
//to do: have a default value for hoverThrottle
|
|
||||||
|
|
||||||
// FIXME: GPS Rescue throttle handling should take into account min_check as the
|
|
||||||
// active throttle is from min_check through PWM_RANGE_MAX. Currently adjusting for this
|
|
||||||
// in gpsRescueGetThrottle() but it would be better handled here.
|
|
||||||
|
|
||||||
float ct = getCosTiltAngle();
|
|
||||||
if (ct > 0.5 && ct < 0.96 && throttleSamples < 1E6 && rescueThrottle > 1070) { //5 to 45 degrees tilt
|
|
||||||
//TO DO: only sample when acceleration is low
|
|
||||||
uint16_t adjustedThrottle = 1000 + (rescueThrottle - PWM_RANGE_MIN) * ct;
|
|
||||||
if (throttleSamples == 0) {
|
|
||||||
averageThrottle = adjustedThrottle;
|
|
||||||
} else {
|
|
||||||
averageThrottle += (adjustedThrottle - averageThrottle) / (throttleSamples + 1);
|
|
||||||
}
|
|
||||||
hoverThrottle = lrintf(averageThrottle);
|
|
||||||
throttleSamples++;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void rescueAttainPosition()
|
|
||||||
{
|
|
||||||
// Point to home if that is in our intent
|
|
||||||
if (rescueState.intent.crosstrack) {
|
|
||||||
setBearing(rescueState.sensor.directionToHome);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!newGPSData) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
Speed controller
|
|
||||||
*/
|
|
||||||
static float previousSpeedError = 0;
|
|
||||||
static int16_t speedIntegral = 0;
|
|
||||||
|
|
||||||
const int16_t speedError = (rescueState.intent.targetGroundspeed - rescueState.sensor.groundSpeed) / 100;
|
|
||||||
const int16_t speedDerivative = speedError - previousSpeedError;
|
|
||||||
|
|
||||||
speedIntegral = constrain(speedIntegral + speedError, -100, 100);
|
|
||||||
|
|
||||||
previousSpeedError = speedError;
|
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
float ct = cos(DECIDEGREES_TO_RADIANS(gpsRescueAngle[AI_PITCH] / 10));
|
|
||||||
|
|
||||||
/**
|
|
||||||
Altitude controller
|
|
||||||
*/
|
|
||||||
static float previousAltitudeError = 0;
|
|
||||||
static int16_t altitudeIntegral = 0;
|
|
||||||
|
|
||||||
const int16_t altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) / 100; // Error in meters
|
|
||||||
const int16_t altitudeDerivative = altitudeError - previousAltitudeError;
|
|
||||||
|
|
||||||
// Only allow integral windup within +-15m absolute altitude error
|
|
||||||
if (ABS(altitudeError) < 25) {
|
|
||||||
altitudeIntegral = constrain(altitudeIntegral + altitudeError, -250, 250);
|
|
||||||
} else {
|
|
||||||
altitudeIntegral = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
previousAltitudeError = altitudeError;
|
|
||||||
|
|
||||||
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, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
|
||||||
|
|
||||||
DEBUG_SET(DEBUG_RTH, 0, rescueThrottle);
|
|
||||||
DEBUG_SET(DEBUG_RTH, 1, gpsRescueAngle[AI_PITCH]);
|
|
||||||
DEBUG_SET(DEBUG_RTH, 2, altitudeAdjustment);
|
|
||||||
DEBUG_SET(DEBUG_RTH, 3, rescueState.failure);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Very similar to maghold function on betaflight/cleanflight
|
|
||||||
void setBearing(int16_t desiredHeading)
|
|
||||||
{
|
|
||||||
float errorAngle = (attitude.values.yaw / 10.0f) - desiredHeading;
|
|
||||||
|
|
||||||
// Determine the most efficient direction to rotate
|
|
||||||
if (errorAngle <= -180) {
|
|
||||||
errorAngle += 360;
|
|
||||||
} else if (errorAngle > 180) {
|
|
||||||
errorAngle -= 360;
|
|
||||||
}
|
|
||||||
|
|
||||||
errorAngle *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
|
|
||||||
|
|
||||||
// Calculate a desired yaw rate based on a maximum limit beyond
|
|
||||||
// an error window and then scale the requested rate down inside
|
|
||||||
// the window as error approaches 0.
|
|
||||||
rescueYaw = -constrainf(errorAngle / GPS_RESCUE_RATE_SCALE_DEGREES * GPS_RESCUE_MAX_YAW_RATE, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
|
|
||||||
}
|
|
||||||
|
|
||||||
float gpsRescueGetYawRate(void)
|
float gpsRescueGetYawRate(void)
|
||||||
{
|
{
|
||||||
return rescueYaw;
|
return rescueYaw;
|
||||||
|
|
|
||||||
|
|
@ -19,12 +19,6 @@
|
||||||
|
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
RESCUE_SANITY_OFF = 0,
|
|
||||||
RESCUE_SANITY_ON,
|
|
||||||
RESCUE_SANITY_FS_ONLY
|
|
||||||
} gpsRescueSanity_e;
|
|
||||||
|
|
||||||
typedef struct gpsRescue_s {
|
typedef struct gpsRescue_s {
|
||||||
uint16_t angle; //degrees
|
uint16_t angle; //degrees
|
||||||
uint16_t initialAltitudeM; //meters
|
uint16_t initialAltitudeM; //meters
|
||||||
|
|
@ -38,77 +32,15 @@ typedef struct gpsRescue_s {
|
||||||
uint16_t velP, velI, velD;
|
uint16_t velP, velI, velD;
|
||||||
uint8_t minSats;
|
uint8_t minSats;
|
||||||
uint16_t minRescueDth; //meters
|
uint16_t minRescueDth; //meters
|
||||||
gpsRescueSanity_e sanityChecks;
|
uint8_t sanityChecks;
|
||||||
} gpsRescueConfig_t;
|
} gpsRescueConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
RESCUE_IDLE,
|
|
||||||
RESCUE_INITIALIZE,
|
|
||||||
RESCUE_ATTAIN_ALT,
|
|
||||||
RESCUE_CROSSTRACK,
|
|
||||||
RESCUE_LANDING_APPROACH,
|
|
||||||
RESCUE_LANDING,
|
|
||||||
RESCUE_ABORT,
|
|
||||||
RESCUE_COMPLETE
|
|
||||||
} rescuePhase_e;
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
RESCUE_HEALTHY,
|
|
||||||
RESCUE_FLYAWAY,
|
|
||||||
RESCUE_CRASH_DETECTED,
|
|
||||||
RESCUE_TOO_CLOSE
|
|
||||||
} rescueFailureState_e;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
int32_t targetAltitudeCm;
|
|
||||||
int32_t targetGroundspeed;
|
|
||||||
uint8_t minAngleDeg;
|
|
||||||
uint8_t maxAngleDeg;
|
|
||||||
bool crosstrack;
|
|
||||||
} rescueIntent_s;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
int32_t maxAltitudeCm;
|
|
||||||
int32_t currentAltitudeCm;
|
|
||||||
uint16_t distanceToHomeM;
|
|
||||||
uint16_t maxDistanceToHomeM;
|
|
||||||
int16_t directionToHome;
|
|
||||||
uint16_t groundSpeed;
|
|
||||||
uint8_t numSat;
|
|
||||||
float zVelocity; // Up/down movement in cm/s
|
|
||||||
float zVelocityAvg; // Up/down average in cm/s
|
|
||||||
float accMagnitude;
|
|
||||||
float accMagnitudeAvg;
|
|
||||||
} rescueSensorData_s;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
bool bumpDetection;
|
|
||||||
bool convergenceDetection;
|
|
||||||
} rescueSanityFlags;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
rescuePhase_e phase;
|
|
||||||
rescueFailureState_e failure;
|
|
||||||
rescueSensorData_s sensor;
|
|
||||||
rescueIntent_s intent;
|
|
||||||
bool isFailsafe;
|
|
||||||
} rescueState_s;
|
|
||||||
|
|
||||||
extern int32_t gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
|
extern int32_t gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
|
||||||
extern rescueState_s rescueState;
|
|
||||||
|
|
||||||
void updateGPSRescueState(void);
|
void updateGPSRescueState(void);
|
||||||
void rescueNewGpsData(void);
|
void rescueNewGpsData(void);
|
||||||
void idleTasks(void);
|
|
||||||
void rescueStop(void);
|
|
||||||
void rescueStart(void);
|
|
||||||
void setBearing(int16_t deg);
|
|
||||||
void performSanityChecks(void);
|
|
||||||
void sensorUpdate(void);
|
|
||||||
|
|
||||||
void rescueAttainPosition(void);
|
|
||||||
|
|
||||||
float gpsRescueGetYawRate(void);
|
float gpsRescueGetYawRate(void);
|
||||||
float gpsRescueGetThrottle(void);
|
float gpsRescueGetThrottle(void);
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue