diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index d482635db2..14da5ffd69 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -190,8 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("TIME (HOUR)", OSD_RTC_TIME), OSD_ELEMENT_ENTRY("FLY MODE", OSD_FLYMODE), OSD_ELEMENT_ENTRY("NAME", OSD_CRAFT_NAME), - OSD_ELEMENT_ENTRY("THR. (MANU)", OSD_THROTTLE_POS), - OSD_ELEMENT_ENTRY("THR. (MANU/AUTO)", OSD_THROTTLE_POS_AUTO_THR), + OSD_ELEMENT_ENTRY("THR. ", OSD_THROTTLE_POS), + OSD_ELEMENT_ENTRY("THR. (SCALED)", OSD_SCALED_THROTTLE_POS), OSD_ELEMENT_ENTRY("SYS MESSAGES", OSD_MESSAGES), OSD_ELEMENT_ENTRY("VTX CHAN", OSD_VTX_CHANNEL), OSD_ELEMENT_ENTRY("CURRENT (A)", OSD_CURRENT_DRAW), diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 93c43b419a..5427f94a33 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -834,7 +834,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, getFlightTime()); // Flight time (seconds) // Throttle - sbufWriteU8(dst, getThrottlePercent()); // Throttle Percent + sbufWriteU8(dst, getThrottlePercent(true)); // Throttle Percent sbufWriteU8(dst, navigationIsControllingThrottle() ? 1 : 0); // Auto Throttle Flag (0 or 1) break; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e326dbe4c2..00142fea55 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -612,9 +612,16 @@ void FAST_CODE mixTable() } } -int16_t getThrottlePercent(void) +int16_t getThrottlePercent(bool useScaled) { - int16_t thr = (constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX ) - getThrottleIdleValue()) * 100 / (motorConfig()->maxthrottle - getThrottleIdleValue()); + int16_t thr = constrain(rcCommand[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); + const int idleThrottle = getThrottleIdleValue(); + + if (useScaled) { + thr = (thr - idleThrottle) * 100 / (motorConfig()->maxthrottle - idleThrottle); + } else { + thr = (rxGetChannelValue(THROTTLE) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN); + } return thr; } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index cfb8fb9a60..f638d7335f 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -111,7 +111,7 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; int getThrottleIdleValue(void); -int16_t getThrottlePercent(void); +int16_t getThrottlePercent(bool); uint8_t getMotorCount(void); float getMotorMixRange(void); bool mixerIsOutputSaturated(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f2c0341d35..bd03644e80 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1079,26 +1079,46 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) } /** - * Formats throttle position prefixed by its symbol. - * Shows output to motor, not stick position - **/ -static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr) + * Check if this OSD layout is using scaled or unscaled throttle. + * If both are used, it will default to scaled. + */ +bool osdUsingScaledThrottle() { - buff[0] = SYM_BLANK; + bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]); + bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]); + + if (!usingScaledThrottle && !usingRCThrottle) + usingScaledThrottle = true; + + return usingScaledThrottle; +} + +/** + * Formats throttle position prefixed by its symbol. + * Shows unscaled or scaled (output to motor) throttle percentage + **/ +static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes_t *elemAttr) +{ + if (useScaled) { + buff[0] = SYM_SCALE; + } else { + buff[0] = SYM_BLANK; + } buff[1] = SYM_THR; - if (autoThr && navigationIsControllingThrottle()) { + if (navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; if (isFixedWingAutoThrottleManuallyIncreased()) { TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } + useScaled = true; } #ifdef USE_POWER_LIMITS if (powerLimiterIsLimiting()) { TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); } #endif - tfp_sprintf(buff + 2, "%3d", getThrottlePercent()); + tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); } /** @@ -2893,7 +2913,7 @@ static bool osdDrawSingleElement(uint8_t item) return true; } - case OSD_THROTTLE_POS_AUTO_THR: + case OSD_SCALED_THROTTLE_POS: { osdFormatThrottlePosition(buff, true, &elemAttr); break; @@ -3704,7 +3724,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2); osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); + osdLayoutsConfig->item_pos[0][OSD_SCALED_THROTTLE_POS] = OSD_POS(6, 2); osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2); osdLayoutsConfig->item_pos[0][OSD_GROUND_COURSE] = OSD_POS(12, 3); osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4bae42847e..0af4352ed2 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -162,7 +162,7 @@ typedef enum { OSD_MESSAGES, OSD_GPS_HDOP, OSD_MAIN_BATT_CELL_VOLTAGE, - OSD_THROTTLE_POS_AUTO_THR, + OSD_SCALED_THROTTLE_POS, OSD_HEADING_GRAPH, OSD_EFFICIENCY_MAH_PER_KM, OSD_WH_DRAWN, @@ -476,6 +476,8 @@ displayCanvas_t *osdGetDisplayPortCanvas(void); int16_t osdGetHeading(void); int32_t osdGetAltitude(void); +bool osdUsingScaledThrottle(void); + void osdStartedSaveProcess(void); void osdShowEEPROMSavedNotification(void); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 294a5fd5c2..957f998c5e 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -1110,7 +1110,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst) activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE; } - if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) { + if (OSD_VISIBLE(osdLayoutConfig[OSD_SCALED_THROTTLE_POS])) { activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR; } diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 3ba09eba38..4abcc1a21c 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "io/gps.h" +#include "io/osd.h" #include "io/serial.h" #include "navigation/navigation.h" @@ -195,7 +196,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); if (ARMING_FLAG(ARMED)) { - uint16_t throttleForRPM = getThrottlePercent() / BLADE_NUMBER_DIVIDER; + uint16_t throttleForRPM = getThrottlePercent(osdUsingScaledThrottle()) / BLADE_NUMBER_DIVIDER; serialize16(throttleForRPM); } else { serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER)); diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 8e29337986..f0819f7cb1 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -33,6 +33,7 @@ #include "fc/runtime_config.h" #include "scheduler/scheduler.h" +#include "io/osd.h" #include "io/serial.h" #include "sensors/barometer.h" @@ -147,7 +148,7 @@ static uint8_t dispatchMeasurementRequest(ibusAddress_t address) { if (!temp_valid || (temperature < -400)) temperature = -400; // Minimum reported temperature is -40°C return sendIbusMeasurement2(address, (uint16_t)(temperature + IBUS_TEMPERATURE_OFFSET)); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_RPM) { - return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent() ); + return sendIbusMeasurement2(address, (uint16_t)getThrottlePercent(osdUsingScaledThrottle()) ); } else if (SENSOR_ADDRESS_TYPE_LOOKUP[address].value == IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE) { //VBAT if (telemetryConfig()->report_cell_voltage) { return sendIbusMeasurement2(address, getBatteryAverageCellVoltage()); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 86d4fa9111..90c1377bc8 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -653,7 +653,7 @@ void mavlinkSendHUDAndHeartbeat(void) mavAltitude = getEstimatedActualPosition(Z) / 100.0f; mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f; - int16_t thr = getThrottlePercent(); + int16_t thr = getThrottlePercent(osdUsingScaledThrottle()); mavlink_msg_vfr_hud_pack(mavSystemId, mavComponentId, &mavSendMsg, // airspeed Current airspeed in m/s mavAirSpeed,