1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00

fix failure to initiate position hold from error in ifdef

This commit is contained in:
ctzsnooze 2024-10-19 10:45:24 +11:00
parent bf7cff3c4c
commit 8641fcd291
4 changed files with 16 additions and 13 deletions

View file

@ -201,10 +201,11 @@ bool positionControl(bool useStickAdjustment, float deadband) {
posHold.previousDistanceCm = posHold.distanceCm; posHold.previousDistanceCm = posHold.distanceCm;
} }
} }
const uint8_t startLogger = posHold.justStarted ? 2 : 1; const uint8_t startLogger = posHold.justStarted ? 2 : 1;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger);
// simple (very 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? // TODO - maybe figure how to make a better check by giving more leeway at the start?
if (posHold.distanceCm > 1000) { if (posHold.distanceCm > 1000) {

View file

@ -37,7 +37,7 @@ static posHoldState_t posHold;
void posHoldReset(void) void posHoldReset(void)
{ {
posHold.posHoldIsOK = true; posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
posHold.targetLocation = gpsSol.llh; posHold.targetLocation = gpsSol.llh;
resetPositionControl(posHold.targetLocation); resetPositionControl(posHold.targetLocation);
} }
@ -46,6 +46,7 @@ void posHoldInit(void)
{ {
posHold.deadband = rcControlsConfig()->pos_hold_deadband / 100.0f; posHold.deadband = rcControlsConfig()->pos_hold_deadband / 100.0f;
posHold.useStickAdjustment = rcControlsConfig()->pos_hold_deadband; posHold.useStickAdjustment = rcControlsConfig()->pos_hold_deadband;
posHold.posHoldIsOK = false;
} }
void posHoldStart(void) { void posHoldStart(void) {
@ -56,6 +57,7 @@ void posHoldStart(void) {
wasInPosHoldMode = true; wasInPosHoldMode = true;
} }
} else { } else {
posHold.posHoldIsOK = false;
wasInPosHoldMode = false; wasInPosHoldMode = false;
} }
} }
@ -77,11 +79,9 @@ void posHoldUpdateTargetLocation(void)
} }
void posHoldNewGpsData(void) { void posHoldNewGpsData(void) {
if (FLIGHT_MODE(POS_HOLD_MODE)) {
posHoldUpdateTargetLocation();
if (posHold.posHoldIsOK) { if (posHold.posHoldIsOK) {
posHoldUpdateTargetLocation();
posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband); posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband);
}
} else { } else {
autopilotAngle[AI_PITCH] = 0.0f; autopilotAngle[AI_PITCH] = 0.0f;
autopilotAngle[AI_ROLL] = 0.0f; autopilotAngle[AI_ROLL] = 0.0f;

View file

@ -33,6 +33,7 @@ typedef struct {
} posHoldState_t; } posHoldState_t;
void posHoldInit(void); void posHoldInit(void);
void posHoldNewGpsData(void);
void updatePosHold(timeUs_t currentTimeUs); void updatePosHold(timeUs_t currentTimeUs);
bool posHoldFailure(void); bool posHoldFailure(void);

View file

@ -56,6 +56,7 @@
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/pos_hold.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
@ -2592,13 +2593,17 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void) void onGpsNewData(void)
{ {
gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
#ifdef USE_POS_HOLD_MODE
posHoldNewGpsData(); // pos hold code itself checks for GPS fix and shows OSD warning if not available
#endif
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {
// if we don't have a 3D fix don't give data to GPS rescue // if we don't have a 3D fix don't give data to GPS rescue
return; return;
} }
gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
GPS_calculateDistanceAndDirectionToHome(); GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
GPS_calculateDistanceFlown(false); GPS_calculateDistanceFlown(false);
@ -2611,10 +2616,6 @@ void onGpsNewData(void)
#ifdef USE_GPS_LAP_TIMER #ifdef USE_GPS_LAP_TIMER
gpsLapTimerNewGpsData(); gpsLapTimerNewGpsData();
#endif // USE_GPS_LAP_TIMER #endif // USE_GPS_LAP_TIMER
#ifdef USE_POS_HOLD
posHoldNewGpsData();
#endif
} }
void gpsSetFixState(bool state) void gpsSetFixState(bool state)