Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +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 brakingModeDisengageAt = 0;
static uint32_t brakingBoostModeDisengageAt = 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 = const bool brakingEntryAllowed =
IS_RC_MODE_ACTIVE(BOXBRAKING) && IS_RC_MODE_ACTIVE(BOXBRAKING) &&
!STATE(NAV_CRUISE_BRAKING_LOCKED) && !STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold && posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!rcPitchAdjustment && !isAdjusting &&
!rcRollAdjustment &&
navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE && navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE &&
navConfig()->mc.braking_speed_threshold > 0 && 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 // We can enter braking only after user started to move the sticks
if (STATE(NAV_CRUISE_BRAKING_LOCKED) && if (STATE(NAV_CRUISE_BRAKING_LOCKED) && isAdjusting) {
(rcPitchAdjustment || rcRollAdjustment)
) {
DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED); DISABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
} }
@ -2421,8 +2410,7 @@ static bool adjustPositionFromRCInput(void)
*/ */
if (STATE(NAV_CRUISE_BRAKING) && ( if (STATE(NAV_CRUISE_BRAKING) && (
posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed || //We stopped posControl.actualState.velXY <= navConfig()->mc.braking_disengage_speed || //We stopped
rcPitchAdjustment || //Moved pitch stick isAdjusting || //Moved the sticks
rcRollAdjustment || //Moved roll stick
brakingModeDisengageAt < millis() //Braking is done to timed disengage brakingModeDisengageAt < millis() //Braking is done to timed disengage
)) { )) {
DISABLE_STATE(NAV_CRUISE_BRAKING); DISABLE_STATE(NAV_CRUISE_BRAKING);
@ -2435,6 +2423,21 @@ static bool adjustPositionFromRCInput(void)
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
} }
#endif #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); retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
} }