diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f4fe19a4040..2b3bea79025 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -228,6 +228,59 @@ static bool osdDisplayHasCanvas; PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3); +/* OSD formatting helpers replacing common tfp_sprintf patterns + * for reduced code size and CPU overhead. */ + +static int i2a_len(int num, char *bf) +{ + char *start = bf; + if (num < 0) { + num = -num; + *bf++ = '-'; + } + ui2a(num, 10, 0, bf); + while (*bf) bf++; + return (int)(bf - start); +} + +static void osdFormatIntUnit(char *buff, int width, int value, char unit) +{ + char tmp[12]; + int len = i2a_len(value, tmp); + int pad = width - len; + int i = 0; + while (pad-- > 0) buff[i++] = ' '; + const char *src = tmp; + while (*src) buff[i++] = *src++; + if (unit) buff[i++] = unit; + buff[i] = '\0'; +} + +static inline void osdWriteChar(char *buff, char c) +{ + buff[0] = c; + buff[1] = '\0'; +} + +static inline void osdWriteChar2(char *buff, char c1, char c2) +{ + buff[0] = c1; + buff[1] = c2; + buff[2] = '\0'; +} + +static void osdFormatTime_MMSS(char *buff, int m, int s) +{ + m %= 100; // clamp to two digits to prevent writing non-ASCII to display + s %= 100; + buff[0] = '0' + m / 10; + buff[1] = '0' + m % 10; + buff[2] = ':'; + buff[3] = '0' + s / 10; + buff[4] = '0' + s % 10; + buff[5] = '\0'; +} + void osdStartedSaveProcess(void) { savingSettings = true; } @@ -660,7 +713,7 @@ static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h) value = seconds / 60; } buff[0] = sym; - tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); + osdFormatTime_MMSS(buff + 1, (int)(value / 60), (int)(value % 60)); } static inline void osdFormatOnTime(char *buff) @@ -771,7 +824,7 @@ static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t s if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320; - tfp_sprintf(buff, "%3d", temperature / 10); + osdFormatIntUnit(buff, 3, temperature / 10, 0); } else strcpy(buff, "---"); @@ -1205,7 +1258,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes strcpy(buff + 1, message); return; } - tfp_sprintf(buff + 2, "%3d", throttlePercent); + osdFormatIntUnit(buff + 2, 3, throttlePercent, 0); } /** @@ -1653,25 +1706,25 @@ static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const } elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); + osdFormatIntUnit(buff, 3, pid->P, 0); if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->I); + osdFormatIntUnit(buff, 3, pid->I, 0); if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->D); + osdFormatIntUnit(buff, 3, pid->D, 0); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->FF); + osdFormatIntUnit(buff, 3, pid->FF, 0); if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr); @@ -1695,19 +1748,19 @@ static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const cha } elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); + osdFormatIntUnit(buff, 3, pid->P, 0); if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->I); + osdFormatIntUnit(buff, 3, pid->I, 0); if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + osdFormatIntUnit(buff, 3, pidType == PID_TYPE_PIFF ? pid->FF : pid->D, 0); if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); @@ -1849,7 +1902,7 @@ static bool osdDrawSingleElement(uint8_t item) uint8_t osdRssi = osdConvertRSSI(); buff[0] = SYM_RSSI; if (osdRssi < 100) - tfp_sprintf(buff + 1, "%2d", osdRssi); + osdFormatIntUnit(buff + 1, 2, osdRssi, 0); else tfp_sprintf(buff + 1, "%c ", SYM_MAX); @@ -1899,7 +1952,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it if (isDJICompatibleVideoSystem(osdConfig())) { //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with - tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah + osdFormatIntUnit(buff, 5, (int)getMAhDrawn(), 0); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; } else @@ -1940,7 +1993,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it if (isDJICompatibleVideoSystem(osdConfig())) { //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with - tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity()); // Use 5 digits to allow packs below 100Ah + osdFormatIntUnit(buff, 5, (int)getBatteryRemainingCapacity(), 0); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; unitsDrawn = true; @@ -1979,7 +2032,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER_SUPPLY_IMPEDANCE: if (isPowerSupplyImpedanceValid()) - tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); + osdFormatIntUnit(buff, 3, getPowerSupplyImpedance(), 0); else strcpy(buff, "---"); buff[3] = SYM_MILLIOHM; @@ -1990,7 +2043,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GPS_SATS: buff[0] = SYM_SAT_L; buff[1] = SYM_SAT_R; - tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); + osdFormatIntUnit(buff + 2, 2, gpsSol.numSat, 0); #ifdef USE_GPS_FIX_ESTIMATION if (STATE(GPS_ESTIMATED_FIX)) { strcpy(buff + 2, "ES"); @@ -2093,7 +2146,7 @@ static bool osdDrawSingleElement(uint8_t item) if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) { int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - (STATE(AIRPLANE) ? posControl.actualState.cog : DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))))); - tfp_sprintf(buff + 2, "%4d", h); + osdFormatIntUnit(buff + 2, 4, h, 0); } else { strcpy(buff + 2, "----"); } @@ -2126,7 +2179,7 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_BLACKBOX if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) { if (!isBlackboxDeviceWorking()) { - tfp_sprintf(buff, "%c%c", SYM_BLACKBOX, SYM_ALERT); + osdWriteChar2(buff, SYM_BLACKBOX, SYM_ALERT); } else if (isBlackboxDeviceFull()) { tfp_sprintf(buff, "%cFULL", SYM_BLACKBOX); } else { @@ -2134,7 +2187,7 @@ static bool osdDrawSingleElement(uint8_t item) if (logNumber >= 0) { tfp_sprintf(buff, "%c%05" PRId32, SYM_BLACKBOX, logNumber); } else { - tfp_sprintf(buff, "%c", SYM_BLACKBOX); + osdWriteChar(buff, SYM_BLACKBOX); } } } @@ -2178,7 +2231,7 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_GROUND_COURSE; if (osdIsHeadingValid()) { - tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); + osdFormatIntUnit(&buff[1], 3, (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog), 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -2203,7 +2256,7 @@ static bool osdDrawSingleElement(uint8_t item) if (ABS(herr) > 99) strcpy(buff + 1, ">99"); else - tfp_sprintf(buff + 1, "%3d", herr); + osdFormatIntUnit(buff + 1, 3, herr, 0); } buff[4] = SYM_DEGREES; @@ -2225,7 +2278,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!ARMING_FLAG(ARMED)) { buff[1] = buff[2] = buff[3] = buff[4] = '-'; } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - tfp_sprintf(buff + 1, "%4d", heading_adjust); + osdFormatIntUnit(buff + 1, 4, heading_adjust, 0); } buff[5] = SYM_DEGREES; @@ -2372,7 +2425,7 @@ static bool osdDrawSingleElement(uint8_t item) } else if (!isImuHeadingValid()) { buff[1] = 'H'; } else { - tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount()); + osdFormatIntUnit(buff + 1, 1, getActiveVehiclesCount(), 0); } break; } @@ -2639,7 +2692,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + osdWriteChar(buff, osdInfo.powerIndexLetter); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); return true; @@ -2651,10 +2704,10 @@ static bool osdDrawSingleElement(uint8_t item) vtxDeviceOsdInfo_t osdInfo; vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "%c", SYM_VTX_POWER); + osdWriteChar(buff, SYM_VTX_POWER); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + osdWriteChar(buff, osdInfo.powerIndexLetter); if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); return true; @@ -2665,11 +2718,7 @@ static bool osdDrawSingleElement(uint8_t item) { int16_t rssi = rxLinkStatistics.uplinkRSSI; buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna - if (rssi <= -100) { - tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); - } else { - tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM); - } + osdFormatIntUnit(buff + 1, 4, rssi, SYM_DBM); if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { @@ -2695,18 +2744,18 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_CRSF_LQ_TYPE3: if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); + osdFormatIntUnit(buff+1, 3, 0, 0); } else { int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ, 0); } break; case OSD_CRSF_LQ_TYPE1: default: if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); + osdFormatIntUnit(buff+1, 3, 0, 0); } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.uplinkLQ, 0); } break; } @@ -2722,9 +2771,9 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_LQ; if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN); + osdFormatIntUnit(buff+1, 3, 0, SYM_AH_DECORATION_DOWN); } else { - tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); + osdFormatIntUnit(buff+1, 3, rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); } if (!failsafeIsReceivingRxData()) { @@ -2752,7 +2801,7 @@ static bool osdDrawSingleElement(uint8_t item) } else if (snrVal <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; if (snrVal <= -10) { - tfp_sprintf(buff + 1, "%3d%c", snrVal, SYM_DB); + osdFormatIntUnit(buff + 1, 3, snrVal, SYM_DB); } else { tfp_sprintf(buff + 1, "%2d%c%c", snrVal, SYM_DB, ' '); } @@ -2765,7 +2814,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!failsafeIsReceivingRxData()) tfp_sprintf(buff, "%s%c", " ", SYM_MW); else - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + osdFormatIntUnit(buff, 4, rxLinkStatistics.uplinkTXPower, SYM_MW); break; } @@ -2889,7 +2938,7 @@ static bool osdDrawSingleElement(uint8_t item) } altc = ABS(constrain(altc, -999, 999)); - tfp_sprintf(buff, "%3d", altc); + osdFormatIntUnit(buff, 3, altc, 0); displayWrite(osdDisplayPort, elemPosX + 1, elemPosY + 2, buff); return true; @@ -3098,7 +3147,7 @@ static bool osdDrawSingleElement(uint8_t item) // mAh/foot if (efficiencyValid) { osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3, false); - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_FT_0, SYM_AH_V_FT_1); + osdWriteChar2(buff + strlen(buff), SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_FT_0; @@ -3112,7 +3161,7 @@ static bool osdDrawSingleElement(uint8_t item) // mAh/metre if (efficiencyValid) { osdFormatCentiNumber(buff, value, 1, 2, 2, 3, false); - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_M_0, SYM_AH_V_M_1); + osdWriteChar2(buff + strlen(buff), SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; buff[3] = SYM_AH_V_M_0; @@ -3135,7 +3184,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[4] = SYM_DECORATION; buff[5] = SYM_DECORATION; } else { - tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); + osdFormatTime_MMSS(buff + 1, (int)(glideTime / 60), (int)(glideTime % 60)); } } else { tfp_sprintf(buff + 1, "%s", "--:--"); @@ -3191,7 +3240,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, -panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); @@ -3207,7 +3256,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); @@ -3215,7 +3264,7 @@ static bool osdDrawSingleElement(uint8_t item) panServoTimeOffCentre = 0; if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); } displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); @@ -3294,7 +3343,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_PITCH]); + osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_PITCH], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3304,7 +3353,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_ROLL]); + osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_ROLL], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3326,7 +3375,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_PITCH]); + osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_PITCH], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3336,7 +3385,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_ROLL]); + osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_ROLL], 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3398,11 +3447,11 @@ static bool osdDrawSingleElement(uint8_t item) const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); strcpy(buff, "POSO "); // display requested velocity cm/s - tfp_sprintf(buff + 5, "%4d", (int)lrintf(nav_pids->pos[X].output_constrained * 100)); + osdFormatIntUnit(buff + 5, 4, (int)lrintf(nav_pids->pos[X].output_constrained * 100), 0); buff[9] = ' '; - tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); + osdFormatIntUnit(buff + 10, 4, (int)lrintf(nav_pids->pos[Y].output_constrained * 100), 0); buff[14] = ' '; - tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); + osdFormatIntUnit(buff + 15, 4, (int)lrintf(nav_pids->pos[Z].output_constrained * 100), 0); buff[19] = '\0'; break; } @@ -3515,7 +3564,7 @@ static bool osdDrawSingleElement(uint8_t item) if (h < 0) { h += 360; } - tfp_sprintf(&buff[1], "%3d", h); + osdFormatIntUnit(&buff[1], 3, h, 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -3575,9 +3624,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_MI_0, SYM_MAH_MI_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_MI); + osdWriteChar(buff + strlen(buff), SYM_AH_MI); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3589,9 +3638,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_GA: moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_NM_0, SYM_MAH_NM_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_NM); + osdWriteChar(buff + strlen(buff), SYM_AH_NM); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3605,9 +3654,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits, false); if (!moreThanAh) { - tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); + osdWriteChar2(buff + strlen(buff), SYM_MAH_KM_0, SYM_MAH_KM_1); } else { - tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_KM); + osdWriteChar(buff + strlen(buff), SYM_AH_KM); } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; @@ -3818,7 +3867,7 @@ static bool osdDrawSingleElement(uint8_t item) else h = h + 180; - tfp_sprintf(&buff[1], "%3d", h); + osdFormatIntUnit(&buff[1], 3, h, 0); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -3948,7 +3997,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlProfile->throttle.dynPID); + osdFormatIntUnit(buff, 3, currentControlProfile->throttle.dynPID, 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -3956,7 +4005,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%4d", currentControlProfile->throttle.pa_breakpoint); + osdFormatIntUnit(buff, 4, currentControlProfile->throttle.pa_breakpoint, 0); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -4646,7 +4695,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE); @@ -4659,7 +4708,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool default: case OSD_UNIT_GA: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE); @@ -4673,7 +4722,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool FALLTHROUGH; case OSD_UNIT_METRIC: if (isBootStats) { - tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER), 0); buffLen = 5; } else { uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER); @@ -5051,7 +5100,7 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX) multiValueXOffset = strlen(buff); // AverageCell osdFormatCentiNumber(buff + multiValueXOffset, stats.min_voltage / getBatteryCellCount(), 0, 2, 0, 3, false); - tfp_sprintf(buff + strlen(buff), "%c", SYM_VOLT); + osdWriteChar(buff + strlen(buff), SYM_VOLT); displayWrite(osdDisplayPort, statValX, row++, buff); @@ -5096,7 +5145,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) strcat(buff, "/"); osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false); strcat(buff, osdFormatTrimWhiteSpace(preBuff)); - tfp_sprintf(buff + strlen(buff), "%c", SYM_WH); + osdWriteChar(buff + strlen(buff), SYM_WH); } displayWrite(osdDisplayPort, statValX, row++, buff); @@ -5135,9 +5184,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1); else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI); moreThanAh = false; } @@ -5147,9 +5196,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1); else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI); } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1); } @@ -5163,7 +5212,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_MI); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_MI); } break; case OSD_UNIT_GA: @@ -5173,9 +5222,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1); else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM); moreThanAh = false; } @@ -5184,9 +5233,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (!moreThanAh) { - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1); } else { - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM); } } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1); @@ -5201,7 +5250,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_NM); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_NM); } break; case OSD_UNIT_METRIC_MPH: @@ -5218,9 +5267,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (osdDisplayIsHD()) { if (!moreThanAh) - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1); else - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM); moreThanAh = false; } @@ -5229,9 +5278,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); if (!moreThanAh) { - tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); + osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1); } else { - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM); + osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM); } } else { tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1); @@ -5246,11 +5295,10 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b } else { strcat(outBuff, "---/---"); } - tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_KM); + osdWriteChar(outBuff + strlen(outBuff), SYM_WH_KM); } } - tfp_sprintf(outBuff + strlen(outBuff), "%c", '\0'); displayWrite(osdDisplayPort, statValX, row++, outBuff); return row; @@ -5284,7 +5332,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) if (osdDisplayIsHD()) { strcat(osdFormatTrimWhiteSpace(buff), "/"); itoa(stats.min_rssi_dbm, buff + 2, 10); - tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM); + osdWriteChar(buff + strlen(buff), SYM_DBM); displayWrite(osdDisplayPort, statValX, row++, buff); } } @@ -5296,7 +5344,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX) memset(buff, '\0', strlen(buff)); tfp_sprintf(buff, ": "); itoa(stats.min_rssi_dbm, buff + 2, 10); - tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM); + osdWriteChar(buff + strlen(buff), SYM_DBM); displayWrite(osdDisplayPort, statValX, row++, buff); }