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

fix failure to initiate position hold from error in ifdef

Browse source
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
Download patch file Download diff file Expand all files Collapse all files

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

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

10
src/main/flight/pos_hold.c
View file

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

1
src/main/flight/pos_hold.h
View file

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

13
src/main/io/gps.c
View file

@ -56,6 +56,7 @@
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "flight/pos_hold.h"
#include "scheduler/scheduler.h"
@ -2592,13 +2593,17 @@ void GPS_calculateDistanceAndDirectionToHome(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 we don't have a 3D fix don't give data to GPS rescue
return;
}
gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) {
GPS_calculateDistanceFlown(false);
@ -2611,10 +2616,6 @@ void onGpsNewData(void)
#ifdef USE_GPS_LAP_TIMER
gpsLapTimerNewGpsData();
#endif // USE_GPS_LAP_TIMER
#ifdef USE_POS_HOLD
posHoldNewGpsData();
#endif
}
void gpsSetFixState(bool state)