diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 741308755c..b593bddcc6 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -114,7 +114,6 @@ void processAirmode(void) { } else if (STATE(MULTIROTOR)) { processAirmodeMultirotor(); } - } bool isUsingNavigationModes(void) @@ -122,6 +121,21 @@ bool isUsingNavigationModes(void) return isUsingNAVModes; } +bool isFwAutoModeActive(boxId_e mode) +{ + /* Sets activation priority of fixed wing auto tune/trim modes: Autotune -> Autotrim -> Autolevel */ + + if (mode == BOXAUTOTUNE) { + return IS_RC_MODE_ACTIVE(BOXAUTOTUNE); + } else if (mode == BOXAUTOTRIM) { + return IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE); + } else if (mode == BOXAUTOLEVEL) { + return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && !IS_RC_MODE_ACTIVE(BOXAUTOTRIM); + } + + return false; +} + bool IS_RC_MODE_ACTIVE(boxId_e boxId) { return bitArrayGet(rcModeActivationMask.bits, boxId); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 04aea681bc..ad2c442225 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -140,3 +140,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(void); void updateUsedModeActivationConditionFlags(void); +bool isFwAutoModeActive(boxId_e mode); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 98aa47ebbf..f1b2f56781 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1385,7 +1385,7 @@ pidBank_t * pidBankMutable(void) { bool isFixedWingLevelTrimActive(void) { - return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() && + return isFwAutoModeActive(BOXAUTOLEVEL) && !areSticksDeflected() && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) && !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel); @@ -1409,7 +1409,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) previousArmingState = ARMING_FLAG(ARMED); // return if not active or disarmed - if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) { + if (!isFwAutoModeActive(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) { return; } diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index af6896f423..c4be8101bc 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -40,6 +40,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_controls.h" +#include "fc/rc_modes.h" #include "fc/rc_adjustments.h" #include "fc/runtime_config.h" #include "fc/settings.h" @@ -130,7 +131,7 @@ void autotuneStart(void) void autotuneUpdateState(void) { - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) { + if (isFwAutoModeActive(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) { if (!FLIGHT_MODE(AUTO_TUNE)) { autotuneStart(); ENABLE_FLIGHT_MODE(AUTO_TUNE); @@ -202,7 +203,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) { if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode - + // Target 80% control surface deflection to leave some room for P and I to work float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index f38a4ea108..f88f4cd2cf 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -82,7 +82,7 @@ void Reset_servoMixers(servoMixer_t *instance) #ifdef USE_PROGRAMMING_FRAMEWORK ,.conditionId = -1 #endif - ); + ); } } @@ -96,7 +96,7 @@ void pgResetFn_servoParams(servoParam_t *instance) .max = DEFAULT_SERVO_MAX, .middle = DEFAULT_SERVO_MIDDLE, .rate = 100 - ); + ); } } @@ -194,7 +194,7 @@ void servosInit(void) } int getServoCount(void) -{ +{ if (mixerUsesServos) { return 1 + maxServoIndex - minServoIndex; } @@ -246,7 +246,7 @@ static void filterServos(void) void writeServos(void) { filterServos(); - + #if !defined(SITL_BUILD) int servoIndex = 0; bool zeroServoValue = false; @@ -449,7 +449,7 @@ void processServoAutotrimMode(void) static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS]; static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS]; - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { + if (isFwAutoModeActive(BOXAUTOTRIM)) { switch (trimState) { case AUTOTRIM_IDLE: if (ARMING_FLAG(ARMED)) { @@ -544,7 +544,7 @@ void processServoAutotrimMode(void) void processContinuousServoAutotrim(const float dT) { static timeMs_t lastUpdateTimeMs; - static servoAutotrimState_e trimState = AUTOTRIM_IDLE; + static servoAutotrimState_e trimState = AUTOTRIM_IDLE; static uint32_t servoMiddleUpdateCount; const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT); @@ -556,16 +556,16 @@ void processContinuousServoAutotrim(const float dT) const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; const bool sticksAreCentered = !areSticksDeflected(); - const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT + const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT; if ( - planeIsFlyingStraight && - noRotationCommanded && + planeIsFlyingStraight && + noRotationCommanded && planeIsFlyingLevel && sticksAreCentered && - !FLIGHT_MODE(MANUAL_MODE) && + !FLIGHT_MODE(MANUAL_MODE) && isGPSHeadingValid() // TODO: proper flying detection - ) { + ) { // Plane is flying straight and level: trim servos for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { // For each stabilized axis, add 5 units of I-term to all associated servo midpoints @@ -610,7 +610,7 @@ void processContinuousServoAutotrim(const float dT) DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered)); DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); - DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); + DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); } void processServoAutotrim(const float dT) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 43e7453033..7d69b4b708 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -525,7 +525,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) buff[0] = ' '; } -#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values if (isDJICompatibleVideoSystem(osdConfig())) { totalDigits++; digits++; @@ -620,7 +620,7 @@ 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) @@ -631,7 +631,7 @@ char *osdFormatTrimWhiteSpace(char *buff) while(isspace((unsigned char)*buff)) buff++; // All spaces? - if(*buff == 0) + if(*buff == 0) return buff; // Trim trailing spaces @@ -1098,7 +1098,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]); @@ -1322,7 +1322,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen } } - if (STATE(GPS_FIX) + if (STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif @@ -1841,7 +1841,7 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_ESTIMATED_FIX)) { strcpy(buff + 2, "ES"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else + } else #endif if (!STATE(GPS_FIX)) { hardwareSensorStatus_e sensorStatus = getHwGPSStatus(); @@ -1900,10 +1900,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIR: { - if ((STATE(GPS_FIX) + if ((STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) -#endif +#endif ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); @@ -2380,8 +2380,9 @@ static bool osdDrawSingleElement(uint8_t item) p = " WP "; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { // If navigationRequiresAngleMode() returns false when ALTHOLD is active, - // it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc... + // it means it can be combined with ANGLE, HORIZON, ACRO, etc... // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. + // (Currently only applies to multirotor). p = " AH "; } else if (FLIGHT_MODE(ANGLE_MODE)) @@ -2540,18 +2541,18 @@ static bool osdDrawSingleElement(uint8_t item) osdCrosshairPosition(&elemPosX, &elemPosY); osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && (STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION + if (osdConfig()->hud_homing && (STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) -#endif +#endif ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { osdHudDrawHoming(elemPosX, elemPosY); } - if ((STATE(GPS_FIX) -#ifdef USE_GPS_FIX_ESTIMATION + if ((STATE(GPS_FIX) +#ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) -#endif +#endif ) && isImuHeadingValid()) { if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { @@ -3176,10 +3177,10 @@ static bool osdDrawSingleElement(uint8_t item) digits = 4U; } #endif - if ((STATE(GPS_FIX) + if ((STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) -#endif +#endif ) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, @@ -3202,7 +3203,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff + strlen(buff), "%c", 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 DJICOMPAT mode buff[digits + 1] = SYM_MAH_MI_1; buff[digits + 2] = '\0'; @@ -3216,7 +3217,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff + strlen(buff), "%c", 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'; @@ -3232,7 +3233,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff + strlen(buff), "%c", 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'; @@ -3407,7 +3408,7 @@ static bool osdDrawSingleElement(uint8_t item) STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier); int digits = osdConfig()->plus_code_digits; int digitsRemoved = osdConfig()->plus_code_short * 2; - if ((STATE(GPS_FIX) + if ((STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif @@ -4214,7 +4215,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); else displayWrite(osdDisplayPort, statNameX, row, "ODOMETER"); - + switch (osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; @@ -4227,7 +4228,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool tfp_sprintf(string_buffer, ": %d", statTotalDist); buffLen = 3 + sizeof(statTotalDist); } - + string_buffer[buffLen++] = SYM_MI; break; default: @@ -4253,8 +4254,8 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER); tfp_sprintf(string_buffer, ": %d", statTotalDist); buffLen = 3 + sizeof(statTotalDist); - } - + } + string_buffer[buffLen++] = SYM_KM; break; } @@ -4265,13 +4266,13 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); else displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME"); - + uint32_t tot_mins = statsConfig()->stats_total_time / 60; if (isBootStats) tfp_sprintf(string_buffer, "%d:%02dH:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0'); else tfp_sprintf(string_buffer, ": %d:%02d H:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0'); - + displayWrite(osdDisplayPort, statValueX-(isBootStats ? 7 : 0), row, string_buffer); #ifdef USE_ADC @@ -4496,7 +4497,7 @@ static void osdUpdateStats(void) if (escTemperatureValid) { if (stats.min_esc_temp > escSensor->temperature) stats.min_esc_temp = escSensor->temperature; - + if (stats.max_esc_temp < escSensor->temperature) stats.max_esc_temp = escSensor->temperature; } @@ -4570,7 +4571,7 @@ uint8_t drawStat_MaxDistanceFromHome(uint8_t col, uint8_t row, uint8_t statValX) } else { displayWrite(osdDisplayPort, col, row, "MAX DISTANCE FROM "); valueXOffset = 18; - } + } displayWriteChar(osdDisplayPort, col + valueXOffset, row, SYM_HOME); tfp_sprintf(buff, ": "); osdFormatDistanceStr(buff + 2, stats.max_distance * 100); @@ -4586,10 +4587,10 @@ uint8_t drawStat_Speed(uint8_t col, uint8_t row, uint8_t statValX) uint8_t multiValueXOffset = 0; displayWrite(osdDisplayPort, col, row, "MAX/AVG SPEED"); - + osdFormatVelocityStr(buff2, stats.max_3D_speed, true, false); tfp_sprintf(buff, ": %s/", osdFormatTrimWhiteSpace(buff2)); - multiValueXOffset = strlen(buff); + multiValueXOffset = strlen(buff); displayWrite(osdDisplayPort, statValX, row, buff); osdGenerateAverageVelocityStr(buff2); @@ -4622,8 +4623,8 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX) tfp_sprintf(buff, ": "); osdFormatCentiNumber(buff + 2, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2, false); strcat(osdFormatTrimWhiteSpace(buff), "/"); - multiValueXOffset = strlen(buff); - // AverageCell + multiValueXOffset = strlen(buff); + // AverageCell osdFormatCentiNumber(buff + multiValueXOffset, stats.min_voltage / getBatteryCellCount(), 0, 2, 0, 3, false); tfp_sprintf(buff + strlen(buff), "%c", SYM_VOLT); @@ -4655,7 +4656,7 @@ uint8_t drawStat_MaximumPowerAndCurrent(uint8_t col, uint8_t row, uint8_t statVa uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; - + if (osdDisplayIsHD()) displayWrite(osdDisplayPort, col, row, "USED ENERGY FLT/TOT"); else @@ -4684,7 +4685,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b int32_t totalDistance = getTotalTravelDistance(); bool moreThanAh = false; bool efficiencyValid = totalDistance >= 10000; - + if (osdDisplayIsHD()) displayWrite(osdDisplayPort, col, row, "AVG EFFICIENCY FLT/TOT"); else @@ -4708,18 +4709,18 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) + if (!moreThanAh) tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); - else + else tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI); - + moreThanAh = false; } strcat(outBuff, "/"); moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); - + if (!moreThanAh) tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); else @@ -4746,11 +4747,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn()-stats.flightStartMAh) * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) + if (!moreThanAh) tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); - else + else tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM); - + moreThanAh = false; } @@ -4791,11 +4792,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { - if (!moreThanAh) + if (!moreThanAh) tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); - else + else tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM); - + moreThanAh = false; } @@ -4848,7 +4849,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) tfp_sprintf(buff, ": "); itoa(stats.min_rssi, buff + 2, 10); strcat(osdFormatTrimWhiteSpace(buff), "%"); - + if (rxConfig()->serialrx_provider == SERIALRX_CRSF) { strcat(osdFormatTrimWhiteSpace(buff), "/"); multiValueXOffset = strlen(buff); @@ -4891,9 +4892,9 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; displayWrite(osdDisplayPort, col, row, "MIN/MAX ESC TEMP"); - tfp_sprintf(buff, ": %3d/%3d%c", - ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.min_esc_temp * 9 / 5.0f + 320) : stats.min_esc_temp), - ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.max_esc_temp * 9 / 5.0f + 320) : stats.max_esc_temp), + tfp_sprintf(buff, ": %3d/%3d%c", + ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.min_esc_temp * 9 / 5.0f + 320) : stats.min_esc_temp), + ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.max_esc_temp * 9 / 5.0f + 320) : stats.max_esc_temp), ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? SYM_TEMP_F : SYM_TEMP_C)); displayWrite(osdDisplayPort, statValX, row++, buff); @@ -4914,10 +4915,10 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) displayWrite(osdDisplayPort, col, row, "MAX G-FORCE"); else displayWrite(osdDisplayPort, col, row, "MAX/MIN Z/MAX Z G-FORCE"); - + tfp_sprintf(outBuff, ": "); osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3, false); - + if (!osdDisplayIsHD()) { strcat(outBuff, osdFormatTrimWhiteSpace(buff)); displayWrite(osdDisplayPort, statValX, row++, outBuff); @@ -4932,7 +4933,7 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); strcat(outBuff, "/"); - + osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); displayWrite(osdDisplayPort, statValX, row++, outBuff); @@ -4959,7 +4960,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) const uint8_t statNameX = (osdDisplayPort->cols - (osdDisplayIsHD() ? 41 : 28)) / 2; const uint8_t statValuesX = osdDisplayPort->cols - statNameX - (osdDisplayIsHD() ? 15 : 11); - + if (page > 1) page = 0; @@ -5033,9 +5034,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (feature(FEATURE_BLACKBOX)) { char buff[12]; displayWrite(osdDisplayPort, statNameX, row, "BLACKBOX FILE"); - + tfp_sprintf(buff, ": %u/%u", stats.min_sats, stats.max_sats); - + int32_t logNumber = blackboxGetLogNumber(); if (logNumber >= 0) tfp_sprintf(buff, ": %05ld ", logNumber); @@ -5053,7 +5054,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) break; } } - + row = drawStat_DisarmMethod(statNameX, row, statValuesX); // The following has been commented out as it will be added in #9688 @@ -5409,27 +5410,27 @@ static void osdRefresh(timeUs_t currentTimeUs) // 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; } } @@ -5472,7 +5473,7 @@ static void osdRefresh(timeUs_t currentTimeUs) displayHeartbeat(osdDisplayPort); isThrottleHigh = checkStickPosition(THR_HI); } - + return; } @@ -5641,24 +5642,20 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (buff != NULL) { const char *message = NULL; - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; //warning: shared buffer. Make sure it is used by single message in code below! - // We might have up to 5 messages to show. - const char *messages[5]; + /* WARNING: messageBuf is shared, use accordingly */ + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)]; + + /* WARNING: ensure number of messages returned does not exceed messages array size + * Messages array set 1 larger than maximum expected message count of 6 */ + const char *messages[7]; unsigned messageCount = 0; + const char *failsafeInfoMessage = NULL; const char *invertedInfoMessage = NULL; if (ARMING_FLAG(ARMED)) { -#ifdef USE_FW_AUTOLAND - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) { - if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) { -#else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - if (isWaypointMissionRTHActive()) { -#endif - // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); - } + /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */ if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) { messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED); } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { @@ -5680,36 +5677,27 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = messageBuf; } - } else if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { - uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); - tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec); - - messages[messageCount++] = messageBuf; } - else { -#ifdef USE_FW_AUTOLAND - if (canFwLandingBeCancelled()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); - } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) { -#endif - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } -#ifdef USE_FW_AUTOLAND + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; } -#endif } - #if defined(USE_SAFE_HOME) const char *safehomeMessage = divertingToSafehomeMessage(); if (safehomeMessage) { messages[messageCount++] = safehomeMessage; } #endif - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too + if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed + if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { + uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); + tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec); + + messages[messageCount++] = messageBuf; + } + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); failsafeInfoMessage = osdFailsafeInfoMessage(); @@ -5719,56 +5707,41 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (failsafeInfoMessage) { messages[messageCount++] = failsafeInfoMessage; } + } else if (isWaypointMissionRTHActive()) { + // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); } - } else { /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */ - if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { - messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) : - OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); - const char *launchStateMessage = fixedWingLaunchStateMessage(); - if (launchStateMessage) { - messages[messageCount++] = launchStateMessage; - } - } else { - if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { - // ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO - // when it doesn't require ANGLE mode (required only in FW - // right now). If it requires ANGLE, its display is handled 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)"); + } else if (STATE(LANDING_DETECTED)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); + } else { + /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ + /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ + if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ +#ifdef USE_FW_AUTOLAND + if (canFwLandingBeCancelled()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS); + } else +#endif + if (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) { + messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) : + OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); + const char *launchStateMessage = fixedWingLaunchStateMessage(); + if (launchStateMessage) { + messages[messageCount++] = launchStateMessage; } - messages[messageCount++] = messageBuf; - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + } else if (FLIGHT_MODE(SOARING_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING); + } else if (isFwAutoModeActive(BOXAUTOTUNE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); if (FLIGHT_MODE(MANUAL_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); } + } else if (isFwAutoModeActive(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); + } else if (isFixedWingLevelTrimActive()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); } - if (isFixedWingLevelTrimActive()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); - } - if (FLIGHT_MODE(HEADFREE_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); - } - if (FLIGHT_MODE(SOARING_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING); - } - if (posControl.flags.wpMissionPlannerActive) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER); - } - if (STATE(LANDING_DETECTED)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); - } + if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) { int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis(); if (isAngleHoldLevel()) { @@ -5779,33 +5752,48 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } + } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ + if (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; + } else if (FLIGHT_MODE(HEADFREE_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); + } + if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) { + /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes + * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field. + * In this case indicate ALTHOLD is active via a system message */ + + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + } } } - } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ unsigned invalidIndex; // Check if we're unable to arm for some reason if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) { + const setting_t *setting = settingGet(invalidIndex); + settingGetName(setting, messageBuf); + for (int ii = 0; messageBuf[ii]; ii++) { + messageBuf[ii] = sl_toupper(messageBuf[ii]); + } + invertedInfoMessage = messageBuf; + messages[messageCount++] = invertedInfoMessage; - const setting_t *setting = settingGet(invalidIndex); - settingGetName(setting, messageBuf); - for (int ii = 0; messageBuf[ii]; ii++) { - messageBuf[ii] = sl_toupper(messageBuf[ii]); - } - invertedInfoMessage = messageBuf; - messages[messageCount++] = invertedInfoMessage; - - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - messages[messageCount++] = invertedInfoMessage; - + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); + messages[messageCount++] = invertedInfoMessage; } else { - - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - messages[messageCount++] = invertedInfoMessage; - - // Show the reason for not arming - messages[messageCount++] = osdArmingDisabledReasonMessage(); - + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); + messages[messageCount++] = invertedInfoMessage; + // Show the reason for not arming + messages[messageCount++] = osdArmingDisabledReasonMessage(); } } else if (!ARMING_FLAG(ARMED)) { if (isWaypointListValid()) { @@ -5814,6 +5802,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } /* Messages that are shown regardless of Arming state */ + /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL NORMALLY, 1 MESSAGE DURING FAILSAFE */ + if (posControl.flags.wpMissionPlannerActive && !FLIGHT_MODE(FAILSAFE_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER); + } // The following has been commented out as it will be added in #9688 // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0; @@ -5844,7 +5836,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (message == invertedInfoMessage) { TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); } - // We're shoing either failsafePhaseMessage or + // We're showing either failsafePhaseMessage or // navStateMessage. Don't BLINK here since // having this text available might be crucial // during a lost aircraft recovery and blinking