Add cruise mode heading change and heading error OSD indicators

src/main/cms/cms_menu_osd.c

@@ -190,6 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
 #endif // GPS
     OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
     OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),
+    OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR),
+    OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT),
 #if defined(USE_BARO) || defined(USE_GPS)
     OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO),
     OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),

src/main/io/osd.c

@@ -1257,6 +1257,53 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[5] = '\0';
             break;
         }
+
+    case OSD_CRUISE_HEADING_ERROR:
+        {
+            if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                displayWrite(osdDisplayPort, elemPosX, elemPosY, "     ");
+                return true;
+            }
+
+            buff[0] = SYM_HEADING;
+
+            if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
+                buff[1] = buff[2] = buff[3] = '-';
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
+                if (ABS(herr) > 99)
+                    strcpy(buff + 1, ">99");
+                else
+                    tfp_sprintf(buff + 1, "%3d", herr);
+            }
+
+            buff[4] = SYM_DEGREES;
+            buff[5] = '\0';
+            break;
+        }
+
+    case OSD_CRUISE_HEADING_ADJUSTMENT:
+        {
+            int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
+
+            if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
+                displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
+                return true;
+            }
+
+            buff[0] = SYM_HEADING;
+
+            if (!ARMING_FLAG(ARMED)) {
+                buff[1] = buff[2] = buff[3] = buff[4] = '-';
+            } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
+                tfp_sprintf(buff + 1, "%4d", heading_adjust);
+            }
+
+            buff[5] = SYM_DEGREES;
+            buff[6] = '\0';
+            break;
+        }
+
     case OSD_GPS_HDOP:
         {
             buff[0] = SYM_HDP_L;
@@ -2194,6 +2241,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
     osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
+    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
+    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
     osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
     osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG;
     osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;

src/main/io/osd.h

@@ -113,6 +113,8 @@ typedef enum {
     OSD_REMAINING_DISTANCE_BEFORE_RTH,
     OSD_POWER_SUPPLY_IMPEDANCE,
     OSD_HOME_HEADING_ERROR,
+    OSD_CRUISE_HEADING_ERROR,
+    OSD_CRUISE_HEADING_ADJUSTMENT,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/navigation/navigation.c

@@ -916,6 +916,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
     }
 
     posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
+    posControl.cruise.previousYaw = posControl.cruise.yaw;
+    posControl.cruise.lastYawAdjustmentTime = 0;
 
     return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
 }
@@ -923,7 +925,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
 {
     const timeMs_t currentTimeMs = millis();
-    static timeMs_t lastYawChangeTime = 0;
 
     if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
 
@@ -936,15 +937,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
 
     // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
     if (posControl.flags.isAdjustingHeading) {
-        timeMs_t timeDifference = currentTimeMs - lastYawChangeTime;
+        timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
         if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
         float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
         float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
         posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
         DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
-        lastYawChangeTime = currentTimeMs;
+        posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
     }
 
+    if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
+        posControl.cruise.previousYaw = posControl.cruise.yaw;
+
     uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
 
     if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
@@ -969,10 +973,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
 
     // User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
     if (posControl.flags.isAdjustingPosition) {
+        posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
+        posControl.cruise.lastYawAdjustmentTime = millis();
         return NAV_FSM_EVENT_NONE;  // reprocess the state
     }
 
-    posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
     resetPositionController();
 
     return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
@@ -3073,6 +3078,18 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) {
     return &posControl.pids;
 }
 
+bool isAdjustingPosition(void) {
+    return posControl.flags.isAdjustingPosition;
+}
+
+bool isAdjustingHeading(void) {
+    return posControl.flags.isAdjustingHeading;
+}
+
+int32_t getCruiseHeadingAdjustment(void) {
+    return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
+}
+
 #else // NAV
 
 #ifdef USE_GPS

src/main/navigation/navigation.h

@@ -394,6 +394,11 @@ float calculateAverageSpeed();
 
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 
+int32_t navigationGetHeadingError(void);
+int32_t getCruiseHeadingAdjustment(void);
+bool isAdjustingPosition(void);
+bool isAdjustingHeading(void);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.

src/main/navigation/navigation_fixedwing.c

@@ -62,6 +62,7 @@ static bool isPitchAdjustmentValid = false;
 static bool isRollAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
+static int32_t navHeadingError;
 
 
 /*-----------------------------------------------------------
@@ -280,29 +281,29 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
 
     // Calculate NAV heading error
-    int32_t headingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
+    navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
 
     // Forced turn direction
     // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
-    if (ABS(headingError) > 17000) {
+    if (ABS(navHeadingError) > 17000) {
         forceTurnDirection = true;
     }
-    else if (ABS(headingError) < 9000 && forceTurnDirection) {
+    else if (ABS(navHeadingError) < 9000 && forceTurnDirection) {
         forceTurnDirection = false;
     }
 
     // If forced turn direction flag is enabled we fix the sign of the direction
     if (forceTurnDirection) {
-        headingError = ABS(headingError);
+        navHeadingError = ABS(navHeadingError);
     }
 
     // Slow error monitoring (2Hz rate)
     if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
         // Check if error is decreasing over time
-        errorIsDecreasing = (ABS(previousHeadingError) > ABS(headingError));
+        errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
 
         // Save values for next iteration
-        previousHeadingError = headingError;
+        previousHeadingError = navHeadingError;
         previousTimeMonitoringUpdate = currentTimeUs;
     }
 
@@ -310,7 +311,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
 
     // Input error in (deg*100), output pitch angle (deg*100)
-    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + headingError, posControl.actualState.yaw, US2S(deltaMicros),
+    float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros),
                                        -DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         pidFlags);
@@ -582,4 +583,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
     }
 }
 
+int32_t navigationGetHeadingError(void)
+{
+    return navHeadingError;
+}
+
 #endif  // NAV

src/main/navigation/navigation_private.h

@@ -294,6 +294,8 @@ typedef struct {
 typedef struct {
     fpVector3_t                 targetPos;
     int32_t                     yaw;
+    int32_t                     previousYaw;
+    timeMs_t                    lastYawAdjustmentTime;
 } navCruise_t;
 
 typedef struct {