Merge pull request #3159 from iNavFlight/dzikuvx-mr-cruise-experiments

Multirotor braking mode

src/main/fc/fc_msp.c

@@ -1357,6 +1357,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 sbufWriteU8(dst, timerHardware[i].usageFlags);
         break;
 
+    case MSP2_INAV_MC_BRAKING:
+#ifdef USE_MR_BRAKING_MODE
+        sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
+        sbufWriteU16(dst, navConfig()->mc.braking_disengage_speed);
+        sbufWriteU16(dst, navConfig()->mc.braking_timeout);
+        sbufWriteU8(dst, navConfig()->mc.braking_boost_factor);
+        sbufWriteU16(dst, navConfig()->mc.braking_boost_timeout);
+        sbufWriteU16(dst, navConfig()->mc.braking_boost_speed_threshold);
+        sbufWriteU16(dst, navConfig()->mc.braking_boost_disengage_speed);
+        sbufWriteU8(dst, navConfig()->mc.braking_bank_angle);
+#endif
+        break;
+
     default:
         return false;
     }
@@ -2530,6 +2543,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 #endif
 
+    case MSP2_INAV_SET_MC_BRAKING:
+#ifdef USE_MR_BRAKING_MODE
+        navConfigMutable()->mc.braking_speed_threshold = sbufReadU16(src);
+        navConfigMutable()->mc.braking_disengage_speed = sbufReadU16(src);
+        navConfigMutable()->mc.braking_timeout = sbufReadU16(src);
+        navConfigMutable()->mc.braking_boost_factor = sbufReadU8(src);
+        navConfigMutable()->mc.braking_boost_timeout = sbufReadU16(src);
+        navConfigMutable()->mc.braking_boost_speed_threshold = sbufReadU16(src);
+        navConfigMutable()->mc.braking_boost_disengage_speed = sbufReadU16(src);
+        navConfigMutable()->mc.braking_bank_angle = sbufReadU8(src);
+#endif
+        break;
+
     case MSP2_INAV_SELECT_BATTERY_PROFILE:
         if (!ARMING_FLAG(ARMED)) {
             if (sbufReadU8Safe(&tmp_u8, src))

src/main/fc/fc_msp_box.c

@@ -29,6 +29,7 @@
 #include "fc/config.h"
 #include "fc/fc_msp_box.h"
 #include "fc/runtime_config.h"
+#include "flight/mixer.h"
 
 #include "io/osd.h"
 
@@ -79,6 +80,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXOSDALT2, "OSD ALT 2", 43 },
     { BOXOSDALT3, "OSD ALT 3", 44 },
     { BOXNAVCRUISE, "NAV CRUISE", 45 },
+    { BOXBRAKING, "MC BRAKING", 46 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -202,6 +204,13 @@ void initActiveBoxIds(void)
             }
         }
     }
+
+#ifdef USE_MR_BRAKING_MODE
+    if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
+        activeBoxIds[activeBoxIdCount++] = BOXBRAKING;
+    }
+#endif
+
 #endif
 
     if (STATE(FIXED_WING)) {
@@ -319,6 +328,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT2)),     BOXOSDALT2);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSDALT3)),     BOXOSDALT3);
     CHECK_ACTIVE_BOX(IS_ENABLED(navigationTerrainFollowingEnabled()),   BOXSURFACE);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBRAKING)),     BOXBRAKING);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/rc_modes.h

@@ -60,6 +60,7 @@ typedef enum {
     BOXOSDALT2      = 33,
     BOXOSDALT3      = 34,
     BOXNAVCRUISE    = 35,
+    BOXBRAKING      = 36,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/runtime_config.h

@@ -100,7 +100,10 @@ typedef enum {
     COMPASS_CALIBRATED      = (1 << 8),
     ACCELEROMETER_CALIBRATED= (1 << 9),
     PWM_DRIVER_AVAILABLE    = (1 << 10),
-    HELICOPTER              = (1 << 11)
+    HELICOPTER              = (1 << 11),
+    NAV_CRUISE_BRAKING      = (1 << 12),
+    NAV_CRUISE_BRAKING_BOOST = (1 << 13),
+    NAV_CRUISE_BRAKING_LOCKED = (1 << 14),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -1307,7 +1307,46 @@ groups:
         field: mc.auto_disarm_delay
         min: 100
         max: 10000
-
+      - name: nav_mc_braking_speed_threshold
+        field: mc.braking_speed_threshold
+        condition: USE_MR_BRAKING_MODE
+        min: 0
+        max: 1000   
+      - name: nav_mc_braking_disengage_speed
+        field: mc.braking_disengage_speed
+        condition: USE_MR_BRAKING_MODE
+        min: 0
+        max: 1000  
+      - name: nav_mc_braking_timeout
+        field: mc.braking_timeout
+        condition: USE_MR_BRAKING_MODE
+        min: 100
+        max: 5000 
+      - name: nav_mc_braking_boost_factor
+        field: mc.braking_boost_factor
+        condition: USE_MR_BRAKING_MODE
+        min: 0
+        max: 200  
+      - name: nav_mc_braking_boost_timeout
+        field: mc.braking_boost_timeout
+        condition: USE_MR_BRAKING_MODE
+        min: 0
+        max: 5000  
+      - name: nav_mc_braking_boost_speed_threshold
+        field: mc.braking_boost_speed_threshold
+        condition: USE_MR_BRAKING_MODE
+        min: 100
+        max: 1000  
+      - name: nav_mc_braking_boost_disengage_speed
+        field: mc.braking_boost_disengage_speed
+        condition: USE_MR_BRAKING_MODE
+        min: 0
+        max: 1000
+      - name: nav_mc_braking_bank_angle
+        field: mc.braking_bank_angle
+        condition: USE_MR_BRAKING_MODE
+        min: 15
+        max: 60  
       - name: nav_fw_cruise_thr
         field: fw.cruise_throttle
         min: 1000

src/main/msp/msp_protocol_v2_inav.h

@@ -29,6 +29,8 @@
 #define MSP2_INAV_SET_RATE_PROFILE              0x2008
 #define MSP2_INAV_AIR_SPEED                     0x2009
 #define MSP2_INAV_OUTPUT_MAPPING                0x200A
+#define MSP2_INAV_MC_BRAKING                    0x200B
+#define MSP2_INAV_SET_MC_BRAKING                0x200C
 
 #define MSP2_INAV_MIXER                         0x2010
 #define MSP2_INAV_SET_MIXER                     0x2011

src/main/navigation/navigation.c

@@ -116,9 +116,17 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
 
     // MC-specific
     .mc = {
-        .max_bank_angle = 30,      // 30 deg
+        .max_bank_angle = 30,                   // 30 deg
         .hover_throttle = 1500,
         .auto_disarm_delay = 2000,
+        .braking_speed_threshold = 100,         // Braking can become active above 1m/s
+        .braking_disengage_speed = 75,          // Stop when speed goes below 0.75m/s
+        .braking_timeout = 2000,                // Timeout barking after 2s
+        .braking_boost_factor = 100,            // A 100% boost by default
+        .braking_boost_timeout = 750,           // Timout boost after 750ms
+        .braking_boost_speed_threshold = 150,   // Boost can happen only above 1.5m/s
+        .braking_boost_disengage_speed = 100,   // Disable boost at 1m/s
+        .braking_bank_angle = 40,               // Max braking angle     
     },
 
     // Fixed wing
@@ -2161,8 +2169,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos)
  *-----------------------------------------------------------*/
 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
 {
-    // XY-position
+    // XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
-    if ((useMask & NAV_POS_UPDATE_XY) != 0) {
+    if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
         posControl.desiredState.pos.x = pos->x;
         posControl.desiredState.pos.y = pos->y;
     }
@@ -2333,6 +2341,90 @@ static void resetPositionController(void)
     }
 }
 
+
+static void processBrakingMode(const bool isAdjusting) 
+{
+#ifdef USE_MR_BRAKING_MODE
+
+    static uint32_t brakingModeDisengageAt = 0;
+    static uint32_t brakingBoostModeDisengageAt = 0;
+
+    const bool brakingEntryAllowed = 
+        IS_RC_MODE_ACTIVE(BOXBRAKING) &&
+        !STATE(NAV_CRUISE_BRAKING_LOCKED) && 
+        posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold && 
+        !isAdjusting &&
+        navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE &&
+        navConfig()->mc.braking_speed_threshold > 0 &&
+        (
+            NAV_Status.state == MW_NAV_STATE_NONE ||
+            NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT
+        );
+
+
+    /*
+        * Case one, when we order to brake (sticks to the center) and we are moving above threshold
+        * Speed is above 1m/s and sticks are centered
+        * Extra condition: BRAKING flight mode has to be enabled
+        */
+    if (brakingEntryAllowed) {
+        /*
+            * Set currnt position and target position
+            * Enabling NAV_CRUISE_BRAKING locks other routines from setting position!
+            */
+        setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
+
+        ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
+        ENABLE_STATE(NAV_CRUISE_BRAKING);
+
+        //Set forced BRAKING disengage moment
+        brakingModeDisengageAt = millis() + navConfig()->mc.braking_timeout;
+
+        //If speed above threshold, start boost mode as well
+        if (posControl.actualState.velXY > navConfig()->mc.braking_boost_speed_threshold) {
+            ENABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
+
+            brakingBoostModeDisengageAt = millis() + navConfig()->mc.braking_boost_timeout;
+        }
+
+    }
+
+    // We can enter braking only after user started to move the sticks
+    if (STATE(NAV_CRUISE_BRAKING_LOCKED) && isAdjusting) {
+        DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
+    }
+
+    /*
+     * Case when speed dropped, disengage BREAKING_BOOST
+     */
+    if (
+        STATE(NAV_CRUISE_BRAKING_BOOST) && (
+            posControl.actualState.velXY <= navConfig()->mc.braking_boost_disengage_speed ||
+            brakingBoostModeDisengageAt < millis()
+    )) {
+        DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
+    }
+
+    /*
+     * Case when we were braking but copter finally stopped or we started to move the sticks
+     */
+    if (STATE(NAV_CRUISE_BRAKING) && (
+        posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed ||  //We stopped
+        isAdjusting ||                                                              //Moved the sticks
+        brakingModeDisengageAt < millis()                                           //Braking is done to timed disengage
+    )) {
+        DISABLE_STATE(NAV_CRUISE_BRAKING);
+        DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
+
+        /*
+            * When braking is done, store current position as desired one
+            * We do not want to go back to the place where braking has started
+            */
+        setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
+    }
+#endif
+}
+
 static bool adjustPositionFromRCInput(void)
 {
     bool retValue;
@@ -2341,7 +2433,13 @@ static bool adjustPositionFromRCInput(void)
         retValue = adjustFixedWingPositionFromRCInput();
     }
     else {
-        retValue = adjustMulticopterPositionFromRCInput();
+
+        const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
+        const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
+
+        processBrakingMode(rcPitchAdjustment || rcRollAdjustment);
+
+        retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
     }
 
     return retValue;

src/main/navigation/navigation.h

@@ -148,9 +148,17 @@ typedef struct navConfig_s {
     } general;
 
     struct {
-        uint8_t  max_bank_angle;             // multicopter max banking angle (deg)
+        uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
-        uint16_t hover_throttle;             // multicopter hover throttle
+        uint16_t hover_throttle;                // multicopter hover throttle
-        uint16_t auto_disarm_delay;          // multicopter safety delay for landing detector
+        uint16_t auto_disarm_delay;             // multicopter safety delay for landing detector
+        uint16_t braking_speed_threshold;       // above this speed braking routine might kick in
+        uint16_t braking_disengage_speed;       // below this speed braking will be disengaged
+        uint16_t braking_timeout;               // Timeout for braking mode
+        uint8_t  braking_boost_factor;          // Acceleration boost multiplier at max speed
+        uint16_t braking_boost_timeout;         // Timeout for boost mode
+        uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
+        uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
+        uint8_t  braking_bank_angle;            // Max angle [deg] that MR is allowed duing braking boost phase
     } mc;
 
     struct {

src/main/navigation/navigation_multicopter.c

@@ -276,11 +276,8 @@ void resetMulticopterPositionController(void)
     }
 }
 
-bool adjustMulticopterPositionFromRCInput(void)
+bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment)
 {
-    const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
-    const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
-
     if (rcPitchAdjustment || rcRollAdjustment) {
         // If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID
         if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) {
@@ -382,15 +379,22 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     if (velErrorMagnitude > 0.1f) {
         accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
         accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
-    }
+    } else {
-    else {
         accelLimitX = maxAccelLimit / 1.414213f;
         accelLimitY = accelLimitX;
     }
 
     // Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate
     // This will assure that we wont't saturate out LEVEL and RATE PID controller
-    const float maxAccelChange = US2S(deltaMicros) * 1700.0f;
+
+    float maxAccelChange = US2S(deltaMicros) * 1700.0f;
+    //When braking, raise jerk limit even if we are not boosting acceleration
+#ifdef USE_MR_BRAKING_MODE
+    if (STATE(NAV_CRUISE_BRAKING)) {
+        maxAccelChange = maxAccelChange * 2;
+    }
+#endif 
+
     const float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX);
     const float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX);
     const float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY);
@@ -401,16 +405,47 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     // Apply PID with output limiting and I-term anti-windup
     // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
     // Thus we don't need to do anything else with calculated acceleration
-    const float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
+    float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
-    const float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
+    float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
+
+    int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle);
+    uint8_t accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ;
+
+#ifdef USE_MR_BRAKING_MODE
+    //Boost required accelerations
+    if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0)
+    {
+        const float rawBoostFactor = (100.0f + (float)navConfig()->mc.braking_boost_factor) / 100.0f;
+        
+        //Scale boost factor according to speed
+        const float boostFactor = constrainf(
+            scaleRangef(
+                posControl.actualState.velXY, 
+                navConfig()->mc.braking_boost_speed_threshold, 
+                navConfig()->general.max_manual_speed, 
+                0, 
+                rawBoostFactor
+            ),
+            0, 
+            rawBoostFactor
+        );
+
+        //Boost required acceleration for harder braking
+        newAccelX = newAccelX * boostFactor;
+        newAccelY = newAccelY * boostFactor;
+
+        maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.braking_bank_angle);
+        accCutoffFrequency = NAV_ACCEL_CUTOFF_FREQUENCY_HZ * 2;
+    }
+#endif
 
     // Save last acceleration target
     lastAccelTargetX = newAccelX;
     lastAccelTargetY = newAccelY;
 
     // Apply LPF to jerk limited acceleration target
-    const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    const float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, accCutoffFrequency, US2S(deltaMicros));
-    const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    const float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, accCutoffFrequency, US2S(deltaMicros));
 
     // Rotate acceleration target into forward-right frame (aircraft)
     const float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;
@@ -420,7 +455,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
     const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
 
-    const int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle);
     posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
     posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
 }

src/main/navigation/navigation_private.h

@@ -392,7 +392,7 @@ void resetMulticopterHeadingController(void);
 
 bool adjustMulticopterAltitudeFromRCInput(void);
 bool adjustMulticopterHeadingFromRCInput(void);
-bool adjustMulticopterPositionFromRCInput(void);
+bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 

src/main/target/OMNIBUS/target.h

@@ -163,3 +163,5 @@
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         (BIT(13)|BIT(14)|BIT(15))
 #define TARGET_IO_PORTF         (BIT(0)|BIT(1)|BIT(4))
+
+#undef USE_MR_BRAKING_MODE

src/main/target/common.h

@@ -56,6 +56,7 @@
 #define USE_TELEMETRY_FRSKY
 
 #define USE_GYRO_BIQUAD_RC_FIR2
+#define USE_MR_BRAKING_MODE
 
 #if defined(STM_FAST_TARGET)
 #define SCHEDULER_DELAY_LIMIT           10