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

More advanced state pseudo-machine for braking

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-05-08 11:48:38 +02:00
parent 6a7995a028
commit 97ee1c86b3
3 changed files with 55 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/runtime_config.h
View file

@ -102,6 +102,7 @@ typedef enum {
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))

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

@ -113,16 +113,16 @@ 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 = 0, // By default do not use boost factor
.braking_boost_timeout = 750, // Timout boost after 750ms
.braking_boost_speed_threshold = 200, //Boost can happen only above 2m/s
.braking_boost_disengage_speed = 100, //Disable boost at 1m/s
.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 = 0, // By default do not use boost factor
.braking_boost_timeout = 750, // Timout boost after 750ms
.braking_boost_speed_threshold = 200, // Boost can happen only above 2m/s
.braking_boost_disengage_speed = 100, // Disable boost at 1m/s
},
// Fixed wing
@ -1990,16 +1990,36 @@ static bool adjustPositionFromRCInput(void)
* 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
*/
if (!STATE(NAV_CRUISE_BRAKING) &&
if (!STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!rcPitchAdjustment &&
!rcRollAdjustment
) {
ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
DEBUG_SET(DEBUG_BRAKING, 2, 1);
ENABLE_STATE(NAV_CRUISE_BRAKING);
DEBUG_SET(DEBUG_BRAKING, 0, 1);
//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);
DEBUG_SET(DEBUG_BRAKING, 1, 1);
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) &&
(rcPitchAdjustment || rcRollAdjustment)
) {
DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
DEBUG_SET(DEBUG_BRAKING, 2, 0);
}
//Forced BRAKING disengage routine
@ -2009,6 +2029,29 @@ static bool adjustPositionFromRCInput(void)
) {
DISABLE_STATE(NAV_CRUISE_BRAKING);
DEBUG_SET(DEBUG_BRAKING, 0, 0);
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
DEBUG_SET(DEBUG_BRAKING, 1, 0);
}
//Forced BRAKING_BOOST disengage routine
if (
STATE(NAV_CRUISE_BRAKING_BOOST) &&
brakingBoostModeDisengageAt < millis()
) {
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
DEBUG_SET(DEBUG_BRAKING, 1, 0);
}
/*
* Case when speed dropped, disengage BREAKING_BOOST
*/
if (
STATE(NAV_CRUISE_BRAKING_BOOST) &&
posControl.actualState.velXY <= navConfig()->mc.braking_boost_disengage_speed
) {
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
DEBUG_SET(DEBUG_BRAKING, 1, 0);
}
/*
@ -2021,6 +2064,8 @@ static bool adjustPositionFromRCInput(void)
)) {
DISABLE_STATE(NAV_CRUISE_BRAKING);
DEBUG_SET(DEBUG_BRAKING, 0, 0);
DISABLE_STATE(NAV_CRUISE_BRAKING_BOOST);
DEBUG_SET(DEBUG_BRAKING, 1, 0);
/*
* When braking is done, store current position as desired one

5
src/main/navigation/navigation_multicopter.c
View file

@ -392,8 +392,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
maxAccelChange = maxAccelChange * 2;
}
DEBUG_SET(DEBUG_BRAKING, 1, maxAccelChange);
const float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX);
const float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX);
const float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY);
@ -417,9 +415,6 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
newAccelY = newAccelY * boostFactor;
}
DEBUG_SET(DEBUG_BRAKING, 2, newAccelX);
DEBUG_SET(DEBUG_BRAKING, 3, newAccelY);
// Save last acceleration target
lastAccelTargetX = newAccelX;
lastAccelTargetY = newAccelY;