Revert "remove attimode, add acro stabilise"

This reverts commit 6f2c501.
breadoven · Sep 22, 2023 · 32fcb13 · 32fcb13
1 parent 79585dc
commit 32fcb13
Show file tree

Hide file tree

Showing 16 changed files with 61 additions and 52 deletions.
diff --git a/docs/Settings.md b/docs/Settings.md
@@ -1072,16 +1072,6 @@ 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`.

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -644,15 +644,18 @@ void processRx(timeUs_t currentTimeUs)
         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
     }
 
-    /* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON */
+    /* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON > ATTITUDE HOLD */
     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);
         }
     }
 

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
@@ -99,6 +99,7 @@ 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 }
 };
 
@@ -275,6 +276,9 @@ void initActiveBoxIds(void)
         if (sensors(SENSOR_BARO)) {
             ADD_ACTIVE_BOX(BOXAUTOLEVEL);
         }
+        if (sensors(SENSOR_ACC)) {
+            ADD_ACTIVE_BOX(BOXATTIHOLD);
+        }
     }
 
     /*
@@ -419,6 +423,7 @@ 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]]) {

diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
@@ -79,6 +79,7 @@ typedef enum {
     BOXCHANGEMISSION = 50,
     BOXBEEPERMUTE    = 51,
     BOXMULTIFUNCTION = 52,
+    BOXATTIHOLD      = 53,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
@@ -176,6 +176,9 @@ 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;
 }
 

diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
@@ -104,6 +104,7 @@ typedef enum {
     TURN_ASSISTANT        = (1 << 14),
     TURTLE_MODE           = (1 << 15),
     SOARING_MODE          = (1 << 16),
+    ATTIHOLD_MODE         = (1 << 17),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
@@ -161,6 +162,7 @@ typedef enum {
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,
+    FLM_ATTIHOLD,
     FLM_COUNT
 } flightModeForTelemetry_e;
 

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -192,7 +192,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
@@ -2141,11 +2141,6 @@ 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

diff --git a/src/main/flight/pid.c b/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, 7);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -298,8 +298,6 @@ 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,
@@ -1109,39 +1107,42 @@ void FAST_CODE pidController(float dT)
 #endif
     }
 
-    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ACRO attitude stabilisation
+    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE
     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) || isFlightAxisAngleOverrideActive(axis)) {
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ATTIHOLD_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];
-            }
         }
     }
 

diff --git a/src/main/flight/pid.h b/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,9 +155,6 @@ typedef struct pidProfile_s {
 
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
-
-    bool fixedWingAcroAttiStab;
-
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -2212,6 +2212,8 @@ 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;

diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
@@ -300,6 +300,7 @@ 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

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
@@ -835,6 +835,10 @@ 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;

diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
@@ -154,6 +154,7 @@ 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 {

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
@@ -352,6 +352,8 @@ 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))) {

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
@@ -191,6 +191,7 @@ 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;
@@ -220,6 +221,7 @@ 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;
@@ -662,7 +664,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
@@ -1102,9 +1104,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)
             )

diff --git a/src/main/telemetry/sim.c b/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" };
+static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS", "ATT" };
 static const char gpsFixIndicators[] = { '!', '*', ' ' };
 
 static bool checkGroundStationNumber(uint8_t* rv)