From b49f20fd8dd37a5fae7aee150ea8e75e44dfe48f Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 22 Nov 2017 07:18:24 +0000 Subject: [PATCH] Tidy of OSD code --- src/main/io/osd.c | 204 +++++++++++++++++++++++----------------------- src/main/io/osd.h | 8 +- 2 files changed, 104 insertions(+), 108 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5a4d165ee7..f437b2f5b1 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -42,7 +42,6 @@ #include "cms/cms.h" #include "cms/cms_types.h" -#include "cms/cms_menu_osd.h" #include "common/maths.h" #include "common/printf.h" @@ -62,27 +61,26 @@ #include "io/flashfs.h" #include "io/gps.h" #include "io/osd.h" -#include "io/vtx_rtc6705.h" #include "io/vtx_control.h" +#include "io/vtx_rtc6705.h" #include "io/vtx_string.h" #include "io/vtx.h" #include "fc/config.h" +#include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "fc/rc_adjustments.h" #include "flight/altitude.h" -#include "flight/navigation.h" -#include "flight/pid.h" #include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" #include "sensors/barometer.h" #include "sensors/battery.h" -#include "sensors/sensors.h" #include "sensors/esc_sensor.h" +#include "sensors/sensors.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -118,26 +116,29 @@ static timeUs_t flyTime = 0; static uint8_t statRssi; typedef struct statistic_s { + timeUs_t armed_time; int16_t max_speed; int16_t min_voltage; // /10 int16_t max_current; // /10 int16_t min_rssi; int16_t max_altitude; int16_t max_distance; - timeUs_t armed_time; } statistic_t; static statistic_t stats; -uint32_t resumeRefreshAt = 0; +timeUs_t resumeRefreshAt = 0; #define REFRESH_1S 1000 * 1000 static uint8_t armState; static displayPort_t *osdDisplayPort; -#define AH_SYMBOL_COUNT 9 +#ifdef USE_ESC_SENSOR +static escSensorData_t *escData; +#endif +#define AH_SYMBOL_COUNT 9 #define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_HEIGHT_POS 3 @@ -156,11 +157,7 @@ static const char compassBar[] = { SYM_HEADING_LINE, SYM_HEADING_DIVIDED_LINE, SYM_HEADING_LINE }; -PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 1); - -#ifdef USE_ESC_SENSOR -static escSensorData_t *escData; -#endif +PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2); /** * Gets the correct altitude symbol for the current unit system @@ -188,8 +185,8 @@ static char osdGetBatterySymbol(int cellVoltage) if (getBatteryState() == BATTERY_CRITICAL) { return SYM_MAIN_BATT; // FIXME: currently the BAT- symbol, ideally replace with a battery with exclamation mark } else { - /* Calculate a symbol offset using cell voltage over full cell voltage range */ - int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7); + // Calculate a symbol offset using cell voltage over full cell voltage range + const int symOffset = scaleRange(cellVoltage, batteryConfig()->vbatmincellvoltage * 10, batteryConfig()->vbatmaxcellvoltage * 10, 0, 7); return SYM_BATT_EMPTY - constrain(symOffset, 0, 6); } } @@ -310,7 +307,7 @@ STATIC_UNIT_TESTED void osdFormatTimer(char *buff, bool showSymbol, int timerInd } #ifdef USE_RTC_TIME -bool printRtcDateTime(char *buffer) +static bool printRtcDateTime(char *buffer) { dateTime_t dateTime; if (!rtcGetDateTime(&dateTime)) { @@ -395,17 +392,6 @@ static void osdDrawSingleElement(uint8_t item) break; } - case OSD_HOME_DIST: - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome); - tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); - } else { - // We use this symbol when we don't have a FIX - buff[0] = SYM_COLON; - buff[1] = 0; - } - break; - case OSD_HOME_DIR: if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { if (GPS_distanceToHome > 0) { @@ -425,18 +411,29 @@ static void osdDrawSingleElement(uint8_t item) break; + case OSD_HOME_DIST: + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceToHome); + tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); + } else { + // We use this symbol when we don't have a FIX + buff[0] = SYM_COLON; + buff[1] = 0; + } + break; + #endif // GPS case OSD_COMPASS_BAR: - { - int16_t h = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + { + int16_t h = DECIDEGREES_TO_DEGREES(attitude.values.yaw); - h = osdGetHeadingIntoDiscreteDirections(h, 16); + h = osdGetHeadingIntoDiscreteDirections(h, 16); - memcpy(buff, compassBar + h, 9); - buff[9]=0; - break; - } + memcpy(buff, compassBar + h, 9); + buff[9]=0; + break; + } case OSD_ALTITUDE: { @@ -464,35 +461,38 @@ static void osdDrawSingleElement(uint8_t item) } else { osdFormatTime(buff, OSD_TIMER_PREC_SECOND, remaining_time); } - break; + break; } case OSD_FLYMODE: { char *p = "ACRO"; - if (isAirmodeActive()) + if (isAirmodeActive()) { p = "AIR"; + } - if (FLIGHT_MODE(FAILSAFE_MODE)) + if (FLIGHT_MODE(FAILSAFE_MODE)) { p = "!FS!"; - else if (FLIGHT_MODE(ANGLE_MODE)) + } else if (FLIGHT_MODE(ANGLE_MODE)) { p = "STAB"; - else if (FLIGHT_MODE(HORIZON_MODE)) + } else if (FLIGHT_MODE(HORIZON_MODE)) { p = "HOR"; + } displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return; } case OSD_CRAFT_NAME: - if (strlen(pilotConfig()->name) == 0) + if (strlen(pilotConfig()->name) == 0) { strcpy(buff, "CRAFT_NAME"); - else { + } else { for (unsigned int i = 0; i < MAX_NAME_LENGTH; i++) { buff[i] = toupper((unsigned char)pilotConfig()->name[i]); - if (pilotConfig()->name[i] == 0) + if (pilotConfig()->name[i] == 0) { break; + } } } break; @@ -586,41 +586,28 @@ static void osdDrawSingleElement(uint8_t item) } case OSD_ROLL_PIDS: - { - const pidProfile_t *pidProfile = currentPidProfile; - osdFormatPID(buff, "ROL", &pidProfile->pid[PID_ROLL]); - break; - } + osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); + break; case OSD_PITCH_PIDS: - { - const pidProfile_t *pidProfile = currentPidProfile; - osdFormatPID(buff, "PIT", &pidProfile->pid[PID_PITCH]); - break; - } + osdFormatPID(buff, "PIT", ¤tPidProfile->pid[PID_PITCH]); + break; case OSD_YAW_PIDS: - { - const pidProfile_t *pidProfile = currentPidProfile; - osdFormatPID(buff, "YAW", &pidProfile->pid[PID_YAW]); - break; - } + osdFormatPID(buff, "YAW", ¤tPidProfile->pid[PID_YAW]); + break; case OSD_POWER: tfp_sprintf(buff, "%4dW", getAmperage() * getBatteryVoltage() / 1000); break; case OSD_PIDRATE_PROFILE: - { - const uint8_t pidProfileIndex = getCurrentPidProfileIndex(); - const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex(); - tfp_sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1); - break; - } + tfp_sprintf(buff, "%d-%d", getCurrentPidProfileIndex() + 1, getCurrentControlRateProfileIndex() + 1); + break; case OSD_WARNINGS: { - uint16_t enabledWarnings = osdConfig()->enabledWarnings; + const uint16_t enabledWarnings = osdConfig()->enabledWarnings; const batteryState_e batteryState = getBatteryState(); @@ -629,7 +616,7 @@ static void osdDrawSingleElement(uint8_t item) break; } - /* Warn when in flip over after crash mode */ + // Warn when in flip over after crash mode if ((enabledWarnings & OSD_WARNING_CRASH_FLIP) && (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { @@ -637,7 +624,7 @@ static void osdDrawSingleElement(uint8_t item) break; } - /* Show most severe reason for arming being disabled */ + // Show most severe reason for arming being disabled if (enabledWarnings & OSD_WARNING_ARMING_DISABLE && IS_RC_MODE_ACTIVE(BOXARM) && isArmingDisabled()) { const armingDisableFlags_e flags = getArmingDisableFlags(); for (int i = 0; i < NUM_ARMING_DISABLE_FLAGS; i++) { @@ -654,14 +641,14 @@ static void osdDrawSingleElement(uint8_t item) break; } - /* Show warning if battery is not fresh */ + // Show warning if battery is not fresh if (enabledWarnings & OSD_WARNING_BATTERY_NOT_FULL && !ARMING_FLAG(WAS_EVER_ARMED) && (getBatteryState() == BATTERY_OK) && getBatteryAverageCellVoltage() < batteryConfig()->vbatfullcellvoltage) { tfp_sprintf(buff, "BATT NOT FULL"); break; } - /* Visual beeper */ + // Visual beeper if (enabledWarnings & OSD_WARNING_VISUAL_BEEPER && showVisualBeeper) { tfp_sprintf(buff, " * * * *"); break; @@ -696,18 +683,19 @@ static void osdDrawSingleElement(uint8_t item) #define MAIN_BATT_USAGE_STEPS 11 // Use an odd number so the bar can be centralised. //Calculate constrained value - float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity); + const float value = constrain(batteryConfig()->batteryCapacity - getMAhDrawn(), 0, batteryConfig()->batteryCapacity); //Calculate mAh used progress - uint8_t mAhUsedProgress = ceil((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS))); + const uint8_t mAhUsedProgress = ceil((value / (batteryConfig()->batteryCapacity / MAIN_BATT_USAGE_STEPS))); //Create empty battery indicator bar buff[0] = SYM_PB_START; for (uint8_t i = 1; i <= MAIN_BATT_USAGE_STEPS; i++) { - if (i <= mAhUsedProgress) + if (i <= mAhUsedProgress) { buff[i] = SYM_PB_FULL; - else + } else { buff[i] = SYM_PB_EMPTY; + } } buff[MAIN_BATT_USAGE_STEPS+1] = SYM_PB_CLOSE; @@ -775,9 +763,10 @@ static void osdDrawElements(void) { displayClearScreen(osdDisplayPort); - /* Hide OSD when OSDSW mode is active */ - if (IS_RC_MODE_ACTIVE(BOXOSD)) - return; + // Hide OSD when OSDSW mode is active + if (IS_RC_MODE_ACTIVE(BOXOSD)) { + return; + } if (sensors(SENSOR_ACC)) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); @@ -824,14 +813,14 @@ static void osdDrawElements(void) #endif // GPS #ifdef USE_ESC_SENSOR - if (feature(FEATURE_ESC_SENSOR)) { - osdDrawSingleElement(OSD_ESC_TMP); - osdDrawSingleElement(OSD_ESC_RPM); - } + if (feature(FEATURE_ESC_SENSOR)) { + osdDrawSingleElement(OSD_ESC_TMP); + osdDrawSingleElement(OSD_ESC_RPM); + } #endif #ifdef USE_RTC_TIME - osdDrawSingleElement(OSD_RTC_DATETIME); + osdDrawSingleElement(OSD_RTC_DATETIME); #endif #ifdef USE_OSD_ADJUSTMENTS @@ -841,12 +830,12 @@ static void osdDrawElements(void) void pgResetFn_osdConfig(osdConfig_t *osdConfig) { - /* Position elements near centre of screen and disabled by default */ + // Position elements near centre of screen and disabled by default for (int i = 0; i < OSD_ITEM_COUNT; i++) { osdConfig->item_pos[i] = OSD_POS(10, 7); } - /* Always enable warnings elements by default */ + // Always enable warnings elements by default osdConfig->item_pos[OSD_WARNINGS] = OSD_POS(9, 10) | VISIBLE_FLAG; osdConfig->enabled_stats[OSD_STAT_MAX_SPEED] = true; @@ -865,7 +854,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig) osdConfig->units = OSD_UNIT_METRIC; - /* Enable all warnings by default */ + // Enable all warnings by default osdConfig->enabledWarnings = UINT16_MAX; osdConfig->timers[OSD_TIMER_1] = OSD_TIMER(OSD_TIMER_SRC_ON, OSD_TIMER_PREC_SECOND, 10); @@ -893,8 +882,9 @@ static void osdDrawLogo(int x, int y) void osdInit(displayPort_t *osdDisplayPortToUse) { - if (!osdDisplayPortToUse) + if (!osdDisplayPortToUse) { return; + } BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31)); @@ -927,7 +917,7 @@ void osdInit(displayPort_t *osdDisplayPortToUse) } #endif -displayResync(osdDisplayPort); + displayResync(osdDisplayPort); resumeRefreshAt = micros() + (4 * REFRESH_1S); } @@ -938,10 +928,11 @@ void osdUpdateAlarms(void) int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; - if (statRssi < osdConfig()->rssi_alarm) + if (statRssi < osdConfig()->rssi_alarm) { SET_BLINK(OSD_RSSI_VALUE); - else + } else { CLR_BLINK(OSD_RSSI_VALUE); + } if (getBatteryState() == BATTERY_OK) { CLR_BLINK(OSD_WARNINGS); @@ -953,19 +944,21 @@ void osdUpdateAlarms(void) SET_BLINK(OSD_AVG_CELL_VOLTAGE); } - if (STATE(GPS_FIX) == 0) + if (STATE(GPS_FIX) == 0) { SET_BLINK(OSD_GPS_SATS); - else + } else { CLR_BLINK(OSD_GPS_SATS); + } for (int i = 0; i < OSD_TIMER_COUNT; i++) { const uint16_t timer = osdConfig()->timers[i]; const timeUs_t time = osdGetTimerValue(OSD_TIMER_SRC(timer)); const timeUs_t alarmTime = OSD_TIMER_ALARM(timer) * 60000000; // convert from minutes to us - if (alarmTime != 0 && time >= alarmTime) + if (alarmTime != 0 && time >= alarmTime) { SET_BLINK(OSD_ITEM_TIMER_1 + i); - else + } else { CLR_BLINK(OSD_ITEM_TIMER_1 + i); + } } if (getMAhDrawn() >= osdConfig()->cap_alarm) { @@ -978,10 +971,11 @@ void osdUpdateAlarms(void) CLR_BLINK(OSD_REMAINING_TIME_ESTIMATE); } - if (alt >= osdConfig()->alt_alarm) + if (alt >= osdConfig()->alt_alarm) { SET_BLINK(OSD_ALTITUDE); - else + } else { CLR_BLINK(OSD_ALTITUDE); + } } void osdResetAlarms(void) @@ -1017,25 +1011,30 @@ static void osdUpdateStats(void) #ifdef USE_GPS value = CM_S_TO_KM_H(gpsSol.groundSpeed); #endif - if (stats.max_speed < value) + if (stats.max_speed < value) { stats.max_speed = value; + } - if (stats.min_voltage > getBatteryVoltage()) + if (stats.min_voltage > getBatteryVoltage()) { stats.min_voltage = getBatteryVoltage(); + } value = getAmperage() / 100; - if (stats.max_current < value) + if (stats.max_current < value) { stats.max_current = value; + } - if (stats.min_rssi > statRssi) + if (stats.min_rssi > statRssi) { stats.min_rssi = statRssi; + } - if (stats.max_altitude < getEstimatedAltitude()) + if (stats.max_altitude < getEstimatedAltitude()) { stats.max_altitude = getEstimatedAltitude(); + } #ifdef USE_GPS if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && (stats.max_distance < GPS_distanceToHome)) { - stats.max_distance = GPS_distanceToHome; + stats.max_distance = GPS_distanceToHome; } #endif } @@ -1092,7 +1091,8 @@ static void osdDisplayStatisticLabel(uint8_t y, const char * text, const char * /* * Test if there's some stat enabled */ -static bool isSomeStatEnabled(void) { +static bool isSomeStatEnabled(void) +{ for (int i = 0; i < OSD_STAT_COUNT; i++) { if (osdConfig()->enabled_stats[i]) { return true; @@ -1187,7 +1187,7 @@ static void osdShowStats(void) } #endif - /* Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen */ + // Reset time since last armed here to ensure this timer is at zero when back at "main" OSD screen stats.armed_time = 0; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2118fbc020..8620f852d4 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,7 +17,6 @@ #pragma once -#ifdef USE_OSD #include "common/time.h" #include "config/parameter_group.h" @@ -141,7 +140,6 @@ typedef enum { typedef struct osdConfig_s { uint16_t item_pos[OSD_ITEM_COUNT]; - bool enabled_stats[OSD_STAT_COUNT]; // Alarms uint16_t cap_alarm; @@ -155,16 +153,14 @@ typedef struct osdConfig_s { uint8_t ahMaxPitch; uint8_t ahMaxRoll; + bool enabled_stats[OSD_STAT_COUNT]; } osdConfig_t; -extern uint32_t resumeRefreshAt; +extern timeUs_t resumeRefreshAt; PG_DECLARE(osdConfig_t, osdConfig); struct displayPort_s; void osdInit(struct displayPort_s *osdDisplayPort); -void osdResetConfig(osdConfig_t *osdProfile); void osdResetAlarms(void); void osdUpdate(timeUs_t currentTimeUs); - -#endif