Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +03:00
Code Issues Wiki Activity

small refactoring of braking entry-leave conditions

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-10-05 14:28:07 +02:00
parent 507b832263
commit b803ae6391
1 changed files with 85 additions and 82 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2341,28 +2341,19 @@ static void resetPositionController(void)
}
}
static bool adjustPositionFromRCInput(void)
static void processBrakingMode(const bool isAdjusting)
{
bool retValue;
#ifdef USE_MR_BRAKING_MODE
static uint32_t brakingModeDisengageAt = 0;
static uint32_t brakingBoostModeDisengageAt = 0;
if (STATE(FIXED_WING)) {
retValue = adjustFixedWingPositionFromRCInput();
}
else {
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
#ifdef USE_MR_BRAKING_MODE
const bool brakingEntryAllowed =
IS_RC_MODE_ACTIVE(BOXBRAKING) &&
!STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!rcPitchAdjustment &&
!rcRollAdjustment &&
!isAdjusting &&
navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE &&
navConfig()->mc.braking_speed_threshold > 0 &&
(
@ -2399,9 +2390,7 @@ static bool adjustPositionFromRCInput(void)
}
// We can enter braking only after user started to move the sticks
if (STATE(NAV_CRUISE_BRAKING_LOCKED) &&
(rcPitchAdjustment || rcRollAdjustment)
) {
if (STATE(NAV_CRUISE_BRAKING_LOCKED) && isAdjusting) {
DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
}
@ -2421,8 +2410,7 @@ static bool adjustPositionFromRCInput(void)
*/
if (STATE(NAV_CRUISE_BRAKING) && (
posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed || //We stopped
rcPitchAdjustment || //Moved pitch stick
rcRollAdjustment || //Moved roll stick
isAdjusting || //Moved the sticks
brakingModeDisengageAt < millis() //Braking is done to timed disengage
)) {
DISABLE_STATE(NAV_CRUISE_BRAKING);
@ -2435,6 +2423,21 @@ static bool adjustPositionFromRCInput(void)
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
}
#endif
}
static bool adjustPositionFromRCInput(void)
{
bool retValue;
if (STATE(FIXED_WING)) {
retValue = adjustFixedWingPositionFromRCInput();
}
else {
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);
}