basic sanity check and OSD warning

src/main/fc/core.c

@@ -1064,11 +1064,14 @@ void processRxModes(timeUs_t currentTimeUs)
         // and we have Acc for self-levelling
         && sensors(SENSOR_ACC)
         // and we have altitude data
+        // TO DO: handle No Mag data depending on user choice to proceed with or without mag
         && isAltitudeAvailable()
         // prevent activation until after takeoff
         && wasThrottleRaised()) {
         if (!FLIGHT_MODE(POS_HOLD_MODE)) {
             ENABLE_FLIGHT_MODE(POS_HOLD_MODE);
+            // this is just a 'request to initiate' the function
+            // if pos hold cannot function or fails after making this request, it should throw a warning and stop
         }
     } else {
         DISABLE_FLIGHT_MODE(POS_HOLD_MODE);

src/main/fc/runtime_config.c

@@ -59,7 +59,10 @@ const char *armingDisableFlagNames[]= {
     "DSHOT_BBANG",
     "NO_ACC_CAL",
     "MOTOR_PROTO",
-    "ARMSWITCH",
+    "FLIP_SWITCH",
+    "ALT_HOLD_SW",
+    "POS_HOLD_SW",
+    "ARM_SWITCH",
 };
 
 static armingDisableFlags_e armingDisableFlags = 0;

src/main/flight/autopilot.c

@@ -25,6 +25,7 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "fc/rc.h"
+#include "fc/runtime_config.h"
 
 #include "flight/imu.h"
 #include "flight/position.h"
@@ -140,10 +141,15 @@ void resetPositionControl (void) {
     pitchI = 0.0f;
     rollI = 0.0f;
 }
-void positionControl(gpsLocation_t targetLocation, float deadband) {
+bool positionControl(gpsLocation_t targetLocation, float deadband) {
     // gpsSol.llh = current gps location
     // get distance and bearing from current location to target location
     // void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing)
+
+    if (!STATE(GPS_FIX)) {
+        return false; // cannot proceed without a GPS location
+    }
+
     uint32_t distanceCm;
     int32_t bearing; // degrees * 100
     static float previousHeading = 0.0f;
@@ -181,6 +187,7 @@ void positionControl(gpsLocation_t targetLocation, float deadband) {
     // each axis separately, so that when overshooting, the filter lag doesn't cause problems
     const float distanceRoll = rollProportion * distanceCm;
     // positive distances mean nose towards target, should roll forward (positive roll)
+
     // we need separate velocity for roll so the filter lag isn't problematic
     float velocityRoll = (distanceRoll - previousDistanceRoll) * gpsDataIntervalHz;
     previousDistanceRoll = distanceRoll;
@@ -200,6 +207,7 @@ void positionControl(gpsLocation_t targetLocation, float deadband) {
 
     const float distancePitch = pitchProportion * distanceCm;
     // positive distances mean nose towards target, should pitch forward (positive pitch)
+
     float velocityPitch = (distancePitch - previousDistancePitch) * gpsDataIntervalHz;
     previousDistancePitch = distancePitch;
     // lowpass filter the velocity
@@ -212,17 +220,24 @@ void positionControl(gpsLocation_t targetLocation, float deadband) {
     pt1FilterUpdateCutoff(&accelerationPitchLpf, gain);
     accelerationPitch = pt1FilterApply(&accelerationPitchLpf, accelerationPitch);
 
+    // simple (very simple) sanity check, mostly to detect flyaway from no Mag or badly oriented Mag
+    if (distanceCm > 1000) {
+        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
+        // if entering poshold from a stable hover, we would only exceed this if IMU was disoriented
+        // if entering poshold at speed, it may overshoot this value and falsely fail, if so need something more complex
+    }
+
     const float pitchP = distancePitch * positionPidCoeffs.Kp;
     const float pitchD = velocityPitch * positionPidCoeffs.Kd;
     const float pitchJ = accelerationPitch * positionPidCoeffs.Kf;
 
     // ** calculate I and rotate if quad yaws
 
-    // NOTE: iTerm readily causes circling issues hence default to zero
     // intent: on windy days, accumulate whatever iTerm we need, then rotate it if the quad yaws.
     // useful only on very windy days when P can't quite get there
-    // needs to be attenuated towards zero when close to target to avoid overshoot
-    // hence cannot completely eliminate position error due to wind.
+    // needs to be attenuated towards zero when close to target to avoid overshoot and circling
+    // hence cannot completely eliminate position error due to wind, will tend to end up a little bit down-wind
 
     if (positionPidCoeffs.Ki) {
         rollI += distanceRoll * positionPidCoeffs.Ki * gpsDataIntervalS;
@@ -291,6 +306,8 @@ void positionControl(gpsLocation_t targetLocation, float deadband) {
         DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pitchD * 10));
         DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pitchJ * 10));
     }
+
+    return true;
 }
 
 bool isBelowLandingAltitude(void)

src/main/flight/autopilot.h

@@ -28,7 +28,7 @@ void resetAltitudeControl(void);
 void resetPositionControl(void);
 
 void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
-void positionControl(gpsLocation_t targetLocation, float deadband);
+bool positionControl(gpsLocation_t targetLocation, float deadband);
 
 bool isBelowLandingAltitude(void);
 const pidCoefficient_t *getAltitudePidCoeffs(void);

src/main/flight/pos_hold.c

@@ -37,13 +37,14 @@ posHoldState_t posHold;
 
 void posHoldReset(void)
 {
+    posHold.posHoldIsOK = true;
     posHold.targetLocation = gpsSol.llh;
     resetPositionControl();
 }
 
 void posHoldInit(void)
 {
-    posHold.isPosHoldActive = false;
+    posHold.isPosHoldRequested = false;
     posHold.deadband = rcControlsConfig()->autopilot_deadband / 100.0f;
     posHoldReset();
 }
@@ -51,13 +52,13 @@ void posHoldInit(void)
 void posHoldProcessTransitions(void)
 {
     if (FLIGHT_MODE(POS_HOLD_MODE)) {
-        if (!posHold.isPosHoldActive) {
+        if (!posHold.isPosHoldRequested) {
             // initialise relevant values each time position hold starts
             posHoldReset();
-            posHold.isPosHoldActive = true;
+            posHold.isPosHoldRequested = true;
         }
     } else {
-        posHold.isPosHoldActive = false;
+        posHold.isPosHoldRequested = false;
     }
 }
 
@@ -79,8 +80,9 @@ void posHoldUpdate(void)
 {
     posHoldUpdateTargetLocation();
 
-    if (getIsNewDataForPosHold()) {
-        positionControl(posHold.targetLocation, posHold.deadband);
+    if (getIsNewDataForPosHold() && posHold.posHoldIsOK) {
+        posHold.posHoldIsOK = positionControl(posHold.targetLocation, posHold.deadband);
+        // TO DO: needs an OSD warning
     }
 }
 
@@ -90,7 +92,7 @@ void updatePosHoldState(timeUs_t currentTimeUs) {
     // check for enabling Alt Hold, otherwise do as little as possible while inactive
     posHoldProcessTransitions();
 
-    if (posHold.isPosHoldActive) {
+    if (posHold.isPosHoldRequested) {
         posHoldUpdate();
     } else {
         posHoldAngle[AI_PITCH] = 0.0f;
@@ -98,4 +100,8 @@ void updatePosHoldState(timeUs_t currentTimeUs) {
     }
 }
 
+bool showPosHoldWarning(void) {
+    return (posHold.isPosHoldRequested && !posHold.posHoldIsOK);
+}
+
 #endif // USE_POS_HOLD

src/main/flight/pos_hold.h

@@ -23,10 +23,11 @@
 #include "common/time.h"
 #include "io/gps.h"
 
-#define POSHOLD_TASK_RATE_HZ 100         // hz
+#define POSHOLD_TASK_RATE_HZ 100 // hz
 
 typedef struct {
-    bool isPosHoldActive;
+    bool isPosHoldRequested;
+    bool posHoldIsOK;
     gpsLocation_t targetLocation;
     float deadband;
 } posHoldState_t;
@@ -34,4 +35,6 @@ typedef struct {
 void posHoldInit(void);
 void updatePosHoldState(timeUs_t currentTimeUs);
 
+bool showPosHoldWarning(void);
+
 #endif

src/main/osd/osd.h

@@ -281,6 +281,7 @@ typedef enum {
     OSD_WARNING_OVER_CAP,
     OSD_WARNING_RSNR,
     OSD_WARNING_LOAD,
+    OSD_WARNING_POSHOLD_FAILED, // does this need to be inside #ifdef POSHOLD?
     OSD_WARNING_COUNT // MUST BE LAST
 } osdWarningsFlags_e;
 

src/main/osd/osd_warnings.c

@@ -50,6 +50,7 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/pos_hold.h"
 
 #include "io/beeper.h"
 
@@ -248,6 +249,15 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
 
 #endif // USE_GPS_RESCUE
 
+#ifdef USE_POS_HOLD_MODE
+    if (osdWarnGetState(OSD_WARNING_POSHOLD_FAILED) && showPosHoldWarning()) {
+        tfp_sprintf(warningText, "POS_HOLD_FAIL");
+        *displayAttr = DISPLAYPORT_SEVERITY_WARNING;
+        *blinking = true;
+        return;
+    }
+#endif
+
     // Show warning if in HEADFREE flight mode
     if (FLIGHT_MODE(HEADFREE_MODE)) {
         tfp_sprintf(warningText, "HEADFREE");