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

Allow leaving braking always and lock positon updates during braking

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-05-20 20:36:05 +02:00
parent 43c44c78dd
commit 46742997e4
2 changed files with 93 additions and 97 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1796,8 +1796,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos)
*-----------------------------------------------------------*/
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask)
{
// XY-position
if ((useMask & NAV_POS_UPDATE_XY) != 0) {
// XY-position update is allowed only when not braking in NAV_CRUISE_BRAKING
if ((useMask & NAV_POS_UPDATE_XY) != 0 && !STATE(NAV_CRUISE_BRAKING)) {
posControl.desiredState.pos.x = pos->x;
posControl.desiredState.pos.y = pos->y;
}
@ -1975,27 +1975,32 @@ static bool adjustPositionFromRCInput(void)
const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband);
const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
/*
* Process states only for POSHOLD. In any other case we go back to old routines
*/
if (
const bool brakingEntryAllowed =
IS_RC_MODE_ACTIVE(BOXBRAKING) &&
!STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!rcPitchAdjustment &&
!rcRollAdjustment &&
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
)
) {
);
/*
* 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
* Extra condition: BRAKING flight mode has to be enabled
*/
if (!STATE(NAV_CRUISE_BRAKING_LOCKED) &&
posControl.actualState.velXY > navConfig()->mc.braking_speed_threshold &&
!rcPitchAdjustment &&
!rcRollAdjustment
) {
if (brakingEntryAllowed) {
/*
* Set currnt position and target position
* Enabling NAV_CRUISE_BRAKING locks other routines from setting position!
*/
setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY);
ENABLE_STATE(NAV_CRUISE_BRAKING_LOCKED);
DEBUG_SET(DEBUG_BRAKING, 2, 1);
@ -2075,8 +2080,6 @@ static bool adjustPositionFromRCInput(void)
setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY);
}
}
retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment);
}

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

@ -296,17 +296,10 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingPosition) {
if (STATE(NAV_CRUISE_BRAKING)) {
//Use current position when we are braking
setDesiredPosition(&posControl.actualState.pos, 0, NAV_POS_UPDATE_XY);
} else {
//If not braking, use displacement based on speed
fpVector3_t stopPosition;
calculateMulticopterInitialHoldPosition(&stopPosition);
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
}
}
return false;
}