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:
parent
bf7cff3c4c
commit
8641fcd291
4 changed files with 16 additions and 13 deletions
|
@ -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) {
|
||||||
|
|
|
@ -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)) {
|
if (posHold.posHoldIsOK) {
|
||||||
posHoldUpdateTargetLocation();
|
posHoldUpdateTargetLocation();
|
||||||
if (posHold.posHoldIsOK) {
|
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;
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue