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

Add cruise mode heading change and heading error OSD indicators

Browse source
This commit is contained in:
Michel Pastor 2018-06-13 17:18:56 +02:00
parent a5b6d49851
commit c9a69a6232
7 changed files with 94 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -916,6 +916,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
}
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
}
@ -923,7 +925,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
{
const timeMs_t currentTimeMs = millis();
static timeMs_t lastYawChangeTime = 0;
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
@ -936,15 +937,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
// 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 - lastYawChangeTime;
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
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 / 1000.0f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
lastYawChangeTime = currentTimeMs;
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
}
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_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
@ -969,10 +973,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
if (posControl.flags.isAdjustingPosition) {
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
posControl.cruise.lastYawAdjustmentTime = millis();
return NAV_FSM_EVENT_NONE; // reprocess the state
}
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
resetPositionController();
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
@ -3073,6 +3078,18 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) {
return &posControl.pids;
}
bool isAdjustingPosition(void) {
return posControl.flags.isAdjustingPosition;
}
bool isAdjustingHeading(void) {
return posControl.flags.isAdjustingHeading;
}
int32_t getCruiseHeadingAdjustment(void) {
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
}
#else // NAV
#ifdef USE_GPS