Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +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

4
docs/Settings.md
View file

@ -2654,11 +2654,11 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
### nav_cruise_yaw_rate
Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]
Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]
| Default | Min | Max |
| --- | --- | --- |
| 20 | 0 | 60 |
| 20 | 0 | 120 |
---

4
src/main/fc/settings.yaml
View file

@ -2572,11 +2572,11 @@ groups:
field: general.flags.mission_planner_reset
type: bool
- name: nav_cruise_yaw_rate
description: "Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]"
description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]"
default_value: 20
field: general.cruise_yaw_rate
min: 0
max: 60
max: 120
- name: nav_mc_bank_angle
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
default_value: 30

10
src/main/io/osd.c
View file

@ -4865,6 +4865,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
// by OSD_FLYMODE.
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
}
if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
if (posControl.cruise.multicopterSpeed >= 50.0f) {
char buf[6];
osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
tfp_sprintf(messageBuf, "(SPD %s)", buf);
} else {
strcpy(messageBuf, "(HOLD)");
}
messages[messageCount++] = messageBuf;
}
if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
}

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);