fixes + telemetry

src/main/fc/runtime_config.c

@@ -173,6 +173,8 @@ 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

@@ -162,6 +162,7 @@ typedef enum {
     FLM_CRUISE,
     FLM_LAUNCH,
     FLM_FAILSAFE,
+    FLM_ATTIHOLD,
     FLM_COUNT
 } flightModeForTelemetry_e;
 

src/main/flight/pid.c

@@ -1110,8 +1110,11 @@ void FAST_CODE pidController(float dT)
     // 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 restartAttiMode = true;
-    bool attiModeActive = false;
+    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)) {
@@ -1120,7 +1123,6 @@ void FAST_CODE pidController(float dT)
 
             if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) {
                 static int16_t attiHoldTarget[2];
-                attiModeActive = true;
 
                 if (restartAttiMode) {      // set target attitude to current attitude when initialised
                     attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
@@ -1143,9 +1145,6 @@ void FAST_CODE pidController(float dT)
             levelingEnabled = true;
         }
     }
-    if (!restartAttiMode) {     // set restart flag if attihold_mode not active on previous loop
-        restartAttiMode = !attiModeActive;
-    }
 
     // Apply Turn Assistance
     if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {

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

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;

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 {

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;

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)