diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 466c905ad1..51caf2093b 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -59,6 +59,7 @@ typedef enum { DEBUG_ALWAYS, DEBUG_STAGE2, DEBUG_WIND_ESTIMATOR, + DEBUG_BRAKING, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 35a2cfc51d..ddc4f57be8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1243,6 +1243,10 @@ groups: field: mc.auto_disarm_delay min: 100 max: 10000 + - name: nav_mc_braking_speed_threshold + field: mc.braking_speed_threshold + min: 0 + max: 1000 - name: nav_fw_cruise_thr field: fw.cruise_throttle diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bac5c4dbdc..90a8e15cec 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -116,6 +116,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .max_bank_angle = 30, // 30 deg .hover_throttle = 1500, .auto_disarm_delay = 2000, + .braking_speed_threshold = 100, }, // Fixed wing @@ -1964,9 +1965,16 @@ static bool adjustPositionFromRCInput(void) else { /* - * Process states + * Process states only for POSHOLD. In any other case we go back to old routines */ - if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) { + if ( + 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 + ) + ) { const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); @@ -1975,22 +1983,24 @@ static bool adjustPositionFromRCInput(void) * Speed is above 1m/s and sticks are centered */ if (!STATE(NAV_CRUISE_BRAKING) && - posControl.actualState.velXY > 100.0f && + posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold && !rcPitchAdjustment && !rcRollAdjustment ) { ENABLE_STATE(NAV_CRUISE_BRAKING); + DEBUG_SET(DEBUG_BRAKING, 0, 1); } /* * Case when we were braking but copter finally stopped or we started to move the sticks */ if (STATE(NAV_CRUISE_BRAKING) && ( - posControl.actualState.velXY <= 100.0f || + posControl.actualState.velXY <= navConfig()->mc.braking_speed_threshold || rcPitchAdjustment || rcRollAdjustment )) { DISABLE_STATE(NAV_CRUISE_BRAKING); + DEBUG_SET(DEBUG_BRAKING, 0, 0); /* * When braking is done, store current position as desired one diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index fb30fa33ed..32e3b23e71 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -141,6 +141,7 @@ typedef struct navConfig_s { uint8_t max_bank_angle; // multicopter max banking angle (deg) uint16_t hover_throttle; // multicopter hover throttle uint16_t auto_disarm_delay; // multicopter safety delay for landing detector + uint16_t braking_speed_threshold; // above this speed braking routine might kick in } mc; struct {