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

Merge pull request #6132 from mikeller/fix_pid_loop_slowness 

Remove GPS rescue update from PID loop.
This commit is contained in:
Michael Keller 2018-06-17 13:35:39 +12:00 committed by GitHub
parent 9c55617f05 f129f7913c
commit 0dd43c92dc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 103 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -548,7 +548,7 @@ uint8_t calculateThrottlePercent(void)
static bool airmodeIsActivated; static bool airmodeIsActivated;
bool isAirmodeActivated() bool isAirmodeActivated()
{ {
return airmodeIsActivated; return airmodeIsActivated;
} }
@ -808,7 +808,7 @@ bool processRx(timeUs_t currentTimeUs)
} }
} }
#endif #endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE); ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else { } else {
@ -923,10 +923,6 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
} }
#endif #endif
#ifdef USE_GPS_RESCUE
updateGPSRescueState();
#endif
#ifdef USE_SDCARD #ifdef USE_SDCARD
afatfs_poll(); afatfs_poll();
#endif #endif
@ -977,6 +973,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
{ {
UNUSED(currentTimeUs);
// If we're armed, at minimum throttle, and we do arming via the // If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the // sticks, do not process yaw input from the rx. We do this so the
@ -999,7 +996,12 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
} }
processRcCommand(); processRcCommand();
UNUSED(currentTimeUs);
#if defined(USE_GPS_RESCUE)
if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
gpsRescueInjectRcCommands();
}
#endif
} }
// Function for loop trigger // Function for loop trigger

5
src/main/fc/fc_rc.c
View File

@ -43,7 +43,6 @@
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/gps_rescue.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "pg/rx.h" #include "pg/rx.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -528,10 +527,6 @@ FAST_CODE FAST_CODE_NOINLINE void updateRcCommands(void)
rcCommand[YAW] = rcCommandBuff.Z; rcCommand[YAW] = rcCommandBuff.Z;
} }
} }
if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
rcCommand[THROTTLE] = rescueThrottle;
}
} }
void resetYawAxis(void) void resetYawAxis(void)

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

@ -71,6 +71,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minSats = 8 .minSats = 8
); );
static uint16_t rescueThrottle;
static uint16_t rescueYaw;
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;
@ -109,87 +112,89 @@ void updateGPSRescueState(void)
switch (rescueState.phase) { switch (rescueState.phase) {
case RESCUE_IDLE: case RESCUE_IDLE:
idleTasks(); idleTasks();
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 = gpsRescueConfig()->throttleHover; hoverThrottle = gpsRescueConfig()->throttleHover;
} }
rescueState.phase = RESCUE_ATTAIN_ALT; rescueState.phase = RESCUE_ATTAIN_ALT;
FALLTHROUGH; FALLTHROUGH;
case RESCUE_ATTAIN_ALT: case RESCUE_ATTAIN_ALT:
// Get to a safe altitude at a low velocity ASAP // Get to a safe altitude at a low velocity ASAP
if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) { if (ABS(rescueState.intent.targetAltitude - rescueState.sensor.currentAltitude) < 1000) {
rescueState.phase = RESCUE_CROSSTRACK; rescueState.phase = RESCUE_CROSSTRACK;
} }
rescueState.intent.targetGroundspeed = 500; rescueState.intent.targetGroundspeed = 500;
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->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 < gpsRescueConfig()->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 = gpsRescueConfig()->rescueGroundspeed; rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
rescueState.intent.targetAltitude = MAX(gpsRescueConfig()->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 = gpsRescueConfig()->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
if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) { if (rescueState.sensor.distanceToHome < 10 && rescueState.sensor.currentAltitude <= 1000) {
rescueState.phase = RESCUE_LANDING; 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) // 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 newAlt = gpsRescueConfig()->initialAltitude * 100 * rescueState.sensor.distanceToHome / gpsRescueConfig()->descentDistance;
int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * 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.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);
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10; rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20; rescueState.intent.maxAngleDeg = 20;
break; break;
case RESCUE_LANDING: 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. // 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 // 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 we are over 120% of average magnitude, just disarm since we're pretty much home
if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) { if (rescueState.sensor.accMagnitude > rescueState.sensor.accMagnitudeAvg * 1.5) {
disarm(); disarm();
rescueState.phase = RESCUE_COMPLETE; rescueState.phase = RESCUE_COMPLETE;
} }
rescueState.intent.targetGroundspeed = 0; rescueState.intent.targetGroundspeed = 0;
rescueState.intent.targetAltitude = 0; rescueState.intent.targetAltitude = 0;
rescueState.intent.crosstrack = true; rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 0; rescueState.intent.minAngleDeg = 0;
rescueState.intent.maxAngleDeg = 15; rescueState.intent.maxAngleDeg = 15;
break; break;
case RESCUE_COMPLETE: case RESCUE_COMPLETE:
rescueStop(); rescueStop();
break; break;
case RESCUE_ABORT: case RESCUE_ABORT:
disarm(); disarm();
rescueStop(); rescueStop();
break; break;
default:
break;
} }
performSanityChecks(); performSanityChecks();
if (rescueState.phase != RESCUE_IDLE) { if (rescueState.phase != RESCUE_IDLE) {
rescueAttainPosition(); rescueAttainPosition();
} }
newGPSData = false; newGPSData = false;
} }
@ -225,7 +230,7 @@ void performSanityChecks()
{ {
if (rescueState.phase == RESCUE_IDLE) { if (rescueState.phase == RESCUE_IDLE) {
rescueState.failure = RESCUE_HEALTHY; rescueState.failure = RESCUE_HEALTHY;
return; return;
} }
@ -335,7 +340,7 @@ void rescueAttainPosition()
return; return;
} }
/** /**
Speed controller Speed controller
*/ */
@ -396,7 +401,13 @@ void setBearing(int16_t deg)
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
rcCommand[YAW] = - (dif * gpsRescueConfig()->yawP / 20); rescueYaw = - (dif * gpsRescueConfig()->yawP / 20);
}
void gpsRescueInjectRcCommands(void)
{
rcCommand[THROTTLE] = rescueThrottle;
rcCommand[YAW] = rescueYaw;
} }
#endif #endif

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

@ -42,8 +42,6 @@ typedef struct gpsRescue_s {
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
uint16_t rescueThrottle;
typedef enum { typedef enum {
RESCUE_IDLE, RESCUE_IDLE,
RESCUE_INITIALIZE, RESCUE_INITIALIZE,
@ -110,3 +108,5 @@ void performSanityChecks(void);
void sensorUpdate(void); void sensorUpdate(void);
void rescueAttainPosition(void); void rescueAttainPosition(void);
void gpsRescueInjectRcCommands(void);

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

@ -394,11 +394,11 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT) if ((fabsf(gyroAverage[X]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Y]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT) || (fabsf(gyroAverage[Z]) > ATTITUDE_RESET_GYRO_LIMIT)
|| (!useAcc)) { || (!useAcc)) {
gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME; gyroQuietPeriodTimeEnd = currentTimeUs + ATTITUDE_RESET_QUIET_TIME;
attitudeResetTimeEnd = 0; attitudeResetTimeEnd = 0;
} }
} }
if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation if (attitudeResetTimeEnd > 0) { // Resetting the attitude estimation
if (currentTimeUs >= attitudeResetTimeEnd) { if (currentTimeUs >= attitudeResetTimeEnd) {
@ -408,14 +408,14 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
} else { } else {
attitudeResetActive = true; 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 // Start the high gain period to bring the estimation into convergence
attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME; attitudeResetTimeEnd = currentTimeUs + ATTITUDE_RESET_ACTIVE_TIME;
gyroQuietPeriodTimeEnd = 0; gyroQuietPeriodTimeEnd = 0;
} }
} }
lastArmState = armState; lastArmState = armState;
if (attitudeResetActive) { if (attitudeResetActive) {
ret = ATTITUDE_RESET_KP_GAIN; ret = ATTITUDE_RESET_KP_GAIN;
} else { } else {
@ -451,7 +451,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true; useCOG = true;
} else { } 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 // probably stop re calculating GPS heading data. Other future modes can also use this extern
if (canUseGPSHeading) { if (canUseGPSHeading) {
@ -532,7 +532,7 @@ bool shouldInitializeGPSHeading()
return true; return true;
} }
return false; return false;
} }

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

@ -531,6 +531,9 @@ void gpsUpdate(timeUs_t currentTimeUs)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTimeUs); updateGpsIndicator(currentTimeUs);
} }
#if defined(USE_GPS_RESCUE)
updateGPSRescueState();
#endif
} }
static void gpsNewData(uint16_t c) static void gpsNewData(uint16_t c)