Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 15:25:29 +03:00
Code Issues Wiki Activity

configurable speed threshold to enter braking mode

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-05-05 10:15:09 +02:00
parent a13f749eb2
commit db3ddd939d
4 changed files with 20 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.h
View file

@ -59,6 +59,7 @@ typedef enum {
DEBUG_ALWAYS,
DEBUG_STAGE2,
DEBUG_WIND_ESTIMATOR,
DEBUG_BRAKING,
DEBUG_COUNT
} debugType_e;

4
src/main/fc/settings.yaml
View file

@ -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

18
src/main/navigation/navigation.c
View file

@ -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

1
src/main/navigation/navigation.h
View file

@ -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 {