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

Add roll control and OSD message

Browse source
This commit is contained in:
breadoven 2023-08-08 22:03:34 +01:00
parent 505b58f9f0
commit 4f396a39ea
4 changed files with 71 additions and 44 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1061,6 +1061,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(
{
UNUSED(previousState);
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
return NAV_FSM_EVENT_ERROR;
}
DEBUG_SET(DEBUG_CRUISE, 0, 1);
// Switch to IDLE if we do not have an healty position. Try the next iteration.
if (checkForPositionSensorTimeout()) {
@ -1100,11 +1104,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS
return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ;
}
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
if (posControl.flags.isAdjustingHeading) {
int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate);
const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband;
// User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR
// We record the desired course and change the desired target in the meanwhile
if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) {
int16_t headingAdjustCommand = rcCommand[YAW];
if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) {
headingAdjustCommand = -rcCommand[ROLL];
}
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime;
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()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate));
float rateTarget = scaleRangef((float)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate);
float centidegsPerIteration = rateTarget * MS2S(timeDifference);
posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course));
@ -1137,6 +1150,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState)
{
if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control
return NAV_FSM_EVENT_ERROR;
}
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState);