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

Merge pull request #6110 from codecae/gps_rescue_update_fix 

Moved updateGPSRescueState to imuUpdateAttitude. gps_rescue.c tidy.
This commit is contained in:
Andrey Mironov 2018-06-14 01:01:33 +03:00 committed by GitHub
parent 11d8171c36 953e2ad0e9
commit 7b035ce3a8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 93 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -549,7 +549,7 @@ uint8_t calculateThrottlePercent(void)
static bool airmodeIsActivated;
bool isAirmodeActivated()
bool isAirmodeActivated()
{
return airmodeIsActivated;
}
@ -809,7 +809,7 @@ bool processRx(timeUs_t currentTimeUs)
}
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
@ -924,10 +924,6 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_GPS_RESCUE
updateGPSRescueState();
#endif
#ifdef USE_SDCARD
afatfs_poll();
#endif

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

@ -108,88 +108,90 @@ void updateGPSRescueState(void)
sensorUpdate();
switch (rescueState.phase) {
case RESCUE_IDLE:
idleTasks();
break;
case RESCUE_INITIALIZE:
if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
hoverThrottle = gpsRescueConfig()->throttleHover;
}
rescueState.phase = RESCUE_ATTAIN_ALT;
FALLTHROUGH;
case RESCUE_ATTAIN_ALT:
// Get to a safe altitude at a low velocity ASAP
if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) {
rescueState.phase = RESCUE_CROSSTRACK;
}
rescueState.intent.targetGroundspeed = 500;
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 < 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 = gpsRescueConfig()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 15;
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
if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) {
rescueState.phase = RESCUE_LANDING;
}
// 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 = 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);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20;
break;
case RESCUE_LANDING:
// We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.
// At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory
// If we are over 120% of average magnitude, just disarm since we're pretty much home
if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) {
disarm();
rescueState.phase = RESCUE_COMPLETE;
}
rescueState.intent.targetGroundspeed = 0;
rescueState.intent.targetAltitude = 0;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 0;
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_COMPLETE:
rescueStop();
break;
case RESCUE_ABORT:
disarm();
rescueStop();
break;
case RESCUE_IDLE:
idleTasks();
break;
case RESCUE_INITIALIZE:
if (hoverThrottle == 0) { //no actual throttle data yet, let's use the default.
hoverThrottle = gpsRescueConfig()->throttleHover;
}
rescueState.phase = RESCUE_ATTAIN_ALT;
FALLTHROUGH;
case RESCUE_ATTAIN_ALT:
// Get to a safe altitude at a low velocity ASAP
if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) {
rescueState.phase = RESCUE_CROSSTRACK;
}
rescueState.intent.targetGroundspeed = 500;
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 < 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 = gpsRescueConfig()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->initialAltitude * 100, rescueState.sensor.maxAltitude + 1500);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 15;
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
if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) {
rescueState.phase = RESCUE_LANDING;
}
// 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 = 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);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20;
break;
case RESCUE_LANDING:
// We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.
// At this point, do not let the target altitude go up anymore, so if we overshoot, we dont' move in a parabolic trajectory
// If we are over 120% of average magnitude, just disarm since we're pretty much home
if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) {
disarm();
rescueState.phase = RESCUE_COMPLETE;
}
rescueState.intent.targetGroundspeed = 0;
rescueState.intent.targetAltitude = 0;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 0;
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_COMPLETE:
rescueStop();
break;
case RESCUE_ABORT:
disarm();
rescueStop();
break;
default:
break;
}
performSanityChecks();
if (rescueState.phase != RESCUE_IDLE) {
rescueAttainPosition();
}
newGPSData = false;
}
@ -225,7 +227,7 @@ void performSanityChecks()
{
if (rescueState.phase == RESCUE_IDLE) {
rescueState.failure = RESCUE_HEALTHY;
return;
}
@ -335,7 +337,7 @@ void rescueAttainPosition()
return;
}
/**
Speed controller
*/

16
src/main/flight/imu.c
View File

@ -41,6 +41,7 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/gps_rescue.h"
#include "io/gps.h"
@ -394,11 +395,11 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (!useAcc)) {
|| (!useAcc)) {
gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
attitudeResetTimeEnd = 0;
}
}
}
if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation
if (currentTimeUs >= attitudeResetTimeEnd) {
@ -408,14 +409,14 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
} else {
attitudeResetActive = true;
}
} else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
} else if ((gyroQuietPeriodTimeEnd > 0) && (currentTimeUs >= gyroQuietPeriodTimeEnd)) {
// Start the high gain period to bring the estimation into convergence
attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
gyroQuietPeriodTimeEnd = 0;
}
}
lastArmState = armState;
if (attitudeResetActive) {
ret = ATTITUDE_RESET_KP_GAIN;
} else {
@ -451,7 +452,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
} else {
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// probably stop re calculating GPS heading data. Other future modes can also use this extern
if (canUseGPSHeading) {
@ -521,6 +522,9 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
acc.accADC[Y] = 0;
acc.accADC[Z] = 0;
}
#ifdef USE_GPS_RESCUE
updateGPSRescueState();
#endif
}
bool shouldInitializeGPSHeading()
@ -532,7 +536,7 @@ bool shouldInitializeGPSHeading()
return true;
}
return false;
}