Revert "CR108"

This reverts commit 6fb4bf1.
breadoven · Sep 22, 2023 · 7b198e4 · 7b198e4
1 parent 6fb4bf1
commit 7b198e4
Show file tree

Hide file tree

Showing 22 changed files with 127 additions and 107 deletions.
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
@@ -518,7 +518,7 @@ bool emergInflightRearmEnabled(void)
 
     bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f;
     if (isProbablyStillFlying() || mcDisarmVertVelCheck) {
-        // DEBUG_SET(DEBUG_ALWAYS, 6, currentTimeMs - US2MS(lastDisarmTimeUs));
+        DEBUG_SET(DEBUG_ALWAYS, 6, currentTimeMs - US2MS(lastDisarmTimeUs));
         emergRearmStabiliseTimeout = currentTimeMs + 5000; // used to activate Angle mode for 5s after rearm to help stabilise craft
         return true;
     }
@@ -697,12 +697,15 @@ void processRx(timeUs_t currentTimeUs)
     // CR108
     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) || autoEnableAngle) { // CR105
             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)) {  /* Attitude hold mode */
+            ENABLE_FLIGHT_MODE(ATTIHOLD_MODE);
         }
     }
 

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -3418,8 +3418,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 {
 #ifdef USE_SIMULATOR
     uint8_t tmp_u8;
-#endif
     const unsigned int dataSize = sbufBytesRemaining(src);
+#endif
 
     switch (cmdMSP) {
 

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 },    // CR108
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -275,6 +276,10 @@ void initActiveBoxIds(void)
         if (sensors(SENSOR_BARO)) {
             ADD_ACTIVE_BOX(BOXAUTOLEVEL);
         }
+        // CR108
+        if (sensors(SENSOR_ACC)) {
+            ADD_ACTIVE_BOX(BOXATTIHOLD);
+        }
     }
 
     /*
@@ -419,6 +424,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);  // CR108
     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,  // CR108
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
@@ -175,6 +175,9 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
     if (FLIGHT_MODE(HORIZON_MODE))
         return FLM_HORIZON;
 
+    if (FLIGHT_MODE(ATTIHOLD_MODE))
+        return FLM_ATTIHOLD; // CR108
+
     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),  // CR108
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
@@ -161,6 +162,7 @@ typedef enum {
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,
+    FLM_ATTIHOLD,   // CR108
     FLM_COUNT
 } flightModeForTelemetry_e;
 

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2157,13 +2157,7 @@ groups:
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
-        # CR108
-      - name: fw_acro_atti_stab
-        description: "Activates attitude stabilisation for ACRO mode when roll/pitch angles within ANGLE mode bank angle limits"
-        default_value: OFF
-        field: fixedWingAcroAttiStab
-        type: bool
-        # CR108
+
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
     condition: USE_AUTOTUNE_FIXED_WING

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
@@ -677,6 +677,7 @@ bool compassHeadingGPSCogErrorCheck(void)
 
     compassGpsCogError = 260;
     bool rcCommandCondition = ABS(rcCommand[PITCH]) > 25 || ABS(rcCommand[ROLL]) > 25 || navigationIsFlyingAutonomousMode();
+        // DEBUG_SET(DEBUG_ALWAYS, 0, rcCommand[PITCH]);
 
     if (sensors(SENSOR_MAG) && compassIsHealthy() && rcCommandCondition) {
         static uint16_t compassGpsCogErrorPrev = 10;

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
@@ -481,6 +481,7 @@ void FAST_CODE mixTable(void)
         return;
     }
 #endif
+    DEBUG_SET(DEBUG_ALWAYS, 4, rcCommand[THROTTLE]);
 #ifdef USE_DEV_TOOLS
     bool isDisarmed = !ARMING_FLAG(ARMED) || systemConfig()->groundTestMode;
 #else

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);   // 108
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -298,7 +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, // CR108
 
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
@@ -1109,37 +1108,42 @@ void FAST_CODE pidController(float dT)
 #endif
     }
 
-    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ACRO attitude stabilisation   // CR108
+    // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ATTI_MODE   // CR108
     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);
+    }
 
     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]; //decidegrees
+
+                if (restartAttiMode) {
+                    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;   // CR108
-        } else if (STATE(AIRPLANE) && pidProfile()->fixedWingAcroAttiStab) {
-            static int16_t attiHoldTarget[2]; //decidegrees
-
-            if (resetAcroStab) {
-                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];
-            }
         }
     }
     // CR108
@@ -1334,7 +1338,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
     DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
 
-    previousArmingState = !!ARMING_FLAG(ARMED); // CR105M
+    previousArmingState = !!ARMING_FLAG(ARMED);
 }
 
 float getFixedWingLevelTrim(void)

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -157,9 +157,6 @@ typedef struct pidProfile_s {
 
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
-
-    bool fixedWingAcroAttiStab; // CR108
-
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -2215,6 +2215,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
+            else if (FLIGHT_MODE(ATTIHOLD_MODE))    // CR108
+                p = "ATTI";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;
@@ -4600,7 +4602,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
     static bool statsAutoPagingEnabled = true;
 
     // Detect arm/disarm
-    if (armState != ARMING_FLAG(ARMED)) {   // CR105M
+    if (armState != ARMING_FLAG(ARMED)) {
         if (ARMING_FLAG(ARMED)) {
             // Display the "Arming" screen
             statsDisplayed = false;

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:  // CR108
         default:
             // Unsupported ATM, keep at ANGLE
             bitArraySet(flightModeBitmask->bits, 1);    // DJI: 1 << 1 : ANGLE

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -1130,7 +1130,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
     } else if (currentTimeMs - posControl.cruise.lastCourseAdjustmentTime > 4000) {
         posControl.cruise.previousCourse = posControl.cruise.course;
     }
-
+    DEBUG_SET(DEBUG_ALWAYS, 3, posControl.cruise.course);
     setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
 
     return NAV_FSM_EVENT_NONE;
@@ -2587,7 +2587,7 @@ void updateHomePosition(void)
     else {
         static bool isHomeResetAllowed = false;
 
-        // If pilot so desires he may reset home position to current position  // CR105M
+        // If pilot so desires he may reset home position to current position
         if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
             if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
                 const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@@ -2910,7 +2910,7 @@ bool isProbablyStillFlying(void)
     bool inFlightSanityCheck;
     if (STATE(MULTIROTOR)) {
         inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
-        // DEBUG_SET(DEBUG_ALWAYS, 7, averageAbsGyroRates());
+        DEBUG_SET(DEBUG_ALWAYS, 7, averageAbsGyroRates());
     } else {
         inFlightSanityCheck = isGPSHeadingValid();
     }
@@ -3695,15 +3695,14 @@ void checkManualEmergencyLandingControl(bool forcedActivation)
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(bool launchBypass)   // CR6
 {
     // General use debugs
-    // DEBUG_SET(DEBUG_ALWAYS, 0, testBitField );
-    // DEBUG_SET(DEBUG_ALWAYS, 1, testBitField & 1 << 0);
-    // DEBUG_SET(DEBUG_ALWAYS, 2, testBitField & 1 << 1);
-    // DEBUG_SET(DEBUG_ALWAYS, 3, testBitField & 1 << 2);
-    // DEBUG_SET(DEBUG_ALWAYS, 4, testBitField & 1 << 3);
-    // DEBUG_SET(DEBUG_ALWAYS, 5, testBitField & 1 << 4);
-    // DEBUG_SET(DEBUG_ALWAYS, 6, testBitField & 1 << 5);
-    // DEBUG_SET(DEBUG_ALWAYS, 7, testBitField & 1 << 6);
-
+    // DEBUG_SET(DEBUG_ALWAYS, 0, posControl.rthTBPointsList[0].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 1, posControl.rthTBPointsList[1].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 2, isFixedWingFlying());
+    // DEBUG_SET(DEBUG_ALWAYS, 3, posControl.rthTBPointsList[3].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 4, posControl.rthTBPointsList[4].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 5, posControl.rthTBPointsList[5].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 6, posControl.rthTBPointsList[5].x);
+    // DEBUG_SET(DEBUG_ALWAYS, 7, posControl.activeRthTBPointIndex);
 
     static bool canActivateWaypoint = false;
     static bool canActivateLaunchMode = false;

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -108,7 +108,7 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur
     }
 }
 
-static bool shouldResetReferenceAltitude(void)   // CR105M
+static bool shouldResetReferenceAltitude(void)
 {
     switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
         case NAV_RESET_NEVER: