Turn assistant - turn on parallel to ground plane (#384)

* Turn assistant - turn on parallel to ground plane

src/main/config/runtime_config.h

@@ -46,6 +46,7 @@ typedef enum {
     NAV_WP_MODE     = (1 << 12),
     HEADING_LOCK    = (1 << 13),
     FLAPERON        = (1 << 14),
+    TURN_ASSISTANT  = (1 << 15),
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;

src/main/flight/pid.c

@@ -438,6 +438,26 @@ float pidMagHold(const pidProfile_t *pidProfile)
     return magHoldRate;
 }
 
+/*
+ * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
+ * and keeping ROLL and PITCH attitude though the turn.
+ */
+static void pidTurnAssistant(pidState_t *pidState)
+{
+    t_fp_vector targetRates;
+
+    targetRates.V.X = 0.0f;
+    targetRates.V.Y = 0.0f;
+    targetRates.V.Z = pidState[YAW].rateTarget;
+
+    imuTransformVectorEarthToBody(&targetRates);
+
+    // Add in roll and pitch, replace yaw completery
+    pidState[ROLL].rateTarget += targetRates.V.X;
+    pidState[PITCH].rateTarget += targetRates.V.Y;
+    pidState[YAW].rateTarget = targetRates.V.Z;
+}
+
 void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig)
 {
 
@@ -475,6 +495,10 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
         pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);
     }
 
+    if (FLIGHT_MODE(TURN_ASSISTANT)) {
+        pidTurnAssistant(pidState);
+    }
+
     // Step 4: Run gyro-driven control
     for (int axis = 0; axis < 3; axis++) {
         // Apply PID setpoint controller

src/main/io/rc_controls.h

@@ -52,6 +52,7 @@ typedef enum {
     BOXHEADINGLOCK,
     BOXSURFACE,
     BOXFLAPERON,
+    BOXTURNASSIST,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/io/serial_msp.c

@@ -149,6 +149,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXHEADINGLOCK, "HEADING LOCK;", 32 },
     { BOXSURFACE, "SURFACE;", 33 },
     { BOXFLAPERON, "FLAPERON;", 34 },
+    { BOXTURNASSIST, "TURN ASSIST;", 35 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -479,6 +480,7 @@ void mspInit(void)
     if (sensors(SENSOR_ACC)) {
         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
+        activeBoxIds[activeBoxIdCount++] = BOXTURNASSIST;
     }
 
     activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
@@ -595,6 +597,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
         IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
+        IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
 
     for (i = 0; i < activeBoxIdCount; i++) {

src/main/mw.c

@@ -383,9 +383,7 @@ void processRx(void)
         DISABLE_FLIGHT_MODE(HEADING_LOCK);
     }
 
-    /*
-    Flaperon mode
-    */
+    /* Flaperon mode */
     if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
         if (!FLIGHT_MODE(FLAPERON)) {
             ENABLE_FLIGHT_MODE(FLAPERON);
@@ -394,6 +392,15 @@ void processRx(void)
         DISABLE_FLIGHT_MODE(FLAPERON);
     }
 
+    /* Turn assistant mode */
+    if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
+        if (!FLIGHT_MODE(TURN_ASSISTANT)) {
+            ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
+    }
+
 #if defined(MAG)
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         if (IS_RC_MODE_ACTIVE(BOXMAG)) {