diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index e07529e106..6909304ab8 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -186,6 +186,45 @@ const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = { OSD_STAT_TOTAL_DIST, }; +// Format a float to the specified number of decimal places with optional rounding. +// OSD symbols can optionally be placed before and after the formatted number (use SYM_NONE for no symbol). +// The formatString can be used for customized formatting of the integer part. Follow the printf style. +// Pass an empty formatString for default. +int osdPrintFloat(char *buffer, char leadingSymbol, float value, char *formatString, unsigned decimalPlaces, bool round, char trailingSymbol) +{ + char mask[7]; + int pos = 0; + int multiplier = 1; + for (unsigned i = 0; i < decimalPlaces; i++) { + multiplier *= 10; + } + + value *= multiplier; + const int scaledValueAbs = ABS(round ? lrintf(value) : value); + const int integerPart = scaledValueAbs / multiplier; + const int fractionalPart = scaledValueAbs % multiplier; + + if (leadingSymbol != SYM_NONE) { + buffer[pos++] = leadingSymbol; + } + if (value < 0 && (integerPart || fractionalPart)) { + buffer[pos++] = '-'; + } + + pos += tfp_sprintf(buffer + pos, (strlen(formatString) ? formatString : "%01u"), integerPart); + if (decimalPlaces) { + tfp_sprintf((char *)&mask, ".%%0%uu", decimalPlaces); // builds up the format string to be like ".%03u" for decimalPlaces == 3 as an example + pos += tfp_sprintf(buffer + pos, mask, fractionalPart); + } + + if (trailingSymbol != SYM_NONE) { + buffer[pos++] = trailingSymbol; + } + buffer[pos] = '\0'; + + return pos; +} + void osdStatSetState(uint8_t statIndex, bool enabled) { if (enabled) { @@ -658,8 +697,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) return true; case OSD_STAT_MAX_ALTITUDE: { - const int alt = osdGetMetersToSelectedUnit(stats.max_altitude) / 10; - tfp_sprintf(buff, "%d.%d%c", alt / 10, alt % 10, osdGetMetersToSelectedUnitSymbol()); + osdPrintFloat(buff, SYM_NONE, osdGetMetersToSelectedUnit(stats.max_altitude) / 100.0f, "", 1, true, osdGetMetersToSelectedUnitSymbol()); osdDisplayStatisticLabel(displayRow, "MAX ALTITUDE", buff); return true; } @@ -692,19 +730,19 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) #endif case OSD_STAT_MIN_BATTERY: - tfp_sprintf(buff, "%d.%02d%c", stats.min_voltage / 100, stats.min_voltage % 100, SYM_VOLT); + osdPrintFloat(buff, SYM_NONE, stats.min_voltage / 100.0f, "", 2, true, SYM_VOLT); osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value? "MIN AVG CELL" : "MIN BATTERY", buff); return true; case OSD_STAT_END_BATTERY: - tfp_sprintf(buff, "%d.%02d%c", stats.end_voltage / 100, stats.end_voltage % 100, SYM_VOLT); + osdPrintFloat(buff, SYM_NONE, stats.end_voltage / 100.0f, "", 2, true, SYM_VOLT); osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value ? "END AVG CELL" : "END BATTERY", buff); return true; case OSD_STAT_BATTERY: { const uint16_t statsVoltage = getStatsVoltage(); - tfp_sprintf(buff, "%d.%02d%c", statsVoltage / 100, statsVoltage % 100, SYM_VOLT); + osdPrintFloat(buff, SYM_NONE, statsVoltage / 100.0f, "", 2, true, SYM_VOLT); osdDisplayStatisticLabel(displayRow, osdConfig()->stat_show_cell_value ? "AVG BATT CELL" : "BATTERY", buff); return true; } @@ -756,8 +794,7 @@ static bool osdDisplayStat(int statistic, uint8_t displayRow) #if defined(USE_ACC) case OSD_STAT_MAX_G_FORCE: if (sensors(SENSOR_ACC)) { - const int gForce = lrintf(stats.max_g_force * 10); - tfp_sprintf(buff, "%01d.%01dG", gForce / 10, gForce % 10); + osdPrintFloat(buff, SYM_NONE, stats.max_g_force, "", 1, true, 'G'); osdDisplayStatisticLabel(displayRow, "MAX G-FORCE", buff); return true; } diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index d47c274379..6c8412595a 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -354,3 +354,4 @@ bool osdElementVisible(uint16_t value); bool osdGetVisualBeeperState(void); statistic_t *osdGetStats(void); bool osdNeedsAccelerometer(void); +int osdPrintFloat(char *buffer, char leadingSymbol, float value, char *formatString, unsigned decimalPlaces, bool round, char trailingSymbol); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 4328bd09ef..5030e576a3 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -257,23 +257,19 @@ int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius) static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementType_e variantType) { - const int alt = ABS(osdGetMetersToSelectedUnit(altitudeCm) / 10); const char unitSymbol = osdGetMetersToSelectedUnitSymbol(); - - *buff++ = SYM_ALTITUDE; - if (altitudeCm < 0) { - *buff++ = '-'; - } + unsigned decimalPlaces; switch (variantType) { case OSD_ELEMENT_TYPE_2: // whole number altitude (no decimal places) - tfp_sprintf(buff, "%d%c", alt / 10, unitSymbol); + decimalPlaces = 0; break; case OSD_ELEMENT_TYPE_1: // one decimal place (default) default: - tfp_sprintf(buff, "%01d.%01d%c", alt / 10 , alt % 10, unitSymbol); + decimalPlaces = 1; break; } + osdPrintFloat(buff, SYM_ALTITUDE, osdGetMetersToSelectedUnit(altitudeCm) / 100.0f, "", decimalPlaces, true, unitSymbol); } #ifdef USE_GPS @@ -284,6 +280,8 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) // We show 7 decimals, so we need to use 12 characters: // eg: s-180.1234567z s=symbol, z=zero terminator, decimal separator between 0 and 1 + // NOTE: Don't use osdPrintFloat() for this. There are too many decimal places and float math doesn't have enough precision + int pos = 0; buff[pos++] = sym; if (val < 0) { @@ -296,14 +294,11 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) void osdFormatDistanceString(char *ptr, int distance, char leadingSymbol) { - const int convertedDistance = osdGetMetersToSelectedUnit(distance); + const float convertedDistance = osdGetMetersToSelectedUnit(distance); char unitSymbol; char unitSymbolExtended; int unitTransition; - if (leadingSymbol != SYM_NONE) { - *ptr++ = leadingSymbol; - } switch (osdConfig()->units) { case UNIT_IMPERIAL: unitTransition = 5280; @@ -317,16 +312,23 @@ void osdFormatDistanceString(char *ptr, int distance, char leadingSymbol) break; } + unsigned decimalPlaces; + float displayDistance; + char displaySymbol; if (convertedDistance < unitTransition) { - tfp_sprintf(ptr, "%d%c", convertedDistance, unitSymbol); + decimalPlaces = 0; + displayDistance = convertedDistance; + displaySymbol = unitSymbol; } else { - const int displayDistance = convertedDistance * 100 / unitTransition; - if (displayDistance >= 1000) { // >= 10 miles or km - 1 decimal place - tfp_sprintf(ptr, "%d.%d%c", displayDistance / 100, (displayDistance / 10) % 10, unitSymbolExtended); + displayDistance = convertedDistance / unitTransition; + displaySymbol = unitSymbolExtended; + if (displayDistance >= 10) { // >= 10 miles or km - 1 decimal place + decimalPlaces = 1; } else { // < 10 miles or km - 2 decimal places - tfp_sprintf(ptr, "%d.%02d%c", displayDistance / 100, displayDistance % 100, unitSymbolExtended); + decimalPlaces = 2; } } + osdPrintFloat(ptr, leadingSymbol, displayDistance, "", decimalPlaces, false, displaySymbol); } static void osdFormatPID(char * buff, const char * label, const pidf_t * pid) @@ -464,13 +466,13 @@ static uint8_t osdGetDirectionSymbolFromHeading(int heading) * Converts altitude based on the current unit system. * @param meters Value in meters to convert */ -int32_t osdGetMetersToSelectedUnit(int32_t meters) +float osdGetMetersToSelectedUnit(int32_t meters) { switch (osdConfig()->units) { case UNIT_IMPERIAL: - return (meters * 328) / 100; // Convert to feet / 100 + return meters * 3.28084f; // Convert to feet default: - return meters; // Already in metre / 100 + return meters; // Already in meters } } @@ -574,8 +576,8 @@ static void osdElementAltitude(osdElementParms_t *element) #ifdef USE_ACC static void osdElementAngleRollPitch(osdElementParms_t *element) { - const int angle = (element->item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll; - tfp_sprintf(element->buff, "%c%c%02d.%01d", (element->item == OSD_PITCH_ANGLE) ? SYM_PITCH : SYM_ROLL , angle < 0 ? '-' : ' ', abs(angle / 10), abs(angle % 10)); + const float angle = ((element->item == OSD_PITCH_ANGLE) ? attitude.values.pitch : attitude.values.roll) / 10.0f; + osdPrintFloat(element->buff, (element->item == OSD_PITCH_ANGLE) ? SYM_PITCH : SYM_ROLL, fabsf(angle), ((angle < 0) ? "-%02u" : " %02u"), 1, true, SYM_NONE); } #endif @@ -645,8 +647,7 @@ static void osdElementUpDownReference(osdElementParms_t *element) static void osdElementAverageCellVoltage(osdElementParms_t *element) { const int cellV = getBatteryAverageCellVoltage(); - element->buff[0] = osdGetBatterySymbol(cellV); - tfp_sprintf(element->buff + 1, "%d.%02d%c", cellV / 100, cellV % 100, SYM_VOLT); + osdPrintFloat(element->buff, osdGetBatterySymbol(cellV), cellV / 100.0f, "", 2, false, SYM_VOLT); } static void osdElementCompassBar(osdElementParms_t *element) @@ -757,8 +758,8 @@ static void osdElementCrosshairs(osdElementParms_t *element) static void osdElementCurrentDraw(osdElementParms_t *element) { - const int32_t amperage = getAmperage(); - tfp_sprintf(element->buff, "%3d.%02d%c", abs(amperage) / 100, abs(amperage) % 100, SYM_AMP); + const float amperage = fabsf(getAmperage() / 100.0f); + osdPrintFloat(element->buff, SYM_NONE, amperage, "%3u", 2, false, SYM_AMP); } static void osdElementDebug(osdElementParms_t *element) @@ -907,8 +908,7 @@ static void osdElementFlymode(osdElementParms_t *element) #ifdef USE_ACC static void osdElementGForce(osdElementParms_t *element) { - const int gForce = lrintf(osdGForce * 10); - tfp_sprintf(element->buff, "%01d.%01dG", gForce / 10, gForce % 10); + osdPrintFloat(element->buff, SYM_NONE, osdGForce, "", 1, true, 'G'); } #endif // USE_ACC @@ -965,10 +965,10 @@ static void osdElementGpsLongitude(osdElementParms_t *element) static void osdElementGpsSats(osdElementParms_t *element) { - if (osdConfig()->gps_sats_show_hdop) { - tfp_sprintf(element->buff, "%c%c%2d %d.%d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat, gpsSol.hdop / 100, (gpsSol.hdop / 10) % 10); - } else { - tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); + int pos = tfp_sprintf(element->buff, "%c%c%2d", SYM_SAT_L, SYM_SAT_R, gpsSol.numSat); + if (osdConfig()->gps_sats_show_hdop) { // add on the GPS module HDOP estimate + element->buff[pos++] = ' '; + osdPrintFloat(element->buff + pos, SYM_NONE, gpsSol.hdop / 100.0f, "", 1, true, SYM_NONE); } } @@ -1115,15 +1115,15 @@ static void osdElementMainBatteryUsage(osdElementParms_t *element) static void osdElementMainBatteryVoltage(osdElementParms_t *element) { - int batteryVoltage = getBatteryVoltage(); + unsigned decimalPlaces; + const float batteryVoltage = getBatteryVoltage() / 100.0f; - element->buff[0] = osdGetBatterySymbol(getBatteryAverageCellVoltage()); - if (batteryVoltage >= 1000) { - batteryVoltage = (batteryVoltage + 5) / 10; - tfp_sprintf(element->buff + 1, "%d.%d%c", batteryVoltage / 10, batteryVoltage % 10, SYM_VOLT); + if (batteryVoltage >= 10) { // if voltage is 10v or more then display only 1 decimal place + decimalPlaces = 1; } else { - tfp_sprintf(element->buff + 1, "%d.%02d%c", batteryVoltage / 100, batteryVoltage % 100, SYM_VOLT); + decimalPlaces = 2; } + osdPrintFloat(element->buff, osdGetBatterySymbol(getBatteryAverageCellVoltage()), batteryVoltage, "", decimalPlaces, true, SYM_VOLT); } static void osdElementMotorDiagnostics(osdElementParms_t *element) @@ -1164,9 +1164,9 @@ static void osdElementNumericalVario(osdElementParms_t *element) haveGps = sensors(SENSOR_GPS) && STATE(GPS_FIX); #endif // USE_GPS if (haveBaro || haveGps) { - const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()); + const float verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario()) / 100.0f; const char directionSymbol = verticalSpeed < 0 ? SYM_ARROW_SMALL_DOWN : SYM_ARROW_SMALL_UP; - tfp_sprintf(element->buff, "%c%01d.%01d%c", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10), osdGetVarioToSelectedUnitSymbol()); + osdPrintFloat(element->buff, directionSymbol, fabsf(verticalSpeed), "", 1, true, osdGetVarioToSelectedUnitSymbol()); } else { // We use this symbol when we don't have a valid measure element->buff[0] = SYM_HYPHEN; diff --git a/src/main/osd/osd_elements.h b/src/main/osd/osd_elements.h index 1a4ce60ff3..31ee7cc0a8 100644 --- a/src/main/osd/osd_elements.h +++ b/src/main/osd/osd_elements.h @@ -49,7 +49,7 @@ void osdFormatDistanceString(char *result, int distance, char leadingSymbol); bool osdFormatRtcDateTime(char *buffer); void osdFormatTime(char * buff, osd_timer_precision_e precision, timeUs_t time); void osdFormatTimer(char *buff, bool showSymbol, bool usePrecision, int timerIndex); -int32_t osdGetMetersToSelectedUnit(int32_t meters); +float osdGetMetersToSelectedUnit(int32_t meters); char osdGetMetersToSelectedUnitSymbol(void); int32_t osdGetSpeedToSelectedUnit(int32_t value); char osdGetSpeedToSelectedUnitSymbol(void); diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 7bdbc33b8f..8969837c13 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -263,7 +263,7 @@ void simulateFlight(void) GPS_distanceToHome = 100; GPS_distanceFlownInCm = 10000; simulationBatteryVoltage = 1470; - simulationAltitude = 200; + simulationAltitude = 200; // converts to 6.56168 feet which rounds to 6.6 in imperial units stats test simulationTime += 1e6; osdRefresh(simulationTime); @@ -501,7 +501,7 @@ TEST_F(OsdTest, TestStatsImperial) // then // statistics screen should display the following int row = 5; - displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 6.5%c", SYM_FT); + displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 6.6%c", SYM_FT); displayPortTestBufferSubstring(2, row++, "MAX SPEED : 17"); displayPortTestBufferSubstring(2, row++, "MAX DISTANCE : 3772%c", SYM_FT); displayPortTestBufferSubstring(2, row++, "FLIGHT DISTANCE : 6.52%c", SYM_MILES); @@ -905,28 +905,28 @@ TEST_F(OsdTest, TestElementAltitude) displayPortTestBufferSubstring(23, 7, "%c0.0%c", SYM_ALTITUDE, SYM_M); // when - simulationAltitude = 247; + simulationAltitude = 247; // rounds to 2.5m displayClearScreen(&testDisplayPort); osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(23, 7, "%c2.4%c", SYM_ALTITUDE, SYM_M); + displayPortTestBufferSubstring(23, 7, "%c2.5%c", SYM_ALTITUDE, SYM_M); // when - simulationAltitude = 4247; + simulationAltitude = 4247; // rounds to 42.5m displayClearScreen(&testDisplayPort); osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(23, 7, "%c42.4%c", SYM_ALTITUDE, SYM_M); + displayPortTestBufferSubstring(23, 7, "%c42.5%c", SYM_ALTITUDE, SYM_M); // when - simulationAltitude = -247; + simulationAltitude = -247; // rounds to -2.5m displayClearScreen(&testDisplayPort); osdRefresh(simulationTime); // then - displayPortTestBufferSubstring(23, 7, "%c-2.4%c", SYM_ALTITUDE, SYM_M); + displayPortTestBufferSubstring(23, 7, "%c-2.5%c", SYM_ALTITUDE, SYM_M); // when simulationAltitude = -70;