Skip to content

Commit

Permalink
Add roll control and OSD message
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Aug 8, 2023
1 parent 505b58f commit 4f396a3
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 44 deletions.
4 changes: 2 additions & 2 deletions docs/Settings.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 |

---

Expand Down
4 changes: 2 additions & 2 deletions src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
84 changes: 47 additions & 37 deletions src/main/io/osd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -542,7 +542,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
buff[0] = ' ';
}

#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values
if (isBfCompatibleVideoSystem(osdConfig())) {
totalDigits++;
digits++;
Expand Down Expand Up @@ -636,8 +636,8 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
}
}

/**
* Trim whitespace from string.
/**
* Trim whitespace from string.
* Used in Stats screen on lines with multiple values.
*/
char *osdFormatTrimWhiteSpace(char *buff)
Expand All @@ -648,7 +648,7 @@ char *osdFormatTrimWhiteSpace(char *buff)
while(isspace((unsigned char)*buff)) buff++;

// All spaces?
if(*buff == 0)
if(*buff == 0)
return buff;

// Trim trailing spaces
Expand Down Expand Up @@ -1094,7 +1094,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
* Check if this OSD layout is using scaled or unscaled throttle.
* If both are used, it will default to scaled.
*/
bool osdUsingScaledThrottle(void)
bool osdUsingScaledThrottle(void)
{
bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
Expand Down Expand Up @@ -2979,7 +2979,7 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
buff[digits + 1] = SYM_MAH_MI_1;
buff[digits + 2] = '\0';
Expand All @@ -2993,7 +2993,7 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[digits] = SYM_MAH_NM_0;
buff[digits + 1] = SYM_MAH_NM_1;
buff[digits + 2] = '\0';
Expand All @@ -3009,7 +3009,7 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
}
if (!efficiencyValid) {
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[0] = buff[1] = buff[2] = buff[3] = '-';
buff[digits] = SYM_MAH_KM_0;
buff[digits + 1] = SYM_MAH_KM_1;
buff[digits + 2] = '\0';
Expand Down Expand Up @@ -4094,7 +4094,7 @@ static void osdUpdateStats(void)
static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
{
const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
uint8_t top = 1; // Start one line down leaving space at the top of the screen.
uint8_t top = 1; // Start one line down leaving space at the top of the screen.
size_t multiValueLengthOffset = 0;

const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
Expand All @@ -4116,14 +4116,14 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
}

if (isSinglePageStatsCompatible || page == 0) {
if (feature(FEATURE_GPS)) {
if (feature(FEATURE_GPS)) {
if (isSinglePageStatsCompatible) {
displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :");
osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
osdLeftAlignString(buff);
strcat(osdFormatTrimWhiteSpace(buff),"/");
multiValueLengthOffset = strlen(buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
osdGenerateAverageVelocityStr(buff);
osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
Expand Down Expand Up @@ -4160,7 +4160,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
osdLeftAlignString(buff);
strcat(osdFormatTrimWhiteSpace(buff), "%/");
multiValueLengthOffset = strlen(buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
osdLeftAlignString(buff);
Expand All @@ -4175,7 +4175,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
}
}

displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :");
itoa(stats.min_lq, buff, 10);
Expand All @@ -4201,7 +4201,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :");
displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
}

if (isSinglePageStatsCompatible || page == 1) {
if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
Expand Down Expand Up @@ -4323,20 +4323,20 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
}
}

const float max_gforce = accGetMeasuredMaxG();
const float max_gforce = accGetMeasuredMaxG();
displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :");
osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3);
displayWrite(osdDisplayPort, statValuesX, top++, buff);

const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
const float acc_extremes_min = acc_extremes[Z].min;
const float acc_extremes_max = acc_extremes[Z].max;
displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4);
osdLeftAlignString(buff);
strcat(osdFormatTrimWhiteSpace(buff),"/");
multiValueLengthOffset = strlen(buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
strcat(osdFormatTrimWhiteSpace(buff),"/");
multiValueLengthOffset = strlen(buff);
displayWrite(osdDisplayPort, statValuesX, top, buff);
osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3);
osdLeftAlignString(buff);
displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
Expand Down Expand Up @@ -4546,41 +4546,41 @@ static void osdRefresh(timeUs_t currentTimeUs)
statsCurrentPage = 0;
statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
osdShowStats(statsSinglePageCompatible, statsCurrentPage);
osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
}

armState = ARMING_FLAG(ARMED);
}

// This block is entered when we're showing the "Splash", "Armed" or "Stats" screens
if (resumeRefreshAt) {
if (resumeRefreshAt) {

// Handle events only when the "Stats" screen is being displayed.
if (statsDisplayed) {

// Manual paging stick commands are only applicable to multi-page stats.
// ******************************
// For single-page stats, this effectively disables the ability to cancel the
// For single-page stats, this effectively disables the ability to cancel the
// automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time
// is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then
// "Saved Settings" should display if it is active within the refresh interval.
// is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then
// "Saved Settings" should display if it is active within the refresh interval.
// ******************************
// With multi-page stats, "Saved Settings" could also be missed if the user
// has canceled automatic paging using the stick commands, because that is only
// updated when osdShowStats() is called. So, in that case, they would only see
// the "Saved Settings" message if they happen to manually change pages using the
// stick commands within the interval the message is displayed.
// has canceled automatic paging using the stick commands, because that is only
// updated when osdShowStats() is called. So, in that case, they would only see
// the "Saved Settings" message if they happen to manually change pages using the
// stick commands within the interval the message is displayed.
bool manualPageUpRequested = false;
bool manualPageDownRequested = false;
bool manualPageDownRequested = false;
if (!statsSinglePageCompatible) {
// These methods ensure the paging stick commands are held for a brief period
// Otherwise it can result in a race condition where the stats are
// updated too quickly and can result in partial blanks, etc.
if (osdIsPageUpStickCommandHeld()) {
// Otherwise it can result in a race condition where the stats are
// updated too quickly and can result in partial blanks, etc.
if (osdIsPageUpStickCommandHeld()) {
manualPageUpRequested = true;
statsAutoPagingEnabled = false;
} else if (osdIsPageDownStickCommandHeld()) {
manualPageDownRequested = true;
manualPageDownRequested = true;
statsAutoPagingEnabled = false;
}
}
Expand All @@ -4603,7 +4603,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
// Process manual page change events for multi-page stats.
if (manualPageUpRequested) {
osdShowStats(statsSinglePageCompatible, 1);
statsCurrentPage = 1;
statsCurrentPage = 1;
} else if (manualPageDownRequested) {
osdShowStats(statsSinglePageCompatible, 0);
statsCurrentPage = 0;
Expand All @@ -4612,7 +4612,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
}

// Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
// Time elapsed or canceled by stick commands.
// Exit to normal OSD operation.
displayClearScreen(osdDisplayPort);
Expand All @@ -4622,7 +4622,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
// Continue "Splash", "Armed" or "Stats" screens.
displayHeartbeat(osdDisplayPort);
}

return;
}

Expand Down Expand Up @@ -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);
}
Expand Down
23 changes: 20 additions & 3 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 4f396a3

Please sign in to comment.