Loiter direction (#4036)

* Loiter direction
https://github.com/iNavFlight/inav/issues/3002

src/main/fc/fc_msp_box.c

@@ -84,6 +84,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXBRAKING, "MC BRAKING", 46 },
     { BOXUSER1, "USER1", 47 },
     { BOXUSER2, "USER2", 48 },
+    { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -195,6 +196,9 @@ void initActiveBoxIds(void)
     const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
     if (navFlowDeadReckoning || navReadyQuads || navReadyPlanes) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
+        if (STATE(FIXED_WING)) {
+            activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN;
+        }
     }
 
     if (navReadyQuads || navReadyPlanes) {
@@ -343,6 +347,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)),     BOXBRAKING);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER1)),       BOXUSER1);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXUSER2)),       BOXUSER2);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)),BOXLOITERDIRCHN);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/rc_modes.h

@@ -66,6 +66,7 @@ typedef enum {
     BOXUSER1        = 37,
     BOXUSER2        = 38,
     BOXFPVANGLEMIX  = 39,
+    BOXLOITERDIRCHN = 40,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -49,6 +49,8 @@ tables:
     enum: gpsDynModel_e
   - name: reset_type
     values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
+  - name: direction
+    values: ["RIGHT", "LEFT", "YAW"]
   - name: nav_user_control_mode
     values: ["ATTI", "CRUISE"]
   - name: nav_rth_alt_mode
@@ -951,6 +953,10 @@ groups:
         field: fixedWingItermThrowLimit
         min: FW_ITERM_THROW_LIMIT_MIN
         max: FW_ITERM_THROW_LIMIT_MAX
+      - name: fw_loiter_direction
+        field: loiter_direction
+        condition: USE_NAV
+        table: direction
       - name: fw_reference_airspeed
         field: fixedWingReferenceAirspeed
         min: 1

src/main/flight/pid.c

@@ -194,6 +194,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingReferenceAirspeed = 1000,
         .fixedWingCoordinatedYawGain = 1.0f,
         .fixedWingItermLimitOnStickPosition = 0.5f,
+
+        .loiter_direction = NAV_LOITER_RIGHT,
 );
 
 void pidInit(void)

src/main/flight/pid.h

@@ -105,6 +105,8 @@ typedef struct pidProfile_s {
     float       fixedWingReferenceAirspeed;     // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
+
+    uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/navigation/navigation.h

@@ -54,6 +54,12 @@ enum {
     NAV_GPS_CRUISE  = 1                     // Pitch/roll stick controls velocity (forward/right speed)
 };
 
+enum {
+    NAV_LOITER_RIGHT = 0,                    // Loitering direction right
+    NAV_LOITER_LEFT  = 1,                    // Loitering direction left
+    NAV_LOITER_YAW   = 2
+};
+
 enum {
     NAV_RTH_NO_ALT          = 0,            // Maintain current altitude
     NAV_RTH_EXTRA_ALT       = 1,            // Maintain current altitude + predefined safety margin

src/main/navigation/navigation_fixedwing.c

@@ -43,6 +43,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
 #include "navigation/navigation.h"
@@ -63,6 +64,7 @@ static bool isRollAdjustmentValid = false;
 static float throttleSpeedAdjustment = 0;
 static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
+static int8_t loiterDirYaw = 1;
 
 
 /*-----------------------------------------------------------
@@ -213,6 +215,18 @@ void resetFixedWingPositionController(void)
     pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 }
 
+static int8_t loiterDirection(void) {
+    int8_t dir = 1; //NAV_LOITER_RIGHT
+    if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1;
+    if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
+        if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise
+        if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT  //see annexCode in fc_core.c
+        dir = loiterDirYaw;
+    }
+    if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1;
+    return dir;
+}
+
 static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 {
     float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
@@ -233,7 +247,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     // Calculate virtual position for straight movement
     if (needToCalculateCircularLoiter) {
         // We are closing in on a waypoint, calculate circular loiter
-        float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
+        float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
 
         float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
         float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
@@ -292,7 +306,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
 
     // If forced turn direction flag is enabled we fix the sign of the direction
     if (forceTurnDirection) {
-        navHeadingError = ABS(navHeadingError);
+        navHeadingError = loiterDirection() * ABS(navHeadingError);
     }
 
     // Slow error monitoring (2Hz rate)
@@ -308,7 +322,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
     // Only allow PID integrator to shrink if error is decreasing over time
     const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
 
-    // Input error in (deg*100), output pitch angle (deg*100)
+    // Input error in (deg*100), output roll angle (deg*100)
     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),