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

changes

Browse source
This commit is contained in:
breadoven 2022-11-24 16:09:25 +00:00
parent bce0cec406
commit 35c267b15c
2 changed files with 14 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1057,7 +1057,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState)
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
UNUSED(previousState);
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
@ -1066,19 +1066,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} // Switch to IDLE if we do not have an healty position. Try the next iteration.
if (!(prevFlags & NAV_CTL_POS)) {
resetPositionController();
}
resetPositionController();
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
posControl.cruise.previousYaw = posControl.cruise.yaw;
posControl.cruise.lastYawAdjustmentTime = 0;
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
return NAV_FSM_EVENT_SUCCESS; // Go to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState)
{
UNUSED(previousState);
const timeMs_t currentTimeMs = millis();
if (checkForPositionSensorTimeout()) {
@ -1095,30 +1095,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
if (posControl.flags.isAdjustingHeading) {
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
float centidegsPerIteration = rateTarget * timeDifference * 0.001f;
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
}
if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
} else if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000) {
posControl.cruise.previousYaw = posControl.cruise.yaw;
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING)
|| (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING)
|| posControl.flags.isAdjustingHeading) {
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 1);
} else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 2);
}
setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
setDesiredPosition(NULL, posControl.cruise.yaw, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}
@ -1137,7 +1124,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
resetPositionController();
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
return NAV_FSM_EVENT_SUCCESS; // go back to NAV_STATE_COURSE_HOLD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
@ -3744,13 +3731,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// PH has priority over COURSE_HOLD
// CRUISE has priority on AH
if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) {
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) {
return NAV_FSM_EVENT_SWITCH_TO_CRUISE;
}
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold))
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) {
return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD;
}
}
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {