remove attimode, add acro stabilise

docs/Settings.md

@@ -1072,6 +1072,16 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da
 
 ---
 
+### fw_acro_atti_stab
+
+Activates attitude stabilisation for ACRO mode when stick centered and roll/pitch angles within ANGLE mode bank angle limits (non inverted flight only)
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### fw_autotune_max_rate_deflection
 
 The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.

src/main/fc/fc_core.c

@@ -644,18 +644,15 @@ void processRx(timeUs_t currentTimeUs)
         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
     }
 
-    /* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON > ATTITUDE HOLD */
+    /* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON */
     DISABLE_FLIGHT_MODE(ANGLE_MODE);
     DISABLE_FLIGHT_MODE(HORIZON_MODE);
-    DISABLE_FLIGHT_MODE(ATTIHOLD_MODE);
 
     if (sensors(SENSOR_ACC)) {
         if (IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) {
             ENABLE_FLIGHT_MODE(ANGLE_MODE);
         } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
             ENABLE_FLIGHT_MODE(HORIZON_MODE);
-        } else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXATTIHOLD)) {
-            ENABLE_FLIGHT_MODE(ATTIHOLD_MODE);
         }
     }
 

src/main/fc/fc_msp_box.c

@@ -99,7 +99,6 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXCHANGEMISSION,    .boxName = "MISSION CHANGE",    .permanentId = 59 },
     { .boxId = BOXBEEPERMUTE,       .boxName = "BEEPER MUTE",       .permanentId = 60 },
     { .boxId = BOXMULTIFUNCTION,    .boxName = "MULTI FUNCTION",    .permanentId = 61 },
-    { .boxId = BOXATTIHOLD,         .boxName = "ATTITUDE HOLD",     .permanentId = 62 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -276,9 +275,6 @@ void initActiveBoxIds(void)
         if (sensors(SENSOR_BARO)) {
             ADD_ACTIVE_BOX(BOXAUTOLEVEL);
         }
-        if (sensors(SENSOR_ACC)) {
-            ADD_ACTIVE_BOX(BOXATTIHOLD);
-        }
     }
 
     /*
@@ -423,7 +419,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #ifdef USE_MULTI_FUNCTIONS
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)),   BOXMULTIFUNCTION);
 #endif
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXATTIHOLD)),        BOXATTIHOLD);
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
         if (activeBoxes[activeBoxIds[i]]) {

src/main/fc/rc_modes.h

@@ -79,7 +79,6 @@ typedef enum {
     BOXCHANGEMISSION = 50,
     BOXBEEPERMUTE    = 51,
     BOXMULTIFUNCTION = 52,
-    BOXATTIHOLD      = 53,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/runtime_config.c

@@ -176,9 +176,6 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
     if (FLIGHT_MODE(HORIZON_MODE))
         return FLM_HORIZON;
 
-    if (FLIGHT_MODE(ATTIHOLD_MODE))
-        return FLM_ATTIHOLD;
-
     return STATE(AIRMODE_ACTIVE) ? FLM_ACRO_AIR : FLM_ACRO;
 }
 

src/main/fc/runtime_config.h

@@ -104,7 +104,6 @@ typedef enum {
     TURN_ASSISTANT        = (1 << 14),
     TURTLE_MODE           = (1 << 15),
     SOARING_MODE          = (1 << 16),
-    ATTIHOLD_MODE         = (1 << 17),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
@@ -162,7 +161,6 @@ typedef enum {
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,
-    FLM_ATTIHOLD,
     FLM_COUNT
 } flightModeForTelemetry_e;
 

src/main/fc/settings.yaml

@@ -195,7 +195,7 @@ tables:
     enum: gpsBaudRate_e
   - name: nav_mc_althold_throttle
     values: ["STICK", "MID_STICK", "HOVER"]
-    enum: navMcAltHoldThrottle_e    
+    enum: navMcAltHoldThrottle_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -2149,6 +2149,11 @@ groups:
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
+      - name: fw_acro_atti_stab
+        description: "Activates attitude stabilisation for ACRO mode when stick centered and roll/pitch angles within ANGLE mode bank angle limits (non inverted flight only)"
+        default_value: OFF
+        field: fixedWingAcroAttiStab
+        type: bool
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -167,7 +167,7 @@ static EXTENDED_FASTRAM bool levelingEnabled = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -298,6 +298,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
         .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
 
+        .fixedWingAcroAttiStab = SETTING_FW_ACRO_ATTI_STAB_DEFAULT,
+
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
@@ -1107,42 +1109,39 @@ void FAST_CODE pidController(float dT)
 #endif
     }
 
-    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
+    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ACRO attitude stabilisation
     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
     levelingEnabled = false;
-
+    static bool resetAcroStab = true;
-    static bool restartAttiMode = true;
-    if (!restartAttiMode) {
-        restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE);  // set restart flag if attihold_mode not active
-    }
 
     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
-        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || isFlightAxisAngleOverrideActive(axis)) {
             // If axis angle override, get the correct angle from Logic Conditions
             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
 
-            if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) {
-                static int16_t attiHoldTarget[2];
-
-                if (restartAttiMode) {      // set target attitude to current attitude when initialised
-                    attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
-                    attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
-                    restartAttiMode = false;
-                }
-
-                uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
-                if (calculateRollPitchCenterStatus() == CENTERED) {
-                    angleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit);
-                } else {
-                    angleTarget = constrain(attitude.raw[axis] + angleTarget, -bankLimit, bankLimit);
-                    attiHoldTarget[axis] = attitude.raw[axis];
-                }
-            }
-
             // Apply the Level PID controller
             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
             levelingEnabled = true;
+            resetAcroStab = true;
+        } else if (STATE(AIRPLANE) && pidProfile()->fixedWingAcroAttiStab) {
+            /* Provides fixed wing acro attitude stabilisation to avoid attitude drift at zero rates
+             * Only active for non inverted flight within Angle mode bank angle limits */
+            static int16_t attiHoldTarget[2];
+
+            if (resetAcroStab) {    // set target attitude to current attitude when initialised
+                attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
+                attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH];
+                resetAcroStab = false;
+            }
+
+            uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
+            bool isNotinverted = ABS(attitude.raw[FD_ROLL]) < pidProfile()->max_angle_inclination[FD_ROLL];
+            if (calculateRollPitchCenterStatus() == CENTERED && ABS(attitude.raw[axis]) <= bankLimit && isNotinverted) {
+                pidLevel(constrain(attiHoldTarget[axis], -bankLimit, bankLimit), &pidState[axis], axis, horizonRateMagnitude, dT);
+            } else {
+                attiHoldTarget[axis] = attitude.raw[axis];
+            }
         }
     }
 

src/main/flight/pid.h

@@ -106,8 +106,8 @@ typedef struct pidProfile_s {
     pidBank_t bank_mc;
 
     uint8_t dterm_lpf_type;                 // Dterm LPF type: PT1, BIQUAD
-    uint16_t dterm_lpf_hz;                  
+    uint16_t dterm_lpf_hz;
-    
+
     uint8_t yaw_lpf_hz;
 
     uint8_t heading_hold_rate_limit;        // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
@@ -129,7 +129,7 @@ typedef struct pidProfile_s {
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
-    
+
     float       navVelXyDTermLpfHz;
     uint8_t navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
     uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
@@ -155,6 +155,9 @@ typedef struct pidProfile_s {
 
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
+
+    bool fixedWingAcroAttiStab;
+
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;

src/main/io/osd.c

@@ -2208,8 +2208,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (FLIGHT_MODE(ATTIHOLD_MODE))
-                p = "ATTI";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;

src/main/io/osd_dji_hd.c

@@ -300,7 +300,6 @@ static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
         case FLM_ALTITUDE_HOLD:
         case FLM_POSITION_HOLD:
         case FLM_MISSION:
-        case FLM_ATTIHOLD:
         default:
             // Unsupported ATM, keep at ANGLE
             bitArraySet(flightModeBitmask->bits, 1);    // DJI: 1 << 1 : ANGLE

src/main/programming/logic_condition.c

@@ -835,10 +835,6 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
             return (bool) FLIGHT_MODE(HORIZON_MODE);
             break;
 
-        case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE:
-            return (bool) FLIGHT_MODE(ATTIHOLD_MODE);
-            break;
-
         case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR:
             return (bool) FLIGHT_MODE(AIRMODE_ACTIVE);
             break;

src/main/programming/logic_condition.h

@@ -154,7 +154,6 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4,                              // 13
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO,                               // 14
     LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION,                   // 15
-    LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ATTIMODE,                           // 16
 } logicFlightModeOperands_e;
 
 typedef enum {

src/main/telemetry/crsf.c

@@ -352,8 +352,6 @@ static void crsfFrameFlightMode(sbuf_t *dst)
             flightMode = "ANGL";
         } else if (FLIGHT_MODE(HORIZON_MODE)) {
             flightMode = "HOR";
-        } else if (FLIGHT_MODE(ATTIHOLD_MODE)) {
-            flightMode = "ATTI";
         }
 #ifdef USE_GPS
     } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {

src/main/telemetry/mavlink.c

@@ -191,7 +191,6 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode)
         case FLM_ACRO_AIR:      return COPTER_MODE_ACRO;
         case FLM_ANGLE:         return COPTER_MODE_STABILIZE;
         case FLM_HORIZON:       return COPTER_MODE_STABILIZE;
-        case FLM_ATTIHOLD:      return COPTER_MODE_STABILIZE;
         case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD;
         case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD;
         case FLM_RTH:           return COPTER_MODE_RTL;
@@ -221,7 +220,6 @@ static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode)
         case FLM_ACRO_AIR:      return PLANE_MODE_ACRO;
         case FLM_ANGLE:         return PLANE_MODE_FLY_BY_WIRE_A;
         case FLM_HORIZON:       return PLANE_MODE_STABILIZE;
-        case FLM_ATTIHOLD:      return PLANE_MODE_STABILIZE;
         case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B;
         case FLM_POSITION_HOLD: return PLANE_MODE_LOITER;
         case FLM_RTH:           return PLANE_MODE_RTL;
@@ -664,7 +662,7 @@ void mavlinkSendHUDAndHeartbeat(void)
         // heading Current heading in degrees, in compass units (0..360, 0=north)
         DECIDEGREES_TO_DEGREES(attitude.values.yaw),
         // throttle Current throttle setting in integer percent, 0 to 100
-        thr,
+        thr, 
         // alt Current altitude (MSL), in meters, if we have surface or baro use them, otherwise use GPS (less accurate)
         mavAltitude,
         // climb Current climb rate in meters/second
@@ -1104,9 +1102,9 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
 
     if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) {
         // Only process scheduled data if we didn't serve any incoming request this cycle
-        if (!incomingRequestServed ||
+        if (!incomingRequestServed || 
             (
-                 (rxConfig()->receiverType == RX_TYPE_SERIAL) &&
+                 (rxConfig()->receiverType == RX_TYPE_SERIAL) && 
                  (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
                  !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
             )

src/main/telemetry/sim.c

@@ -122,7 +122,7 @@ static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
 static int simRssi;
 static uint8_t accEvent = ACC_EVENT_NONE;
 static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
-static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS", "ATT" };
+static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS" };
 static const char gpsFixIndicators[] = { '!', '*', ' ' };
 
 static bool checkGroundStationNumber(uint8_t* rv)