Acro trainer

Adds a new angle limiting mode for pilots who are learning to fly in acro mode. Primarily targeted at new LOS acro pilots, but can be used with FPV as well.

The feature is activated with a new mode named "ACRO TRAINER". When the feature is active, the craft will fly in normal acro mode but will limit roll/pitch axes so that they don't exceed the configured angle limit. New pilots can start with a small angle limit and progressively increase as their skills improve.

The accelerometer must be enabled for the feature to be configured and function.

Also the feature will only be active while in acro flight and will disable if ANGLE or HORIZON modes are selected.

For most users all they need to do is simply configure the new mode to be active as desired on the "Modes" tab in the configurator and configure the desired angle limit in the cli.

Configuration parameters:

acro_trainer_angle_limit: (range 10-80) Angle limit in degrees.

acro_trainer_lookahead_ms: (range 10-200) Time in milliseconds that the logic will "look ahead" to help minimize overshoot and bounce-back if the limit is approached at high gyro rates. The default value of 50 should be good for most users. For low powered or unresponsive craft (micros or brushed) it may be helpful to increase this setting if you're seeing substantial overshoot.

acro_trainer_debug_axis: (ROLL, PITCH) The axis that will log information if debugging is active.

To enable debugging:
set debug_mode = ACRO_TRAINER

debug(0) - Current angle
debug(1) - The internal logic state
debug(2) - Modified setpoint
debug(3) - Projected angle based gyro rate and lookahead period

src/main/build/debug.c

@@ -69,5 +69,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "USB",
     "SMARTAUDIO",
     "RTH",
-    "ITERM_RELAX"
+    "ITERM_RELAX",
+    "ACRO_TRAINER",
 };

src/main/build/debug.h

@@ -88,6 +88,7 @@ typedef enum {
     DEBUG_SMARTAUDIO,
     DEBUG_RTH,
     DEBUG_ITERM_RELAX,
+    DEBUG_ACRO_TRAINER,
     DEBUG_COUNT
 } debugType_e;
 

src/main/fc/fc_core.c

@@ -382,6 +382,10 @@ void tryArm(void)
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 
+#ifdef USE_ACRO_TRAINER
+        pidAcroTrainerInit();
+#endif // USE_ACRO_TRAINER
+
         if (isModeActivationConditionPresent(BOXPREARM)) {
             ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
         }
@@ -833,6 +837,10 @@ bool processRx(timeUs_t currentTimeUs)
     }
 #endif
 
+#ifdef USE_ACRO_TRAINER
+    pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
+#endif // USE_ACRO_TRAINER
+
     return true;
 }
 

src/main/fc/rc_modes.h

@@ -80,6 +80,7 @@ typedef enum {
     BOXUSER3,
     BOXUSER4,
     BOXPIDAUDIO,
+    BOXACROTRAINER,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/flight/pid.c

@@ -88,6 +88,11 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
 );
 #endif
 
+#ifdef USE_ACRO_TRAINER
+#define ACRO_TRAINER_GAIN 5.0f             // matches default angle mode strength of 50
+#define ACRO_TRAINER_LOOKAHEAD_MIN 10000   // 10ms
+#endif // USE_ACRO_TRAINER
+
 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
 
 void resetPidProfile(pidProfile_t *pidProfile)
@@ -142,7 +147,10 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .smart_feedforward = false,
         .iterm_relax = ITERM_RELAX_OFF,
         .iterm_relax_cutoff_low = 3,
-        .iterm_relax_cutoff_high = 15,
+        .iterm_relax_cutoff_high = 15,      
+        .acro_trainer_angle_limit = 20,
+        .acro_trainer_lookahead_ms = 50,
+        .acro_trainer_debug_axis = FD_ROLL
     );
 }
 
@@ -312,6 +320,14 @@ pt1Filter_t throttleLpf;
 static FAST_RAM_ZERO_INIT bool itermRotation;
 static FAST_RAM_ZERO_INIT bool smartFeedforward;
 
+#ifdef USE_ACRO_TRAINER
+static FAST_RAM_ZERO_INIT float acro_trainer_angle_limit;
+static FAST_RAM_ZERO_INIT timeUs_t acro_trainer_lookahead_time;
+static FAST_RAM_ZERO_INIT uint8_t acro_trainer_debug_axis;
+static FAST_RAM_ZERO_INIT bool acroTrainerActive;
+static FAST_RAM_ZERO_INIT int8_t acroTrainerAxisState[2];  // only need roll and pitch
+#endif // USE_ACRO_TRAINER
+
 void pidInitConfig(const pidProfile_t *pidProfile)
 {
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@@ -351,6 +367,12 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     itermRelax = pidProfile->iterm_relax;
     itermRelaxCutoffLow = pidProfile->iterm_relax_cutoff_low;
     itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
+
+#ifdef USE_ACRO_TRAINER
+    acro_trainer_angle_limit = pidProfile->acro_trainer_angle_limit;
+    acro_trainer_lookahead_time = pidProfile->acro_trainer_lookahead_ms * 1000;
+    acro_trainer_debug_axis = pidProfile->acro_trainer_debug_axis;
+#endif // USE_ACRO_TRAINER
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -360,6 +382,13 @@ void pidInit(const pidProfile_t *pidProfile)
     pidInitConfig(pidProfile);
 }
 
+#ifdef USE_ACRO_TRAINER
+void pidAcroTrainerInit(void)
+{
+    acroTrainerAxisState[FD_ROLL] = 0;
+    acroTrainerAxisState[FD_PITCH] = 0;
+}
+#endif // USE_ACRO_TRAINER
 
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex)
 {
@@ -545,6 +574,79 @@ static void handleItermRotation()
     }
 }
 
+#ifdef USE_ACRO_TRAINER
+
+int8_t acroTrainerSign(float x)
+{
+    return (x > 0.0f) - (x < 0.0f);
+}
+
+// Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
+// There are three states:
+// 1. Current angle has exceeded limit
+//    Apply correction to return to limit (similar to pidLevel)
+// 2. Future overflow has been projected based on current angle and gyro rate
+//    Manage the setPoint to control the gyro rate as the actual angle  approaches the limit (try to prevent overshoot)
+// 3. If no potential overflow is detected, then return the original setPoint
+
+static float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint)
+{
+    float ret = setPoint;
+
+#ifndef SIMULATOR_BUILD
+    if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) {
+        bool resetIterm = false;
+        float projectedAngle;
+        const int8_t setpointSign = acroTrainerSign(setPoint);
+        const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
+        const int8_t angleSign = acroTrainerSign(currentAngle);
+
+        // Limit and correct the angle when it exceeds the limit
+        if ((fabsf(currentAngle) > acro_trainer_angle_limit) || (acroTrainerAxisState[axis] != 0)) {
+            if (angleSign == -setpointSign) {  // stick has reversed - stop limiting
+                acroTrainerAxisState[axis] = 0;
+            } else {
+                if (acroTrainerAxisState[axis] != angleSign) {
+                    acroTrainerAxisState[axis] = angleSign;
+                    resetIterm = true;
+                }
+                ret = ((acro_trainer_angle_limit * angleSign) - currentAngle) * ACRO_TRAINER_GAIN;
+            }
+        }
+        
+        // Not currently over the limit so project the angle based on current angle and
+        // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
+        // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
+        if (acroTrainerAxisState[axis] == 0) {
+            const timeUs_t checkInterval = MAX(lrintf(constrainf(fabsf(gyro.gyroADCf[axis]) / 500.0f, 0.0f, 1.0f) * acro_trainer_lookahead_time), ACRO_TRAINER_LOOKAHEAD_MIN);
+            projectedAngle = (gyro.gyroADCf[axis] * checkInterval * 0.000001f) + currentAngle;
+            const int8_t projectedAngleSign = acroTrainerSign(projectedAngle);
+            if ((fabsf(projectedAngle) > acro_trainer_angle_limit) && (projectedAngleSign == setpointSign)) {
+                ret = ((acro_trainer_angle_limit * projectedAngleSign) - projectedAngle) * ACRO_TRAINER_GAIN;
+                resetIterm = true;
+            }
+        }
+
+        if (resetIterm) {
+            pidData[axis].I = 0.0f;
+        }
+ 
+        if (axis == acro_trainer_debug_axis) {
+            DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
+            DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]);
+            DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
+            DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
+        }
+    }
+#else
+    UNUSED(axis);
+    UNUSED(angleTrim);
+#endif // SIMULATOR_BUILD
+
+    return ret;
+}
+#endif // USE_ACRO_TRAINER
+
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
 void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
@@ -589,6 +691,12 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
         }
 
+#ifdef USE_ACRO_TRAINER
+        if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode) {
+            currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
+        }
+#endif // USE_ACRO_TRAINER
+
         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
         // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
 #ifdef USE_YAW_SPIN_RECOVERY
@@ -723,3 +831,10 @@ bool crashRecoveryModeActive(void)
 {
     return inCrashRecoveryMode;
 }
+
+#ifdef USE_ACRO_TRAINER
+void pidSetAcroTrainerState(bool newState)
+{
+    acroTrainerActive = newState;
+}
+#endif // USE_ACRO_TRAINER

src/main/flight/pid.h

@@ -128,7 +128,9 @@ typedef struct pidProfile_s {
     uint8_t iterm_relax_cutoff_low;        // Slowest setpoint response to prevent iterm accumulation
     uint8_t iterm_relax_cutoff_high;       // Fastest setpoint response to prevent iterm accumulation
     itermRelax_e iterm_relax;                   // Enable iterm suppression during stick input
-    
+    uint8_t acro_trainer_angle_limit;       // Acro trainer roll/pitch angle limit in degrees
+    uint16_t acro_trainer_lookahead_ms;     // The lookahead window in milliseconds used to reduce overshoot
+    uint8_t acro_trainer_debug_axis;        // The axis for which record debugging values are captured 0=roll, 1=pitch
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE
@@ -171,4 +173,6 @@ void pidInitConfig(const pidProfile_t *pidProfile);
 void pidInit(const pidProfile_t *pidProfile);
 void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
 bool crashRecoveryModeActive(void);
+void pidAcroTrainerInit(void);
+void pidSetAcroTrainerState(bool newState);
 

src/main/interface/msp_box.c

@@ -94,6 +94,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXPIDAUDIO, "PID AUDIO", 44 },
     { BOXPARALYZE, "PARALYZE", 45 },
     { BOXGPSRESCUE, "GPS RESCUE", 46 },
+    { BOXACROTRAINER, "ACRO TRAINER", 47},
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -300,6 +301,12 @@ void initActiveBoxIds(void)
     BME(BOXPIDAUDIO);
 #endif
 
+#if defined(USE_ACRO_TRAINER) && defined(USE_ACC)
+    if (sensors(SENSOR_ACC)) {
+        BME(BOXACROTRAINER);
+    }
+#endif // USE_ACRO_TRAINER
+
 #undef BME
     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
     for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)

src/main/interface/settings.c

@@ -337,6 +337,11 @@ static const char * const lookupTableItermRelax[] = {
     "OFF", "RP", "RPY"
 };
 
+#ifdef USE_ACRO_TRAINER
+static const char * const lookupTableAcroTrainerDebug[] = {
+    "ROLL", "PITCH"
+};
+#endif // USE_ACRO_TRAINER
 
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
@@ -413,6 +418,9 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableVideoSystem),
 #endif // USE_MAX7456
     LOOKUP_TABLE_ENTRY(lookupTableItermRelax),
+#ifdef USE_ACRO_TRAINER
+    LOOKUP_TABLE_ENTRY(lookupTableAcroTrainerDebug),
+#endif // USE_ACRO_TRAINER
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -759,6 +767,12 @@ const clivalue_t valueTable[] = {
     { "throttle_boost",             VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost) },
     { "throttle_boost_cutoff",      VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 5, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, throttle_boost_cutoff) },
 
+#ifdef USE_ACRO_TRAINER
+    { "acro_trainer_angle_limit",   VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) },
+    { "acro_trainer_lookahead_ms",  VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_lookahead_ms) },
+    { "acro_trainer_debug_axis",    VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACRO_TRAINER_DEBUG }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_debug_axis) },
+#endif // USE_ACRO_TRAINER
+    
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) },
     { "d_pitch",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) },

src/main/interface/settings.h

@@ -98,6 +98,9 @@ typedef enum {
     TABLE_VIDEO_SYSTEM,
 #endif // USE_MAX7456
     TABLE_ITERM_RELAX,
+#ifdef USE_ACRO_TRAINER
+    TABLE_ACRO_TRAINER_DEBUG,
+#endif // USE_ACRO_TRAINER
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

src/main/target/common_fc_pre.h

@@ -144,6 +144,7 @@
 #endif
 
 #if (FLASH_SIZE > 64)
+#define USE_ACRO_TRAINER
 #define USE_BLACKBOX
 #define USE_LED_STRIP
 #define USE_RESOURCE_MGMT