Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

allow greater overshoot at the start for high incoming speed

Browse source 
rename justStarted to isDeceleratingAtStart
This commit is contained in:
ctzsnooze 2024-10-20 13:39:17 +11:00
parent a56c3e7b29
commit 376e5201df
1 changed files with 13 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

21
src/main/flight/autopilot.c
View file

@ -60,7 +60,8 @@ typedef struct {
float previousHeading; float previousHeading;
float pitchI; float pitchI;
float rollI; float rollI;
bool justStarted; bool isDeceleratingAtStart;
float sanityCheckDistance;
} posHoldState; } posHoldState;
static posHoldState posHold = { static posHoldState posHold = {
@ -73,7 +74,8 @@ static posHoldState posHold = {
.previousHeading = 0.0f, .previousHeading = 0.0f,
.pitchI = 0.0f, .pitchI = 0.0f,
.rollI = 0.0f, .rollI = 0.0f,
.justStarted = true, .isDeceleratingAtStart = true,
.sanityCheckDistance = 1000.0f,
}; };
static gpsLocation_t currentTargetLocation = {0, 0, 0}; static gpsLocation_t currentTargetLocation = {0, 0, 0};
@ -155,11 +157,12 @@ void resetPositionControlParams(void) {
posHold.previousHeading = attitude.values.yaw * 0.1f; posHold.previousHeading = attitude.values.yaw * 0.1f;
posHold.pitchI = 0.0f; posHold.pitchI = 0.0f;
posHold.rollI = 0.0f; posHold.rollI = 0.0f;
posHold.sanityCheckDistance = 1000.0f;
} }
void resetPositionControl(gpsLocation_t initialTargetLocation) { void resetPositionControl(gpsLocation_t initialTargetLocation) {
currentTargetLocation = initialTargetLocation; currentTargetLocation = initialTargetLocation;
posHold.justStarted = true; posHold.isDeceleratingAtStart = true;
resetPositionControlParams(); resetPositionControlParams();
} }
@ -192,13 +195,15 @@ bool positionControl(bool useStickAdjustment, float deadband) {
// once it has 'stopped' the PIDs will push back towards home, and the distance away will decrease // once it has 'stopped' the PIDs will push back towards home, and the distance away will decrease
// it looks a lot better if we reset the target point to the point that we 'pull up' at // it looks a lot better if we reset the target point to the point that we 'pull up' at
// otherwise there is a big distance to pull back if we start pos hold while carrying some speed // otherwise there is a big distance to pull back if we start pos hold while carrying some speed
if (posHold.justStarted) { if (posHold.isDeceleratingAtStart) {
positionLpfCutoffHz = autopilotConfig()->position_cutoff * 0.003f; // acquire D and A more gradually positionLpfCutoffHz = autopilotConfig()->position_cutoff * 0.003f; // acquire D and A more gradually
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
if (posHold.distanceCm < posHold.previousDistanceCm) { if (posHold.distanceCm < posHold.previousDistanceCm) {
// reset the values now the craft has 'stopped'; this happens only once
currentTargetLocation = gpsSol.llh; currentTargetLocation = gpsSol.llh;
positionLpfCutoffHz = autopilotConfig()->position_cutoff * 0.01f; // normal D and A responsiveness positionLpfCutoffHz = autopilotConfig()->position_cutoff * 0.01f; // normal D and A responsiveness
resetPositionControlParams(); resetPositionControlParams();
posHold.justStarted = false; posHold.isDeceleratingAtStart = false;
} else { } else {
posHold.previousDistanceCm = posHold.distanceCm; posHold.previousDistanceCm = posHold.distanceCm;
} }
@ -208,13 +213,13 @@ bool positionControl(bool useStickAdjustment, float deadband) {
// TO DO - maybe use fixed at GPS data rate? // TO DO - maybe use fixed at GPS data rate?
const float gain = pt1FilterGain(positionLpfCutoffHz, gpsDataIntervalS); const float gain = pt1FilterGain(positionLpfCutoffHz, gpsDataIntervalS);
const uint8_t startLogger = posHold.justStarted ? 2 : 1; const uint8_t startLogger = posHold.isDeceleratingAtStart ? 2 : 1;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger);
// ** simple (too simple) sanity check ** // ** simple (too simple) sanity check **
// primarily to detect flyaway from no Mag or badly oriented Mag // primarily to detect flyaway from no Mag or badly oriented Mag
// TODO - maybe figure how to make a better check by giving more leeway at the start? // but must accept some overshoot at the start, especially if entering at high speed
if (posHold.distanceCm > 1000) { if (posHold.distanceCm > posHold.sanityCheckDistance) {
return false; // must stay within 10m or probably flying away return false; // must stay within 10m or probably flying away
// value at this point is a 'best guess' to detect IMU failure in the event the user has no Mag // value at this point is a 'best guess' to detect IMU failure in the event the user has no Mag
// if entering poshold from a stable hover, we would only exceed this if IMU was disoriented // if entering poshold from a stable hover, we would only exceed this if IMU was disoriented