From 541d589af92b04b648ea826824941fff10e8c104 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:04:55 +0100 Subject: [PATCH 01/62] Filter setup via EzTune --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_init.c | 5 ++ src/main/fc/fc_msp.c | 2 + src/main/fc/settings.c | 2 + src/main/fc/settings.h | 1 + src/main/fc/settings.yaml | 17 +++++ src/main/flight/ez_tune.c | 91 +++++++++++++++++++++++++++ src/main/flight/ez_tune.h | 36 +++++++++++ src/main/target/common.h | 2 + 10 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/ez_tune.c create mode 100644 src/main/flight/ez_tune.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 317da760b6..cdc01ac315 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -331,6 +331,8 @@ main_sources(COMMON_SRC flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h + flight/ez_tune.c + flight/ez_tune.h io/beeper.c io/beeper.h diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b1..329f42b219 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -119,7 +119,8 @@ #define PG_UNUSED_1 1029 #define PG_POWER_LIMITS_CONFIG 1030 #define PG_OSD_COMMON_CONFIG 1031 -#define PG_INAV_END 1031 +#define PG_EZ_TUNE 1032 +#define PG_INAV_END PG_EZ_TUNE // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ab8146d2..64b421c583 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -96,6 +96,7 @@ #include "flight/power_limits.h" #include "flight/rpm_filter.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -509,6 +510,10 @@ void init(void) owInit(); #endif +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ee5c8f0397..7f8354deee 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3164,6 +3164,8 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); break; + case EZ_TUNE_VALUE: + FALLTHROUGH; case PROFILE_VALUE: FALLTHROUGH; case CONTROL_RATE_VALUE: diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8dc17edc63..bef0750e88 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -237,6 +237,8 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(pidProfile_t) * getConfigProfile(); case CONTROL_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case EZ_TUNE_VALUE: + return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index b14289ba27..a6cc403842 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -34,6 +34,7 @@ typedef enum { PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), + EZ_TUNE_VALUE = (4 << SETTING_SECTION_OFFSET) } setting_section_e; typedef enum { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c0df0e204c..de52e9a7a5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1477,6 +1477,23 @@ groups: min: 0 max: 99 + - name: PG_EZ_TUNE + headers: ["flight/ez_tune.h"] + type: ezTuneSettings_t + value_type: EZ_TUNE_VALUE + members: + - name: ez_tune_enabled + description: "Enables EzTune feature" + default_value: OFF + field: enabled + type: bool + - name: ez_tune_filter_hz + description: "EzTune filter cutoff frequency" + default_value: 110 + field: filterHz + min: 10 + max: 300 + - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] condition: USE_RPM_FILTER diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c new file mode 100644 index 0000000000..a9656bd83f --- /dev/null +++ b/src/main/flight/ez_tune.c @@ -0,0 +1,91 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "fc/config.h" +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "flight/ez_tune.h" + +#include "fc/settings.h" +#include "flight/pid.h" +#include "sensors/gyro.h" + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); + +PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, + .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, + .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, +); + +static float computePt1FilterDelayMs(uint8_t filterHz) { + return 1.0f / (2.0f * M_PIf * filterHz); +} + +/** + * Update INAV settings based on current EZTune settings + * This has to be called every time control profile is changed, or EZTune settings are changed + * FIXME call on profile change + * FIXME call on EZTune settings change + */ +void ezTuneUpdate(void) { + if (ezTune()->enabled) { + + // Setup filtering + + //Enable Smith predictor + pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) + + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); + + //Set Dterm LPF + pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); + pidProfileMutable()->dterm_lpf_type = FILTER_PT2; + + //Set main gyro filter + gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz; + gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; + + //Set anti-aliasing filter + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250); + gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; + + //Enable dynamic notch + gyroConfigMutable()->dynamicGyroNotchEnabled = 1; + gyroConfigMutable()->dynamicGyroNotchQ = 250; + gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT); + gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D; + + //Make sure Kalman filter is enabled + gyroConfigMutable()->kalmanEnabled = 1; + if (ezTune()->filterHz < 150) { + gyroConfigMutable()->kalman_q = 200; + } else { + gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); + } + + + + } +} \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h new file mode 100644 index 0000000000..a2ba6f0249 --- /dev/null +++ b/src/main/flight/ez_tune.h @@ -0,0 +1,36 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "config/parameter_group.h" + +typedef struct ezTuneSettings_s { + uint8_t enabled; + uint16_t filterHz; +} ezTuneSettings_t; + +PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); + +void ezTuneUpdate(void); \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index 2365be9671..26ec4ff012 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -192,3 +192,5 @@ #define USE_HOTT_TEXTMODE #endif + +#define USE_EZ_TUNE \ No newline at end of file From a2051c1371b7689ce3a97a5e5ed5b82ff2f4ae03 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:06:29 +0100 Subject: [PATCH 02/62] Disable gyro dynamic LPF for EZ-Tune --- src/main/flight/ez_tune.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index a9656bd83f..240e64f921 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -85,7 +85,8 @@ void ezTuneUpdate(void) { gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); } - + //Disable dynamic LPF + gyroConfigMutable()->useDynamicLpf = 0; } } \ No newline at end of file From fb5210d0b952601f6ca9721321a4313414db065a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:52:22 +0100 Subject: [PATCH 03/62] Add EzTune to the settings.yaml --- src/main/fc/settings.yaml | 30 ++++++++++++++++++++++++++++++ src/main/flight/ez_tune.c | 10 ++++++++++ src/main/flight/ez_tune.h | 8 ++++++++ 3 files changed, 48 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index de52e9a7a5..7dbc1d8453 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1493,6 +1493,36 @@ groups: field: filterHz min: 10 max: 300 + - name: ez_tune_axis_ratio + description: "EzTune axis ratio" + default_value: 100 + field: axisRatio + min: 25 + max: 175 + - name: ez_tune_response + description: "EzTune response" + default_value: 100 + field: response + min: 0 + max: 200 + - name: ez_tune_damping + description: "EzTune damping" + default_value: 100 + field: damping + min: 0 + max: 200 + - name: ez_tune_stability + description: "EzTune stability" + default_value: 100 + field: stability + min: 0 + max: 200 + - name: ez_tune_aggressiveness + description: "EzTune aggressiveness" + default_value: 100 + field: aggressiveness + min: 0 + max: 200 - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 240e64f921..35717ed828 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -38,6 +38,11 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0) PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, + .axisRatio = SETTING_EZ_TUNE_AXIS_RATIO_DEFAULT, + .response = SETTING_EZ_TUNE_RESPONSE_DEFAULT, + .damping = SETTING_EZ_TUNE_DAMPING_DEFAULT, + .stability = SETTING_EZ_TUNE_STABILITY_DEFAULT, + .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, ); static float computePt1FilterDelayMs(uint8_t filterHz) { @@ -88,5 +93,10 @@ void ezTuneUpdate(void) { //Disable dynamic LPF gyroConfigMutable()->useDynamicLpf = 0; + //Setup PID controller + + //Roll + + } } \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index a2ba6f0249..98cb692636 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -29,8 +29,16 @@ typedef struct ezTuneSettings_s { uint8_t enabled; uint16_t filterHz; + uint8_t axisRatio; + uint8_t response; + uint8_t damping; + uint8_t stability; + uint8_t aggressiveness; } ezTuneSettings_t; +#define EZ_TUNE_PID_RP_DEFAULT {40, 75, 23, 100} +#define EZ_TUNE_PID_YAW_DEFAULT {45, 80, 0, 100} + PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); void ezTuneUpdate(void); \ No newline at end of file From de6c442a11028d72035ba191ab6bcaea2ed6bd96 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 13 Mar 2023 11:24:10 +0100 Subject: [PATCH 04/62] Adjust PIDs from EzTune --- src/main/fc/settings.yaml | 2 +- src/main/flight/ez_tune.c | 33 +++++++++++++++++++++++++++++++-- src/main/flight/ez_tune.h | 3 --- 3 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7dbc1d8453..0d00044049 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1495,7 +1495,7 @@ groups: max: 300 - name: ez_tune_axis_ratio description: "EzTune axis ratio" - default_value: 100 + default_value: 110 field: axisRatio min: 25 max: 175 diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 35717ed828..c337332e1f 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -45,10 +45,21 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, ); +#define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 } +#define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 } + +#define EZ_TUNE_YAW_SCALE 0.5f + static float computePt1FilterDelayMs(uint8_t filterHz) { return 1.0f / (2.0f * M_PIf * filterHz); } +static float getYawPidScale(float input) { + const float normalized = (input - 100) * 0.01f; + + return 1.0f + (normalized * 0.5f); +} + /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed @@ -95,8 +106,26 @@ void ezTuneUpdate(void) { //Setup PID controller - //Roll - + const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT; + const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT; + const float pitchRatio = ezTune()->axisRatio / 100.0f; + //Roll + pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f; + + //Pitch + pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio; + + //Yaw + pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response); + pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability); + pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping); + pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); } } \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index 98cb692636..aa0088fe7f 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -36,9 +36,6 @@ typedef struct ezTuneSettings_s { uint8_t aggressiveness; } ezTuneSettings_t; -#define EZ_TUNE_PID_RP_DEFAULT {40, 75, 23, 100} -#define EZ_TUNE_PID_YAW_DEFAULT {45, 80, 0, 100} - PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); void ezTuneUpdate(void); \ No newline at end of file From 554eaeaa0ea59cd584e300b6c4fc9e3ab6d662cc Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 13 Mar 2023 17:03:41 +0100 Subject: [PATCH 05/62] Add rate and expo to EZ-Tune --- src/main/fc/settings.yaml | 26 +++++++++++++++++++------- src/main/flight/ez_tune.c | 16 +++++++++------- src/main/flight/ez_tune.h | 2 ++ 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0d00044049..188f1c9119 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1482,47 +1482,59 @@ groups: type: ezTuneSettings_t value_type: EZ_TUNE_VALUE members: - - name: ez_tune_enabled + - name: ez_enabled description: "Enables EzTune feature" default_value: OFF field: enabled type: bool - - name: ez_tune_filter_hz + - name: ez_filter_hz description: "EzTune filter cutoff frequency" default_value: 110 field: filterHz min: 10 max: 300 - - name: ez_tune_axis_ratio + - name: ez_axis_ratio description: "EzTune axis ratio" default_value: 110 field: axisRatio min: 25 max: 175 - - name: ez_tune_response + - name: ez_response description: "EzTune response" default_value: 100 field: response min: 0 max: 200 - - name: ez_tune_damping + - name: ez_damping description: "EzTune damping" default_value: 100 field: damping min: 0 max: 200 - - name: ez_tune_stability + - name: ez_stability description: "EzTune stability" default_value: 100 field: stability min: 0 max: 200 - - name: ez_tune_aggressiveness + - name: ez_aggressiveness description: "EzTune aggressiveness" default_value: 100 field: aggressiveness min: 0 max: 200 + - name: ez_rate + description: "EzTune rate" + default_value: 100 + field: rate + min: 0 + max: 200 + - name: ez_expo + description: "EzTune expo" + default_value: 100 + field: expo + min: 0 + max: 200 - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index c337332e1f..cc1c3dab5b 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -36,13 +36,15 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, - .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, - .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, - .axisRatio = SETTING_EZ_TUNE_AXIS_RATIO_DEFAULT, - .response = SETTING_EZ_TUNE_RESPONSE_DEFAULT, - .damping = SETTING_EZ_TUNE_DAMPING_DEFAULT, - .stability = SETTING_EZ_TUNE_STABILITY_DEFAULT, - .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, + .enabled = SETTING_EZ_ENABLED_DEFAULT, + .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT, + .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT, + .response = SETTING_EZ_RESPONSE_DEFAULT, + .damping = SETTING_EZ_DAMPING_DEFAULT, + .stability = SETTING_EZ_STABILITY_DEFAULT, + .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT, + .rate = SETTING_EZ_RATE_DEFAULT, + .expo = SETTING_EZ_EXPO_DEFAULT, ); #define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 } diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index aa0088fe7f..5fcc1ef6f7 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -34,6 +34,8 @@ typedef struct ezTuneSettings_s { uint8_t damping; uint8_t stability; uint8_t aggressiveness; + uint8_t rate; + uint8_t expo; } ezTuneSettings_t; PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); From 0589b5636e6c1f7ac371675bdb439cc761f74428 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 17 Mar 2023 23:10:37 +0000 Subject: [PATCH 06/62] Increase RC Channels to 24 Both FrSky and Jeti can use 24 channels. INAV doesn't handle the additional channels gracefully, certainly from Jeti. This PR increases the channel count to 24. But, does not apply to F411 or F722. --- src/main/navigation/navigation.c | 17 ++++++++--------- src/main/rx/jetiexbus.c | 7 ++++++- src/main/rx/rx.h | 6 +++++- src/main/rx/sbus_channels.h | 4 ++++ src/main/target/common.h | 1 + 5 files changed, 24 insertions(+), 11 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e164b33ab5..4d88b1dbef 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3808,8 +3808,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; } - } - else { + } else { canActivateWaypoint = false; // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) @@ -3924,13 +3923,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) } } - /* - * Don't allow arming if any of JUMP waypoint has invalid settings - * First WP can't be JUMP - * Can't jump to immediately adjacent WPs (pointless) - * Can't jump beyond WP list - * Only jump to geo-referenced WP types - */ + /* + * Don't allow arming if any of JUMP waypoint has invalid settings + * First WP can't be JUMP + * Can't jump to immediately adjacent WPs (pointless) + * Can't jump beyond WP list + * Only jump to geo-referenced WP types + */ if (posControl.waypointCount) { for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){ if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){ diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d9b009342e..ce6ebfe958 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -63,7 +63,12 @@ #define JETIEXBUS_BAUDRATE 125000 // EX Bus 125000; EX Bus HS 250000 not supported #define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) #define JETIEXBUS_MIN_FRAME_GAP 1000 -#define JETIEXBUS_CHANNEL_COUNT 16 // most Jeti TX transmit 16 channels + +#ifdef USE_24CHANNELS +#define JETIEXBUS_CHANNEL_COUNT 24 +#else +#define JETIEXBUS_CHANNEL_COUNT 16 +#endif #define EXBUS_START_CHANNEL_FRAME (0x3E) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 4880c4c4fe..4655f23e8c 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -81,7 +81,11 @@ typedef enum { SERIALRX_MAVLINK, } rxSerialReceiverType_e; -#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 +#ifdef USE_24CHANNELS +#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26 +#else +#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18 +#endif #define NON_AUX_CHANNEL_COUNT 4 #define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT) diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h index 5e8a6a0268..e5ec0ff2c9 100644 --- a/src/main/rx/sbus_channels.h +++ b/src/main/rx/sbus_channels.h @@ -20,7 +20,11 @@ #include #include "rx/rx.h" +#ifdef USE_24CHANNELS +#define SBUS_MAX_CHANNEL 26 +#else #define SBUS_MAX_CHANNEL 18 +#endif #define SBUS_FLAG_SIGNAL_LOSS (1 << 2) #define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) diff --git a/src/main/target/common.h b/src/main/target/common.h index 2365be9671..e07ac9704a 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -190,5 +190,6 @@ #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE +#define USE_24CHANNELS #endif From 9799ae07882822c65ced1028accd1105bcee741c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 15 Jun 2023 10:16:00 +0200 Subject: [PATCH 07/62] Cap gyro_anti_aliasing_lpf_hz and gyro nyquist frequency to 1/2 of gyro sampling rate --- src/main/flight/ez_tune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index cc1c3dab5b..c4846d3989 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -86,7 +86,7 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250); + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; //Enable dynamic notch From 67890ed3ec392c98f259470e4a4d69f68aa26618 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 14 Aug 2023 10:56:45 +0200 Subject: [PATCH 08/62] Add files --- .vscode/c_cpp_properties.json | 66 +++++++++++++++++++++++++++++++++++ .vscode/tasks.json | 41 ++++++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100755 .vscode/c_cpp_properties.json create mode 100755 .vscode/tasks.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100755 index 0000000000..6e7d914b25 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,66 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**", + "/usr/include/**" + ], + "browse": { + "limitSymbolsToIncludedHeaders": false, + "path": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**" + ] + }, + "intelliSenseMode": "linux-gcc-arm", + "cStandard": "c11", + "cppStandard": "c++17", + "defines": [ + "MCU_FLASH_SIZE 512", + "USE_NAV", + "NAV_FIXED_WING_LANDING", + "USE_OSD", + "USE_GYRO_NOTCH_1", + "USE_GYRO_NOTCH_2", + "USE_DTERM_NOTCH", + "USE_ACC_NOTCH", + "USE_GYRO_BIQUAD_RC_FIR2", + "USE_D_BOOST", + "USE_SERIALSHOT", + "USE_ANTIGRAVITY", + "USE_ASYNC_GYRO_PROCESSING", + "USE_RPM_FILTER", + "USE_GLOBAL_FUNCTIONS", + "USE_DYNAMIC_FILTERS", + "USE_IMU_BNO055", + "USE_SECONDARY_IMU", + "USE_DSHOT", + "FLASH_SIZE 480", + "USE_I2C_IO_EXPANDER", + "USE_PCF8574", + "USE_ESC_SENSOR", + "USE_PROGRAMMING_FRAMEWORK", + "USE_SERIALRX_GHST", + "USE_TELEMETRY_GHST", + "USE_CMS", + "USE_DJI_HD_OSD", + "USE_GYRO_KALMAN", + "USE_RANGEFINDER", + "USE_RATE_DYNAMICS", + "USE_SMITH_PREDICTOR", + "USE_ALPHA_BETA_GAMMA_FILTER", + "USE_MAG_VCM5883", + "USE_TELEMETRY_JETIEXBUS", + "USE_NAV", + "USE_SDCARD_SDIO", + "USE_SDCARD", + "USE_Q_TUNE", + "USE_GYRO_FFT_FILTER" + ], + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100755 index 0000000000..3ca90b787d --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,41 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Build Matek F722-SE", + "type": "shell", + "command": "make MATEKF722SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + { + "label": "Build Matek F722", + "type": "shell", + "command": "make MATEKF722", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + , + { + "label": "CMAKE Update", + "type": "shell", + "command": "cmake ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + ] +} \ No newline at end of file From 26deae54bbdf08085e85cace5d5d203ba7fb7fa1 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Aug 2023 23:28:11 +0100 Subject: [PATCH 09/62] first build --- src/main/fc/fc_core.c | 20 ++++++++++++++------ src/main/fc/fc_core.h | 1 + src/main/navigation/navigation.c | 20 +++++++++++++++----- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 2 +- 5 files changed, 32 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index af8d1202b1..46ef140f84 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -118,6 +118,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; +timeMs_t emergInflightRearmTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -176,7 +177,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) { int16_t stickDeflection = 0; -#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 +#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 const int16_t value = rawData - PWM_RANGE_MIDDLE; if (value < -500) { stickDeflection = -500; @@ -186,9 +187,9 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) stickDeflection = value; } #else - stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); + stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); #endif - + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } @@ -432,6 +433,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -509,6 +511,11 @@ void releaseSharedTelemetryPorts(void) { } } +bool emergInflightRearmEnabled(void) +{ + return millis() < emergInflightRearmTimeout; +} + void tryArm(void) { updateArmingStatus(); @@ -529,9 +536,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled()) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -837,7 +845,7 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens void taskMainPidLoop(timeUs_t currentTimeUs) { - + cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 37d0bbda79..1d34e31742 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,6 +43,7 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); +bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d8bc20554c..04c910aa63 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -228,6 +228,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; +static bool landingDetectorIsActive; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -2803,14 +2804,14 @@ void updateLandingStatus(timeMs_t currentTimeMs) } lastUpdateTimeMs = currentTimeMs; - static bool landingDetectorIsActive; - DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive); DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; + if (!emergInflightRearmEnabled()) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } @@ -2852,6 +2853,15 @@ bool isFlightDetected(void) return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); } +bool isProbablyStillFlying(void) +{ + bool inFlightSanityCheck = false; + if (STATE(AIRPLANE)) { + inFlightSanityCheck = isGPSHeadingValid(); + } + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; +} + /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ @@ -3801,7 +3811,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) canActivateWaypoint = false; // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) - canActivateLaunchMode = isNavLaunchEnabled(); + canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } return NAV_FSM_EVENT_SWITCH_TO_IDLE; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf4289e14a..9ffe532e6c 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -610,6 +610,7 @@ const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); +bool isProbablyStillFlying(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 91354d97ae..7b55a48e6a 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -692,7 +692,7 @@ bool isFixedWingFlying(void) bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; - return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition; + return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition; } /*----------------------------------------------------------- From 8f5d78376649e18b9be92c61807cbd8fd4ee8bea Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 22 Aug 2023 17:31:23 +0100 Subject: [PATCH 10/62] Initial cut - Separated HD and SD arming screen - Added symbol for pilot logo - Switched space indents to tabs in osd_symbols.h --- src/main/drivers/osd_symbols.h | 416 +++++++++++++++++---------------- src/main/io/osd.c | 27 ++- 2 files changed, 234 insertions(+), 209 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index a127234579..8dfaa10186 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -23,243 +23,247 @@ #if defined(USE_OSD) || defined(OSD_UNIT_TEST) -#define SYM_RSSI 0x01 // 001 Icon RSSI -#define SYM_LQ 0x02 // 002 LQ -#define SYM_LAT 0x03 // 003 GPS LAT -#define SYM_LON 0x04 // 004 GPS LON -#define SYM_AZIMUTH 0x05 // 005 Azimuth -#define SYM_TELEMETRY_0 0x06 // 006 Antenna tracking telemetry -#define SYM_TELEMETRY_1 0x07 // 007 Antenna tracking telemetry -#define SYM_SAT_L 0x08 // 008 Sats left -#define SYM_SAT_R 0x09 // 009 Sats right -#define SYM_HOME_NEAR 0x0A // 010 Home, near -#define SYM_DEGREES 0x0B // 011 ° heading angle -#define SYM_HEADING 0x0C // 012 Compass Heading symbol -#define SYM_SCALE 0x0D // 013 Map scale -#define SYM_HDP_L 0x0E // 014 HDOP left -#define SYM_HDP_R 0x0F // 015 HDOP right -#define SYM_HOME 0x10 // 016 Home icon -#define SYM_2RSS 0x11 // 017 RSSI 2 -#define SYM_DB 0x12 // 018 dB -#define SYM_DBM 0x13 // 019 dBm -#define SYM_SNR 0x14 // 020 SNR +#define SYM_RSSI 0x01 // 001 Icon RSSI +#define SYM_LQ 0x02 // 002 LQ +#define SYM_LAT 0x03 // 003 GPS LAT +#define SYM_LON 0x04 // 004 GPS LON +#define SYM_AZIMUTH 0x05 // 005 Azimuth +#define SYM_TELEMETRY_0 0x06 // 006 Antenna tracking telemetry +#define SYM_TELEMETRY_1 0x07 // 007 Antenna tracking telemetry +#define SYM_SAT_L 0x08 // 008 Sats left +#define SYM_SAT_R 0x09 // 009 Sats right +#define SYM_HOME_NEAR 0x0A // 010 Home, near +#define SYM_DEGREES 0x0B // 011 ° heading angle +#define SYM_HEADING 0x0C // 012 Compass Heading symbol +#define SYM_SCALE 0x0D // 013 Map scale +#define SYM_HDP_L 0x0E // 014 HDOP left +#define SYM_HDP_R 0x0F // 015 HDOP right +#define SYM_HOME 0x10 // 016 Home icon +#define SYM_2RSS 0x11 // 017 RSSI 2 +#define SYM_DB 0x12 // 018 dB +#define SYM_DBM 0x13 // 019 dBm +#define SYM_SNR 0x14 // 020 SNR -#define SYM_AH_DIRECTION_UP 0x15 // 021 Arrow up AHI -#define SYM_AH_DIRECTION_DOWN 0x16 // 022 Arrow down AHI -#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows +#define SYM_AH_DIRECTION_UP 0x15 // 021 Arrow up AHI +#define SYM_AH_DIRECTION_DOWN 0x16 // 022 Arrow down AHI +#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows -#define SYM_VOLT 0x1F // 031 VOLTS -#define SYM_MAH 0x99 // 153 mAh -// 0x21 // 033 ASCII ! -#define SYM_AH_KM 0x22 // 034 Ah/km -// 0x23 // 035 ASCII # -#define SYM_AH_MI 0x24 // 036 Ah/mi -// 0x25 // 037 ASCII % -// 0x26 // 038 ASCII & -#define SYM_VTX_POWER 0x27 // 039 VTx Power -// 0x28 // 040 to 062 ASCII -#define SYM_AH_NM 0x3F // 063 Ah/NM -// 0x40 // 064 to 095 ASCII -#define SYM_MAH_NM_0 0x60 // 096 mAh/NM left -#define SYM_MAH_NM_1 0x61 // 097 mAh/NM right -#define SYM_MAH_KM_0 0x6B // 107 MAH/KM left -#define SYM_MAH_KM_1 0x6C // 108 MAH/KM right -#define SYM_MILLIOHM 0x62 // 098 battery impedance Mohm +#define SYM_VOLT 0x1F // 031 VOLTS +#define SYM_MAH 0x99 // 153 mAh +// 0x21 // 033 ASCII ! +#define SYM_AH_KM 0x22 // 034 Ah/km +// 0x23 // 035 ASCII # +#define SYM_AH_MI 0x24 // 036 Ah/mi +// 0x25 // 037 ASCII % +// 0x26 // 038 ASCII & +#define SYM_VTX_POWER 0x27 // 039 VTx Power +// 0x28 // 040 to 062 ASCII +#define SYM_AH_NM 0x3F // 063 Ah/NM +// 0x40 // 064 to 095 ASCII +#define SYM_MAH_NM_0 0x60 // 096 mAh/NM left +#define SYM_MAH_NM_1 0x61 // 097 mAh/NM right +#define SYM_MAH_KM_0 0x6B // 107 MAH/KM left +#define SYM_MAH_KM_1 0x6C // 108 MAH/KM right +#define SYM_MILLIOHM 0x62 // 098 battery impedance Mohm -#define SYM_BATT_FULL 0x63 // 099 Battery full -#define SYM_BATT_5 0x64 // 100 Battery -#define SYM_BATT_4 0x65 // 101 Battery -#define SYM_BATT_3 0x66 // 102 Battery -#define SYM_BATT_2 0x67 // 103 Battery -#define SYM_BATT_1 0x68 // 104 Battery -#define SYM_BATT_EMPTY 0x69 // 105 Battery empty +#define SYM_BATT_FULL 0x63 // 099 Battery full +#define SYM_BATT_5 0x64 // 100 Battery +#define SYM_BATT_4 0x65 // 101 Battery +#define SYM_BATT_3 0x66 // 102 Battery +#define SYM_BATT_2 0x67 // 103 Battery +#define SYM_BATT_1 0x68 // 104 Battery +#define SYM_BATT_EMPTY 0x69 // 105 Battery empty -#define SYM_AMP 0x6A // 106 AMPS -#define SYM_WH 0x6D // 109 WH -#define SYM_WH_KM 0x6E // 110 WH/KM -#define SYM_WH_MI 0x6F // 111 WH/MI -#define SYM_WH_NM 0x70 // 112 Wh/NM -#define SYM_WATT 0x71 // 113 WATTS -#define SYM_MW 0x72 // 114 mW -#define SYM_KILOWATT 0x73 // 115 kW +#define SYM_AMP 0x6A // 106 AMPS +#define SYM_WH 0x6D // 109 WH +#define SYM_WH_KM 0x6E // 110 WH/KM +#define SYM_WH_MI 0x6F // 111 WH/MI +#define SYM_WH_NM 0x70 // 112 Wh/NM +#define SYM_WATT 0x71 // 113 WATTS +#define SYM_MW 0x72 // 114 mW +#define SYM_KILOWATT 0x73 // 115 kW -#define SYM_FT 0x74 // 116 FEET -#define SYM_TRIP_DIST 0x75 // 117 Trip distance -#define SYM_TOTAL 0x75 // 117 Total -#define SYM_ALT_M 0x76 // 118 ALT M -#define SYM_ALT_KM 0x77 // 119 ALT KM -#define SYM_ALT_FT 0x78 // 120 ALT FT -#define SYM_ALT_KFT 0x79 // 121 Alt KFT -#define SYM_DIST_M 0x7A // 122 DIS M -// 0x7B // 123 to 125 ASCII -#define SYM_DIST_KM 0x7E // 126 DIST KM -#define SYM_DIST_FT 0x7F // 127 DIST FT -#define SYM_DIST_MI 0x80 // 128 DIST MI -#define SYM_DIST_NM 0x81 // 129 DIST NM -#define SYM_M 0x82 // 130 M -#define SYM_KM 0x83 // 131 KM -#define SYM_MI 0x84 // 132 MI -#define SYM_NM 0x85 // 133 NM +#define SYM_FT 0x74 // 116 FEET +#define SYM_TRIP_DIST 0x75 // 117 Trip distance +#define SYM_TOTAL 0x75 // 117 Total +#define SYM_ALT_M 0x76 // 118 ALT M +#define SYM_ALT_KM 0x77 // 119 ALT KM +#define SYM_ALT_FT 0x78 // 120 ALT FT +#define SYM_ALT_KFT 0x79 // 121 Alt KFT +#define SYM_DIST_M 0x7A // 122 DIS M +// 0x7B // 123 to 125 ASCII +#define SYM_DIST_KM 0x7E // 126 DIST KM +#define SYM_DIST_FT 0x7F // 127 DIST FT +#define SYM_DIST_MI 0x80 // 128 DIST MI +#define SYM_DIST_NM 0x81 // 129 DIST NM +#define SYM_M 0x82 // 130 M +#define SYM_KM 0x83 // 131 KM +#define SYM_MI 0x84 // 132 MI +#define SYM_NM 0x85 // 133 NM -#define SYM_WIND_HORIZONTAL 0x86 // 134 Air speed horizontal -#define SYM_WIND_VERTICAL 0x87 // 135 Air speed vertical -#define SYM_3D_KMH 0x88 // 136 KM/H 3D -#define SYM_3D_MPH 0x89 // 137 MPH 3D -#define SYM_3D_KT 0x8A // 138 Knots 3D -#define SYM_RPM 0x8B // 139 RPM -#define SYM_AIR 0x8C // 140 Air speed -#define SYM_FTS 0x8D // 141 FT/S -#define SYM_100FTM 0x8E // 142 100 Feet per Min -#define SYM_MS 0x8F // 143 M/S -#define SYM_KMH 0x90 // 144 KM/H -#define SYM_MPH 0x91 // 145 MPH -#define SYM_KT 0x92 // 146 Knots +#define SYM_WIND_HORIZONTAL 0x86 // 134 Air speed horizontal +#define SYM_WIND_VERTICAL 0x87 // 135 Air speed vertical +#define SYM_3D_KMH 0x88 // 136 KM/H 3D +#define SYM_3D_MPH 0x89 // 137 MPH 3D +#define SYM_3D_KT 0x8A // 138 Knots 3D +#define SYM_RPM 0x8B // 139 RPM +#define SYM_AIR 0x8C // 140 Air speed +#define SYM_FTS 0x8D // 141 FT/S +#define SYM_100FTM 0x8E // 142 100 Feet per Min +#define SYM_MS 0x8F // 143 M/S +#define SYM_KMH 0x90 // 144 KM/H +#define SYM_MPH 0x91 // 145 MPH +#define SYM_KT 0x92 // 146 Knots -#define SYM_MAH_MI_0 0x93 // 147 mAh/mi left -#define SYM_MAH_MI_1 0x94 // 148 mAh/mi right -#define SYM_THR 0x95 // 149 Throttle -#define SYM_TEMP_F 0x96 // 150 °F -#define SYM_TEMP_C 0x97 // 151 °C -// 0x98 // 152 Home point map -#define SYM_BLANK 0x20 // 32 blank (space) -#define SYM_ON_H 0x9A // 154 ON HR -#define SYM_FLY_H 0x9B // 155 FLY HR -#define SYM_ON_M 0x9E // 158 On MIN -#define SYM_FLY_M 0x9F // 159 FL MIN -#define SYM_GLIDESLOPE 0x9C // 156 Glideslope -#define SYM_WAYPOINT 0x9D // 157 Waypoint -#define SYM_CLOCK 0xA0 // 160 Clock +#define SYM_MAH_MI_0 0x93 // 147 mAh/mi left +#define SYM_MAH_MI_1 0x94 // 148 mAh/mi right +#define SYM_THR 0x95 // 149 Throttle +#define SYM_TEMP_F 0x96 // 150 °F +#define SYM_TEMP_C 0x97 // 151 °C +// 0x98 // 152 Home point map +#define SYM_BLANK 0x20 // 32 blank (space) +#define SYM_ON_H 0x9A // 154 ON HR +#define SYM_FLY_H 0x9B // 155 FLY HR +#define SYM_ON_M 0x9E // 158 On MIN +#define SYM_FLY_M 0x9F // 159 FL MIN +#define SYM_GLIDESLOPE 0x9C // 156 Glideslope +#define SYM_WAYPOINT 0x9D // 157 Waypoint +#define SYM_CLOCK 0xA0 // 160 Clock -#define SYM_ZERO_HALF_TRAILING_DOT 0xA1 // 161 to 170 Numbers with trailing dot -#define SYM_ZERO_HALF_LEADING_DOT 0xB1 // 177 to 186 Numbers with leading dot +#define SYM_ZERO_HALF_TRAILING_DOT 0xA1 // 161 to 170 Numbers with trailing dot +#define SYM_ZERO_HALF_LEADING_DOT 0xB1 // 177 to 186 Numbers with leading dot -#define SYM_AUTO_THR0 0xAB // 171 Auto-throttle left -#define SYM_AUTO_THR1 0xAC // 172 Auto-throttle right -#define SYM_ROLL_LEFT 0xAD // 173 Sym roll left -#define SYM_ROLL_LEVEL 0xAE // 174 Sym roll horizontal -#define SYM_ROLL_RIGHT 0xAF // 175 Sym roll right -#define SYM_PITCH_UP 0xB0 // 176 Pitch up -#define SYM_PITCH_DOWN 0xBB // 187 Pitch down -#define SYM_GFORCE 0xBC // 188 Gforce (all axis) -#define SYM_GFORCE_X 0xBD // 189 Gforce X -#define SYM_GFORCE_Y 0xBE // 190 Gforce Y -#define SYM_GFORCE_Z 0xBF // 191 Gforce Z +#define SYM_AUTO_THR0 0xAB // 171 Auto-throttle left +#define SYM_AUTO_THR1 0xAC // 172 Auto-throttle right +#define SYM_ROLL_LEFT 0xAD // 173 Sym roll left +#define SYM_ROLL_LEVEL 0xAE // 174 Sym roll horizontal +#define SYM_ROLL_RIGHT 0xAF // 175 Sym roll right +#define SYM_PITCH_UP 0xB0 // 176 Pitch up +#define SYM_PITCH_DOWN 0xBB // 187 Pitch down +#define SYM_GFORCE 0xBC // 188 Gforce (all axis) +#define SYM_GFORCE_X 0xBD // 189 Gforce X +#define SYM_GFORCE_Y 0xBE // 190 Gforce Y +#define SYM_GFORCE_Z 0xBF // 191 Gforce Z -#define SYM_BARO_TEMP 0xC0 // 192 -#define SYM_IMU_TEMP 0xC1 // 193 -#define SYM_TEMP 0xC2 // 194 Thermometer icon -#define SYM_TEMP_SENSOR_FIRST 0xC2 // 194 -#define SYM_ESC_TEMP 0xC3 // 195 -#define SYM_TEMP_SENSOR_LAST 0xC7 // 199 +#define SYM_BARO_TEMP 0xC0 // 192 +#define SYM_IMU_TEMP 0xC1 // 193 +#define SYM_TEMP 0xC2 // 194 Thermometer icon +#define SYM_TEMP_SENSOR_FIRST 0xC2 // 194 +#define SYM_ESC_TEMP 0xC3 // 195 +#define SYM_TEMP_SENSOR_LAST 0xC7 // 199 #define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1) -#define SYM_HEADING_N 0xC8 // 200 Heading Graphic north -#define SYM_HEADING_S 0xC9 // 201 Heading Graphic south -#define SYM_HEADING_E 0xCA // 202 Heading Graphic east -#define SYM_HEADING_W 0xCB // 203 Heading Graphic west -#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic -#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic -#define SYM_MAX 0xCE // 206 MAX symbol -#define SYM_PROFILE 0xCF // 207 Profile symbol -#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High -#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid -#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low -#define SYM_AH 0xD3 // 211 Amphours symbol -#define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance -#define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes -#define SYM_AH_V_FT_0 0xD6 // 214 mAh/v-ft left -#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right -#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left -#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right -#define SYM_FLIGHT_MINS_REMAINING 0xDA // 218 Flight time (mins) remaining -#define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining -#define SYM_GROUND_COURSE 0xDC // 220 Ground course -#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error +#define SYM_HEADING_N 0xC8 // 200 Heading Graphic north +#define SYM_HEADING_S 0xC9 // 201 Heading Graphic south +#define SYM_HEADING_E 0xCA // 202 Heading Graphic east +#define SYM_HEADING_W 0xCB // 203 Heading Graphic west +#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic +#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic +#define SYM_MAX 0xCE // 206 MAX symbol +#define SYM_PROFILE 0xCF // 207 Profile symbol +#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High +#define SYM_SWITCH_INDICATOR_MID 0xD1 // 209 Switch Mid +#define SYM_SWITCH_INDICATOR_HIGH 0xD2 // 210 Switch Low +#define SYM_AH 0xD3 // 211 Amphours symbol +#define SYM_GLIDE_DIST 0xD4 // 212 Glide Distance +#define SYM_GLIDE_MINS 0xD5 // 213 Glide Minutes +#define SYM_AH_V_FT_0 0xD6 // 214 mAh/v-ft left +#define SYM_AH_V_FT_1 0xD7 // 215 mAh/v-ft right +#define SYM_AH_V_M_0 0xD8 // 216 mAh/v-m left +#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right +#define SYM_FLIGHT_MINS_REMAINING 0xDA // 218 Flight time (mins) remaining +#define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining +#define SYM_GROUND_COURSE 0xDC // 220 Ground course +#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error -#define SYM_LOGO_START 0x101 // 257 to 297, INAV logo -#define SYM_LOGO_WIDTH 10 -#define SYM_LOGO_HEIGHT 4 +#define SYM_LOGO_START 0x101 // 257 to 297, INAV logo +#define SYM_LOGO_WIDTH 10 +#define SYM_LOGO_HEIGHT 4 -#define SYM_AH_LEFT 0x12C // 300 Arrow left -#define SYM_AH_RIGHT 0x12D // 301 Arrow right -#define SYM_AH_DECORATION_MIN 0x12E // 302 to 307 Scrolling -#define SYM_AH_DECORATION 0x131 // 305 Scrolling -#define SYM_AH_DECORATION_MAX 0x133 // 307 Scrolling +#define SYM_AH_LEFT 0x12C // 300 Arrow left +#define SYM_AH_RIGHT 0x12D // 301 Arrow right +#define SYM_AH_DECORATION_MIN 0x12E // 302 to 307 Scrolling +#define SYM_AH_DECORATION 0x131 // 305 Scrolling +#define SYM_AH_DECORATION_MAX 0x133 // 307 Scrolling #define SYM_AH_DECORATION_COUNT (SYM_AH_DECORATION_MAX - SYM_AH_DECORATION_MIN + 1) // Scrolling -#define SYM_AH_CH_LEFT 0x13A // 314 Crossair left -#define SYM_AH_CH_RIGHT 0x13B // 315 Crossair right +#define SYM_AH_CH_LEFT 0x13A // 314 Crossair left +#define SYM_AH_CH_RIGHT 0x13B // 315 Crossair right -#define SYM_ARROW_UP 0x13C // 316 Direction arrow 0° -#define SYM_ARROW_2 0x13D // 317 Direction arrow 22.5° -#define SYM_ARROW_3 0x13E // 318 Direction arrow 45° -#define SYM_ARROW_4 0x13F // 319 Direction arrow 67.5° -#define SYM_ARROW_RIGHT 0x140 // 320 Direction arrow 90° -#define SYM_ARROW_6 0x141 // 321 Direction arrow 112.5° -#define SYM_ARROW_7 0x142 // 322 Direction arrow 135° -#define SYM_ARROW_8 0x143 // 323 Direction arrow 157.5° -#define SYM_ARROW_DOWN 0x144 // 324 Direction arrow 180° -#define SYM_ARROW_10 0x145 // 325 Direction arrow 202.5° -#define SYM_ARROW_11 0x146 // 326 Direction arrow 225° -#define SYM_ARROW_12 0x147 // 327 Direction arrow 247.5° -#define SYM_ARROW_LEFT 0x148 // 328 Direction arrow 270° -#define SYM_ARROW_14 0x149 // 329 Direction arrow 292.5° -#define SYM_ARROW_15 0x14A // 330 Direction arrow 315° -#define SYM_ARROW_16 0x14B // 331 Direction arrow 337.5° +#define SYM_ARROW_UP 0x13C // 316 Direction arrow 0° +#define SYM_ARROW_2 0x13D // 317 Direction arrow 22.5° +#define SYM_ARROW_3 0x13E // 318 Direction arrow 45° +#define SYM_ARROW_4 0x13F // 319 Direction arrow 67.5° +#define SYM_ARROW_RIGHT 0x140 // 320 Direction arrow 90° +#define SYM_ARROW_6 0x141 // 321 Direction arrow 112.5° +#define SYM_ARROW_7 0x142 // 322 Direction arrow 135° +#define SYM_ARROW_8 0x143 // 323 Direction arrow 157.5° +#define SYM_ARROW_DOWN 0x144 // 324 Direction arrow 180° +#define SYM_ARROW_10 0x145 // 325 Direction arrow 202.5° +#define SYM_ARROW_11 0x146 // 326 Direction arrow 225° +#define SYM_ARROW_12 0x147 // 327 Direction arrow 247.5° +#define SYM_ARROW_LEFT 0x148 // 328 Direction arrow 270° +#define SYM_ARROW_14 0x149 // 329 Direction arrow 292.5° +#define SYM_ARROW_15 0x14A // 330 Direction arrow 315° +#define SYM_ARROW_16 0x14B // 331 Direction arrow 337.5° -#define SYM_AH_H_START 0x14C // 332 to 340 Horizontal AHI -#define SYM_AH_V_START 0x15A // 346 to 351 Vertical AHI +#define SYM_AH_H_START 0x14C // 332 to 340 Horizontal AHI +#define SYM_AH_V_START 0x15A // 346 to 351 Vertical AHI -#define SYM_VARIO_UP_2A 0x155 // 341 Vario up up -#define SYM_VARIO_UP_1A 0x156 // 342 Vario up -#define SYM_VARIO_DOWN_1A 0x157 // 343 Vario down -#define SYM_VARIO_DOWN_2A 0x158 // 344 Vario down down -#define SYM_ALT 0x159 // 345 ALT +#define SYM_VARIO_UP_2A 0x155 // 341 Vario up up +#define SYM_VARIO_UP_1A 0x156 // 342 Vario up +#define SYM_VARIO_DOWN_1A 0x157 // 343 Vario down +#define SYM_VARIO_DOWN_2A 0x158 // 344 Vario down down +#define SYM_ALT 0x159 // 345 ALT -#define SYM_HUD_SIGNAL_0 0x160 // 352 Hud signal icon Lost -#define SYM_HUD_SIGNAL_1 0x161 // 353 Hud signal icon 25% -#define SYM_HUD_SIGNAL_2 0x162 // 354 Hud signal icon 50% -#define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75% -#define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100% +#define SYM_HUD_SIGNAL_0 0x160 // 352 Hud signal icon Lost +#define SYM_HUD_SIGNAL_1 0x161 // 353 Hud signal icon 25% +#define SYM_HUD_SIGNAL_2 0x162 // 354 Hud signal icon 50% +#define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75% +#define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100% -#define SYM_HOME_DIST 0x165 // 357 DIST -#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center -#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing +#define SYM_HOME_DIST 0x165 // 357 DIST +#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center +#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing -#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 -#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 -#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 -#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6 -#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7 -#define SYM_AH_CH_TYPE8 0x19F // 415 to 417, crosshair 8 +#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 +#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 +#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 +#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6 +#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7 +#define SYM_AH_CH_TYPE8 0x19F // 415 to 417, crosshair 8 -#define SYM_AH_CH_AIRCRAFT0 0x1A2 // 418 Crossair aircraft left -#define SYM_AH_CH_AIRCRAFT1 0x1A3 // 419 Crossair aircraft -#define SYM_AH_CH_AIRCRAFT2 0x1A4 // 420 Crossair aircraft center -#define SYM_AH_CH_AIRCRAFT3 0x1A5 // 421 Crossair aircraft -#define SYM_AH_CH_AIRCRAFT4 0x1A6 // 422 Crossair aircraft RIGHT +#define SYM_AH_CH_AIRCRAFT0 0x1A2 // 418 Crossair aircraft left +#define SYM_AH_CH_AIRCRAFT1 0x1A3 // 419 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT2 0x1A4 // 420 Crossair aircraft center +#define SYM_AH_CH_AIRCRAFT3 0x1A5 // 421 Crossair aircraft +#define SYM_AH_CH_AIRCRAFT4 0x1A6 // 422 Crossair aircraft RIGHT -#define SYM_HUD_ARROWS_L1 0x1AE // 430 1 arrow left -#define SYM_HUD_ARROWS_L2 0x1AF // 431 2 arrows left -#define SYM_HUD_ARROWS_L3 0x1B0 // 432 3 arrows left -#define SYM_HUD_ARROWS_R1 0x1B1 // 433 1 arrow right -#define SYM_HUD_ARROWS_R2 0x1B2 // 434 2 arrows right -#define SYM_HUD_ARROWS_R3 0x1B3 // 435 3 arrows right -#define SYM_HUD_ARROWS_U1 0x1B4 // 436 1 arrow up -#define SYM_HUD_ARROWS_U2 0x1B5 // 437 2 arrows up -#define SYM_HUD_ARROWS_U3 0x1B6 // 438 3 arrows up -#define SYM_HUD_ARROWS_D1 0x1B7 // 439 1 arrow down -#define SYM_HUD_ARROWS_D2 0x1B8 // 440 2 arrows down -#define SYM_HUD_ARROWS_D3 0x1B9 // 441 3 arrows down +#define SYM_HUD_ARROWS_L1 0x1AE // 430 1 arrow left +#define SYM_HUD_ARROWS_L2 0x1AF // 431 2 arrows left +#define SYM_HUD_ARROWS_L3 0x1B0 // 432 3 arrows left +#define SYM_HUD_ARROWS_R1 0x1B1 // 433 1 arrow right +#define SYM_HUD_ARROWS_R2 0x1B2 // 434 2 arrows right +#define SYM_HUD_ARROWS_R3 0x1B3 // 435 3 arrows right +#define SYM_HUD_ARROWS_U1 0x1B4 // 436 1 arrow up +#define SYM_HUD_ARROWS_U2 0x1B5 // 437 2 arrows up +#define SYM_HUD_ARROWS_U3 0x1B6 // 438 3 arrows up +#define SYM_HUD_ARROWS_D1 0x1B7 // 439 1 arrow down +#define SYM_HUD_ARROWS_D2 0x1B8 // 440 2 arrows down +#define SYM_HUD_ARROWS_D3 0x1B9 // 441 3 arrows down -#define SYM_HUD_CARDINAL 0x1BA // 442-453 Cardinal direction in 30 degree segments +#define SYM_HUD_CARDINAL 0x1BA // 442-453 Cardinal direction in 30 degree segments #define SYM_SERVO_PAN_IS_CENTRED 0x1C6 // 454 Pan servo is centred #define SYM_SERVO_PAN_IS_OFFSET_L 0x1C7 // 455 Pan servo is offset left #define SYM_SERVO_PAN_IS_OFFSET_R 0x1C8 // 456 Pan servo is offset right +#define SYM_PILOT_LOGO_SML_L 0x1D7 // 471 small Pilot logo L +#define SYM_PILOT_LOGO_SML_R 0x1D8 // 471 small Pilot logo R +#define SYM_PILOT_LOGO_LRG_START 0x1D9 // 473 to 511, Pilot logo + #else -#define TEMP_SENSOR_SYM_COUNT 0 +#define TEMP_SENSOR_SYM_COUNT 0 #endif // USE_OSD diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 947c03845d..b3edc03f05 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4355,8 +4355,18 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayCommitTransaction(osdDisplayPort); } -// called when motors armed -static void osdShowArmed(void) +// HD arming screen. based on the minimum HD OSD grid size of 50 x 18 +static void osdShowHDArmScreen(void) +{ + dateTime_t dt; + char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[30]; + char *date; + char *time; +} + +static void osdShowSDArmScreen(void) { dateTime_t dt; char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; @@ -4367,7 +4377,6 @@ static void osdShowArmed(void) // We need 12 visible rows, start row never < first fully visible row 1 uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; - displayClearScreen(osdDisplayPort); strcpy(buf, "ARMED"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); y += 2; @@ -4436,6 +4445,18 @@ static void osdShowArmed(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); } +// called when motors armed +static void osdShowArmed(void) +{ + displayClearScreen(osdDisplayPort); + + if (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS) { + osdShowHDArmScreen(); + } else { + osdShowSDArmScreen(); + } +} + static void osdFilterData(timeUs_t currentTimeUs) { static timeUs_t lastRefresh = 0; float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); From 3b9e2a24ff26e697af412c000741bf3998466989 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 15:07:20 +0100 Subject: [PATCH 11/62] add MR flight sanity and forced Angle --- src/main/fc/fc_core.c | 67 +++++++++++++++++++------------- src/main/navigation/navigation.c | 7 ++-- 2 files changed, 44 insertions(+), 30 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 46ef140f84..187613a9ee 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -104,6 +104,7 @@ enum { #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10 +#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000 timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; @@ -119,6 +120,7 @@ static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; timeMs_t emergInflightRearmTimeout = 0; +timeMs_t mcEmergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -314,11 +316,11 @@ static void updateArmingStatus(void) /* CHECK: Do not allow arming if Servo AutoTrim is enabled */ if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } #ifdef USE_DSHOT /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ @@ -433,7 +435,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -501,19 +503,20 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) - -void releaseSharedTelemetryPorts(void) { - serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - while (sharedPort) { - mspSerialReleasePortIfAllocated(sharedPort); - sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - } -} - bool emergInflightRearmEnabled(void) { - return millis() < emergInflightRearmTimeout; + /* Emergency rearm allowed within emergInflightRearmTimeout window. + * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (STATE(MULTIROTOR)) { + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm + if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + } else { + mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR + } + } + return currentTimeMs < emergInflightRearmTimeout; } void tryArm(void) @@ -594,6 +597,16 @@ void tryArm(void) } } +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) + +void releaseSharedTelemetryPorts(void) { + serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + while (sharedPort) { + mspSerialReleasePortIfAllocated(sharedPort); + sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + } +} + void processRx(timeUs_t currentTimeUs) { // Calculate RPY channel data @@ -644,9 +657,12 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } + // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { // bumpless transfer to Level mode canUseHorizonMode = false; @@ -813,7 +829,6 @@ void processRx(timeUs_t currentTimeUs) } } #endif - } // Function for loop trigger @@ -929,15 +944,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs) //Servos should be filtered or written only when mixer is using servos or special feaures are enabled #ifdef USE_SIMULATOR - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if (isServoOutputEnabled()) { - writeServos(); - } + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { + if (isServoOutputEnabled()) { + writeServos(); + } - if (motorControlEnable) { - writeMotors(); - } - } + if (motorControlEnable) { + writeMotors(); + } + } #else if (isServoOutputEnabled()) { writeServos(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 04c910aa63..96d99ebf3b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2855,10 +2855,9 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - bool inFlightSanityCheck = false; - if (STATE(AIRPLANE)) { - inFlightSanityCheck = isGPSHeadingValid(); - } + // Multirotor flight sanity checked after disarm so always true here + bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; } From 7bb9b69a5f5b89e5d75534248b0931929d0b990f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 22:16:45 +0100 Subject: [PATCH 12/62] Fix logic --- src/main/fc/fc_core.c | 24 +++++++++++++++++------- src/main/navigation/navigation.c | 7 +++---- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 187613a9ee..60c658809e 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -435,7 +435,10 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); + if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { + emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; + } + DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -506,17 +509,23 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + return false; + } + if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm - if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm + if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + return false; // MR doesn't appear to be flying so don't allow emergency rearm } else { mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR } } - return currentTimeMs < emergInflightRearmTimeout; + + return true; } void tryArm(void) @@ -555,6 +564,7 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; + emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -657,7 +667,7 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 96d99ebf3b..759ddd42f3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2808,10 +2808,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (!emergInflightRearmEnabled()) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + landingDetectorIsActive = false; + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } From ce44da742cd4839cc8a35c0c3118fbd6e0e678af Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 24 Aug 2023 15:44:22 +0100 Subject: [PATCH 13/62] Initial code update - Switch small pilot logo to 3 characters - Added base code for arm screen --- src/main/drivers/osd_symbols.h | 5 +- src/main/io/osd.c | 7670 ++++++++++++++++---------------- 2 files changed, 3887 insertions(+), 3788 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 8dfaa10186..a1b84d52b1 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -258,8 +258,9 @@ #define SYM_SERVO_PAN_IS_OFFSET_L 0x1C7 // 455 Pan servo is offset left #define SYM_SERVO_PAN_IS_OFFSET_R 0x1C8 // 456 Pan servo is offset right -#define SYM_PILOT_LOGO_SML_L 0x1D7 // 471 small Pilot logo L -#define SYM_PILOT_LOGO_SML_R 0x1D8 // 471 small Pilot logo R +#define SYM_PILOT_LOGO_SML_L 0x1D6 // 470 small Pilot logo Left +#define SYM_PILOT_LOGO_SML_C 0x1D7 // 471 small Pilot logo Centre +#define SYM_PILOT_LOGO_SML_R 0x1D8 // 472 small Pilot logo Right #define SYM_PILOT_LOGO_LRG_START 0x1D9 // 473 to 511, Pilot logo #else diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b3edc03f05..f6289520bc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -142,8 +142,8 @@ // Wrap all string constants intenteded for display as messages with // this macro to ensure compile time length validation. #define OSD_MESSAGE_STR(x) ({ \ - STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \ - x; \ + STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \ + x; \ }) #define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9') @@ -164,17 +164,17 @@ static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { - uint16_t max_speed; - uint16_t max_3D_speed; - uint16_t max_air_speed; - uint16_t min_voltage; // /100 - int16_t max_current; - int32_t max_power; - int16_t min_rssi; - int16_t min_lq; // for CRSF - int16_t min_rssi_dbm; // for CRSF - int32_t max_altitude; - uint32_t max_distance; + uint16_t max_speed; + uint16_t max_3D_speed; + uint16_t max_air_speed; + uint16_t min_voltage; // /100 + int16_t max_current; + int32_t max_power; + int16_t min_rssi; + int16_t min_lq; // for CRSF + int16_t min_rssi_dbm; // for CRSF + int32_t max_altitude; + uint32_t max_distance; } statistic_t; static statistic_t stats; @@ -187,8 +187,8 @@ static bool fullRedraw = false; static uint8_t armState; typedef struct osdMapData_s { - uint32_t scale; - char referenceSymbol; + uint32_t scale; + char referenceSymbol; } osdMapData_t; static osdMapData_t osdMapData; @@ -208,28 +208,28 @@ PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { - savingSettings = true; + savingSettings = true; } void osdShowEEPROMSavedNotification(void) { - savingSettings = false; - notify_settings_saved = millis() + 5000; + savingSettings = false; + notify_settings_saved = millis() + 5000; } bool osdDisplayIsPAL(void) { - return displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL; + return displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL; } bool osdDisplayIsHD(void) { - if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO) - { - return true; - } - return false; + if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO) + { + return true; + } + return false; } @@ -239,11 +239,11 @@ bool osdDisplayIsHD(void) */ static void osdLeftAlignString(char *buff) { - uint8_t sp = 0, ch = 0; - uint8_t len = strlen(buff); - while (buff[sp] == ' ') sp++; - for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; - for (sp = ch; sp < len; sp++) buff[sp] = ' '; + uint8_t sp = 0, ch = 0; + uint8_t len = strlen(buff); + while (buff[sp] == ' ') sp++; + for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; + for (sp = ch; sp < len; sp++) buff[sp] = ' '; } /* @@ -253,29 +253,29 @@ static void osdLeftAlignString(char *buff) */ /* void osdSimpleDistanceSymbol(char *buff, int32_t dist) { - int32_t convertedDistance; - char suffix; + int32_t convertedDistance; + char suffix; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - convertedDistance = CENTIMETERS_TO_FEET(dist); - suffix = SYM_ALT_FT; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - convertedDistance = CENTIMETERS_TO_METERS(dist); - suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + convertedDistance = CENTIMETERS_TO_FEET(dist); + suffix = SYM_ALT_FT; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + convertedDistance = CENTIMETERS_TO_METERS(dist); + suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode + break; + } - tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases - buff[5] = suffix; - buff[6] = '\0'; + tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases + buff[5] = suffix; + buff[6] = '\0'; } */ /** @@ -285,58 +285,58 @@ static void osdLeftAlignString(char *buff) */ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { - uint8_t digits = 3U; // Total number of digits (including decimal point) - uint8_t sym_index = 3U; // Position (index) at buffer of units symbol - uint8_t symbol_m = SYM_DIST_M; - uint8_t symbol_km = SYM_DIST_KM; - uint8_t symbol_ft = SYM_DIST_FT; - uint8_t symbol_mi = SYM_DIST_MI; - uint8_t symbol_nm = SYM_DIST_NM; + uint8_t digits = 3U; // Total number of digits (including decimal point) + uint8_t sym_index = 3U; // Position (index) at buffer of units symbol + uint8_t symbol_m = SYM_DIST_M; + uint8_t symbol_km = SYM_DIST_KM; + uint8_t symbol_ft = SYM_DIST_FT; + uint8_t symbol_mi = SYM_DIST_MI; + uint8_t symbol_nm = SYM_DIST_NM; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so up no switch to scaled decimal occurs above 99 - digits = 4U; - sym_index = 4U; - // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode - symbol_m = SYM_ALT_M; - symbol_km = SYM_ALT_KM; - symbol_ft = SYM_ALT_FT; - symbol_mi = SYM_MI; - symbol_nm = SYM_MI; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so up no switch to scaled decimal occurs above 99 + digits = 4U; + sym_index = 4U; + // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode + symbol_m = SYM_ALT_M; + symbol_km = SYM_ALT_KM; + symbol_ft = SYM_ALT_FT; + symbol_mi = SYM_MI; + symbol_nm = SYM_MI; + } #endif - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { - buff[sym_index] = symbol_mi; - } else { - buff[sym_index] = symbol_ft; - } - buff[sym_index + 1] = '\0'; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { - buff[sym_index] = symbol_km; - } else { - buff[sym_index] = symbol_m; - } - buff[sym_index + 1] = '\0'; - break; - case OSD_UNIT_GA: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { - buff[sym_index] = symbol_nm; - } else { - buff[sym_index] = symbol_ft; - } - buff[sym_index + 1] = '\0'; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { + buff[sym_index] = symbol_mi; + } else { + buff[sym_index] = symbol_ft; + } + buff[sym_index + 1] = '\0'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { + buff[sym_index] = symbol_km; + } else { + buff[sym_index] = symbol_m; + } + buff[sym_index + 1] = '\0'; + break; + case OSD_UNIT_GA: + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { + buff[sym_index] = symbol_nm; + } else { + buff[sym_index] = symbol_ft; + } + buff[sym_index + 1] = '\0'; + break; + } } /** @@ -345,45 +345,45 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) */ static void osdFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); - } - break; - case OSD_UNIT_GA: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < 100000) { - // Show feet when dist < 1000ft - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show nautical miles when dist >= 1000ft - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), - (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), SYM_NM); - } - break; - } + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); + } + break; + case OSD_UNIT_GA: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < 100000) { + // Show feet when dist < 1000ft + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show nautical miles when dist >= 1000ft + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), + (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), SYM_NM); + } + break; + } } /** @@ -392,20 +392,20 @@ static void osdFormatDistanceStr(char *buff, int32_t dist) */ static int32_t osdConvertVelocityToUnit(int32_t vel) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph - case OSD_UNIT_METRIC: - return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh - case OSD_UNIT_GA: - return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots - } - // Unreachable - return -1; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph + case OSD_UNIT_METRIC: + return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh + case OSD_UNIT_GA: + return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots + } + // Unreachable + return -1; } /** @@ -416,41 +416,41 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) */ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } - break; - case OSD_UNIT_METRIC: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } - break; - case OSD_UNIT_GA: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } + break; + case OSD_UNIT_METRIC: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } + break; + case OSD_UNIT_GA: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } + break; + } } /** * Returns the average velocity. This always uses stats, so can be called as an OSD element later if wanted, to show a real time average */ static void osdGenerateAverageVelocityStr(char* buff) { - uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); - osdFormatVelocityStr(buff, cmPerSec, false, false); + uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); + osdFormatVelocityStr(buff, cmPerSec, false, false); } /** @@ -462,35 +462,35 @@ static void osdGenerateAverageVelocityStr(char* buff) { #ifdef USE_WIND_ESTIMATOR static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) { - int32_t centivalue; - char suffix; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - centivalue = CMSEC_TO_CENTIMPH(ws); - suffix = SYM_MPH; - break; - case OSD_UNIT_GA: - centivalue = CMSEC_TO_CENTIKNOTS(ws); - suffix = SYM_KT; - break; - default: - case OSD_UNIT_METRIC: - centivalue = CMSEC_TO_CENTIKPH(ws); - suffix = SYM_KMH; - break; - } + int32_t centivalue; + char suffix; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centivalue = CMSEC_TO_CENTIMPH(ws); + suffix = SYM_MPH; + break; + case OSD_UNIT_GA: + centivalue = CMSEC_TO_CENTIKNOTS(ws); + suffix = SYM_KT; + break; + default: + case OSD_UNIT_METRIC: + centivalue = CMSEC_TO_CENTIKPH(ws); + suffix = SYM_KMH; + break; + } - osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); + osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); - if (!isValid && ((millis() / 1000) % 4 < 2)) - suffix = '*'; + if (!isValid && ((millis() / 1000) % 4 < 2)) + suffix = '*'; - buff[3] = suffix; - buff[4] = '\0'; + buff[3] = suffix; + buff[4] = '\0'; } #endif @@ -500,29 +500,29 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ /* void osdSimpleAltitudeSymbol(char *buff, int32_t alt) { - int32_t convertedAltutude = 0; - char suffix = '\0'; + int32_t convertedAltutude = 0; + char suffix = '\0'; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - convertedAltutude = CENTIMETERS_TO_FEET(alt); - suffix = SYM_ALT_FT; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - convertedAltutude = CENTIMETERS_TO_METERS(alt); - suffix = SYM_ALT_M; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + convertedAltutude = CENTIMETERS_TO_FEET(alt); + suffix = SYM_ALT_FT; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + convertedAltutude = CENTIMETERS_TO_METERS(alt); + suffix = SYM_ALT_M; + break; + } - tfp_sprintf(buff, "%4d", (int) convertedAltutude); - buff[4] = suffix; - buff[5] = '\0'; + tfp_sprintf(buff, "%4d", (int) convertedAltutude); + buff[4] = suffix; + buff[5] = '\0'; } */ /** @@ -532,54 +532,54 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { - uint8_t totalDigits = 4U; - uint8_t digits = 4U; - uint8_t symbolIndex = 4U; - uint8_t symbolKFt = SYM_ALT_KFT; + uint8_t totalDigits = 4U; + uint8_t digits = 4U; + uint8_t symbolIndex = 4U; + uint8_t symbolKFt = SYM_ALT_KFT; - if (alt >= 0) { - digits = 3U; - buff[0] = ' '; - } + if (alt >= 0) { + digits = 3U; + buff[0] = ' '; + } #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - totalDigits++; - digits++; - symbolIndex++; - symbolKFt = SYM_ALT_FT; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + totalDigits++; + digits++; + symbolIndex++; + symbolKFt = SYM_ALT_FT; + } #endif - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { - // Scaled to kft - buff[symbolIndex++] = symbolKFt; - } else { - // Formatted in feet - buff[symbolIndex++] = SYM_ALT_FT; - } - buff[symbolIndex] = '\0'; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // alt is alredy in cm - if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { - // Scaled to km - buff[symbolIndex++] = SYM_ALT_KM; - } else { - // Formatted in m - buff[symbolIndex++] = SYM_ALT_M; - } - buff[symbolIndex] = '\0'; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { + // Scaled to kft + buff[symbolIndex++] = symbolKFt; + } else { + // Formatted in feet + buff[symbolIndex++] = SYM_ALT_FT; + } + buff[symbolIndex] = '\0'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // alt is alredy in cm + if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { + // Scaled to km + buff[symbolIndex++] = SYM_ALT_KM; + } else { + // Formatted in m + buff[symbolIndex++] = SYM_ALT_M; + } + buff[symbolIndex] = '\0'; + break; + } } /** @@ -588,52 +588,52 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) */ static void osdFormatAltitudeStr(char *buff, int32_t alt) { - int32_t value; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - value = CENTIMETERS_TO_FEET(alt); - tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - value = CENTIMETERS_TO_METERS(alt); - tfp_sprintf(buff, "%d%c", (int)value, SYM_M); - break; - } + int32_t value; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + value = CENTIMETERS_TO_FEET(alt); + tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + value = CENTIMETERS_TO_METERS(alt); + tfp_sprintf(buff, "%d%c", (int)value, SYM_M); + break; + } } static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h) { - uint32_t value = seconds; - char sym = sym_m; - // Maximum value we can show in minutes is 99 minutes and 59 seconds - if (seconds > (99 * 60) + 59) { - sym = sym_h; - value = seconds / 60; - } - buff[0] = sym; - tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); + uint32_t value = seconds; + char sym = sym_m; + // Maximum value we can show in minutes is 99 minutes and 59 seconds + if (seconds > (99 * 60) + 59) { + sym = sym_h; + value = seconds / 60; + } + buff[0] = sym; + tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); } static inline void osdFormatOnTime(char *buff) { - osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H); + osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H); } static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) { - uint32_t seconds = getFlightTime(); - osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H); - if (attr && osdConfig()->time_alarm > 0) { - if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) { - TEXT_ATTRIBUTES_ADD_BLINK(*attr); - } - } + uint32_t seconds = getFlightTime(); + osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H); + if (attr && osdConfig()->time_alarm > 0) { + if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) { + TEXT_ATTRIBUTES_ADD_BLINK(*attr); + } + } } /** @@ -642,23 +642,23 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) */ char *osdFormatTrimWhiteSpace(char *buff) { - char *end; + char *end; - // Trim leading spaces - while(isspace((unsigned char)*buff)) buff++; + // Trim leading spaces + while(isspace((unsigned char)*buff)) buff++; - // All spaces? - if(*buff == 0) - return buff; + // All spaces? + if(*buff == 0) + return buff; - // Trim trailing spaces - end = buff + strlen(buff) - 1; - while(end > buff && isspace((unsigned char)*end)) end--; + // Trim trailing spaces + end = buff + strlen(buff) - 1; + while(end > buff && isspace((unsigned char)*end)) end--; - // Write new null terminator character - end[1] = '\0'; + // Write new null terminator character + end[1] = '\0'; - return buff; + return buff; } /** @@ -666,32 +666,32 @@ char *osdFormatTrimWhiteSpace(char *buff) */ static uint16_t osdConvertRSSI(void) { - // change range to [0, 99] - return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); + // change range to [0, 99] + return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } static uint16_t osdGetCrsfLQ(void) { - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - int16_t displayedLQ = 0; - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - displayedLQ = statsLQ; - break; - case OSD_CRSF_LQ_TYPE2: - displayedLQ = statsLQ; - break; - case OSD_CRSF_LQ_TYPE3: - displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ; - break; - } - return displayedLQ; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + int16_t displayedLQ = 0; + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + displayedLQ = statsLQ; + break; + case OSD_CRSF_LQ_TYPE2: + displayedLQ = statsLQ; + break; + case OSD_CRSF_LQ_TYPE3: + displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ; + break; + } + return displayedLQ; } static int16_t osdGetCrsfdBm(void) { - return rxLinkStatistics.uplinkRSSI; + return rxLinkStatistics.uplinkRSSI; } /** * Displays a temperature postfixed with a symbol depending on the current unit system @@ -701,293 +701,293 @@ static int16_t osdGetCrsfdBm(void) */ static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t symbol, const char *label, bool valid, int16_t temperature, int16_t alarm_min, int16_t alarm_max) { - char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2]; - textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT; - uint8_t valueXOffset = 0; + char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2]; + textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT; + uint8_t valueXOffset = 0; - if (symbol) { - buff[0] = symbol; - buff[1] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - valueXOffset = 1; - } + if (symbol) { + buff[0] = symbol; + buff[1] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = 1; + } #ifdef USE_TEMPERATURE_SENSOR - else if (label[0] != '\0') { - uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN); - memcpy(buff, label, label_len); - memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len); - buff[5] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1; - } + else if (label[0] != '\0') { + uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN); + memcpy(buff, label, label_len); + memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len); + buff[5] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1; + } #else - UNUSED(label); + UNUSED(label); #endif - if (valid) { + if (valid) { - 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); + 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); - } else - strcpy(buff, "---"); + } else + strcpy(buff, "---"); - buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C; - buff[4] = '\0'; + buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C; + buff[4] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr); } #ifdef USE_TEMPERATURE_SENSOR static void osdDisplayTemperatureSensor(uint8_t elemPosX, uint8_t elemPosY, uint8_t sensorIndex) { - int16_t temperature; - const bool valid = getSensorTemperature(sensorIndex, &temperature); - const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex); - uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0; - osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max); + int16_t temperature; + const bool valid = getSensorTemperature(sensorIndex, &temperature); + const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex); + uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0; + osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max); } #endif static void osdFormatCoordinate(char *buff, char sym, int32_t val) { - // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals - const int coordinateLength = osdConfig()->coordinate_digits + 1; + // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals + const int coordinateLength = osdConfig()->coordinate_digits + 1; - buff[0] = sym; - int32_t integerPart = val / GPS_DEGREES_DIVIDER; - // Latitude maximum integer width is 3 (-90) while - // longitude maximum integer width is 4 (-180). - int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); - // We can show up to 7 digits in decimalPart. - int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); - STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); - int decimalDigits; - bool bfcompat = false; // Assume BFCOMPAT mode is no enabled + buff[0] = sym; + int32_t integerPart = val / GPS_DEGREES_DIVIDER; + // Latitude maximum integer width is 3 (-90) while + // longitude maximum integer width is 4 (-180). + int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); + // We can show up to 7 digits in decimalPart. + int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); + STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); + int decimalDigits; + bool bfcompat = false; // Assume BFCOMPAT mode is no enabled #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; - } + if(isBfCompatibleVideoSystem(osdConfig())) { + bfcompat = true; + } #endif - if (!bfcompat) { - decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); - // Embbed the decimal separator - buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; - buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; - } else { - // BFCOMPAT mode enabled - decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart); - } - // Fill up to coordinateLength with zeros - int total = 1 + integerDigits + decimalDigits; - while(total < coordinateLength) { - buff[total] = '0'; - total++; - } - buff[coordinateLength] = '\0'; + if (!bfcompat) { + decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); + // Embbed the decimal separator + buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; + buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; + } else { + // BFCOMPAT mode enabled + decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart); + } + // Fill up to coordinateLength with zeros + int total = 1 + integerDigits + decimalDigits; + while(total < coordinateLength) { + buff[total] = '0'; + total++; + } + buff[coordinateLength] = '\0'; } static void osdFormatCraftName(char *buff) { - if (strlen(systemConfig()->craftName) == 0) - strcpy(buff, "CRAFT_NAME"); - else { - for (int i = 0; i < MAX_NAME_LENGTH; i++) { - buff[i] = sl_toupper((unsigned char)systemConfig()->craftName[i]); - if (systemConfig()->craftName[i] == 0) - break; - } - } + if (strlen(systemConfig()->craftName) == 0) + strcpy(buff, "CRAFT_NAME"); + else { + for (int i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = sl_toupper((unsigned char)systemConfig()->craftName[i]); + if (systemConfig()->craftName[i] == 0) + break; + } + } } void osdFormatPilotName(char *buff) { - if (strlen(systemConfig()->pilotName) == 0) - strcpy(buff, "PILOT_NAME"); - else { - for (int i = 0; i < MAX_NAME_LENGTH; i++) { - buff[i] = sl_toupper((unsigned char)systemConfig()->pilotName[i]); - if (systemConfig()->pilotName[i] == 0) - break; - } - } + if (strlen(systemConfig()->pilotName) == 0) + strcpy(buff, "PILOT_NAME"); + else { + for (int i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = sl_toupper((unsigned char)systemConfig()->pilotName[i]); + if (systemConfig()->pilotName[i] == 0) + break; + } + } } static const char * osdArmingDisabledReasonMessage(void) { - const char *message = NULL; - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - switch (isArmingDisabledReason()) { - case ARMING_DISABLED_FAILSAFE_SYSTEM: - // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c - if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { - if (failsafeIsReceivingRxData()) { - // If we're not using sticks, it means the ARM switch - // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING - // yet - return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); - } - // Not receiving RX data - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); - } - return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); - case ARMING_DISABLED_NOT_LEVEL: - return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL); - case ARMING_DISABLED_SENSORS_CALIBRATING: - return OSD_MESSAGE_STR(OSD_MSG_SENSORS_CAL); - case ARMING_DISABLED_SYSTEM_OVERLOADED: - return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED); - case ARMING_DISABLED_NAVIGATION_UNSAFE: - // Check the exact reason - switch (navigationIsBlockingArming(NULL)) { - char buf[6]; - case NAV_ARMING_BLOCKER_NONE: - break; - case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: - return OSD_MESSAGE_STR(OSD_MSG_WAITING_GPS_FIX); - case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: - return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); - case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); - tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); - return message = messageBuf; - case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: - return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); - } - break; - case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: - return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL); - case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: - return OSD_MESSAGE_STR(OSD_MSG_ACC_NOT_CAL); - case ARMING_DISABLED_ARM_SWITCH: - return OSD_MESSAGE_STR(OSD_MSG_DISARM_1ST); - case ARMING_DISABLED_HARDWARE_FAILURE: - { - if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_GYRO_FAILURE); - } - if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_ACC_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_MAG_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_BARO_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_GPS_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_RANGEFINDER_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_PITOT_FAIL); - } - } - return OSD_MESSAGE_STR(OSD_MSG_HW_FAIL); - case ARMING_DISABLED_BOXFAILSAFE: - return OSD_MESSAGE_STR(OSD_MSG_FS_EN); - case ARMING_DISABLED_BOXKILLSWITCH: - return OSD_MESSAGE_STR(OSD_MSG_KILL_SW_EN); - case ARMING_DISABLED_RC_LINK: - return OSD_MESSAGE_STR(OSD_MSG_NO_RC_LINK); - case ARMING_DISABLED_THROTTLE: - return OSD_MESSAGE_STR(OSD_MSG_THROTTLE_NOT_LOW); - case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED: - return OSD_MESSAGE_STR(OSD_MSG_ROLLPITCH_OFFCENTER); - case ARMING_DISABLED_SERVO_AUTOTRIM: - return OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM_ACTIVE); - case ARMING_DISABLED_OOM: - return OSD_MESSAGE_STR(OSD_MSG_NOT_ENOUGH_MEMORY); - case ARMING_DISABLED_INVALID_SETTING: - return OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - case ARMING_DISABLED_CLI: - return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); - case ARMING_DISABLED_PWM_OUTPUT_ERROR: - return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); - case ARMING_DISABLED_NO_PREARM: - return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); - case ARMING_DISABLED_DSHOT_BEEPER: - return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); - // Cases without message - case ARMING_DISABLED_LANDING_DETECTED: - FALLTHROUGH; - case ARMING_DISABLED_CMS_MENU: - FALLTHROUGH; - case ARMING_DISABLED_OSD_MENU: - FALLTHROUGH; - case ARMING_DISABLED_ALL_FLAGS: - FALLTHROUGH; - case ARMED: - FALLTHROUGH; - case SIMULATOR_MODE_HITL: - FALLTHROUGH; - case SIMULATOR_MODE_SITL: - FALLTHROUGH; - case WAS_EVER_ARMED: - break; - } - return NULL; + switch (isArmingDisabledReason()) { + case ARMING_DISABLED_FAILSAFE_SYSTEM: + // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c + if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { + if (failsafeIsReceivingRxData()) { + // If we're not using sticks, it means the ARM switch + // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING + // yet + return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + } + // Not receiving RX data + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); + } + return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); + case ARMING_DISABLED_NOT_LEVEL: + return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL); + case ARMING_DISABLED_SENSORS_CALIBRATING: + return OSD_MESSAGE_STR(OSD_MSG_SENSORS_CAL); + case ARMING_DISABLED_SYSTEM_OVERLOADED: + return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED); + case ARMING_DISABLED_NAVIGATION_UNSAFE: + // Check the exact reason + switch (navigationIsBlockingArming(NULL)) { + char buf[6]; + case NAV_ARMING_BLOCKER_NONE: + break; + case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: + return OSD_MESSAGE_STR(OSD_MSG_WAITING_GPS_FIX); + case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: + return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); + case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); + return message = messageBuf; + case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: + return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); + } + break; + case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: + return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL); + case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: + return OSD_MESSAGE_STR(OSD_MSG_ACC_NOT_CAL); + case ARMING_DISABLED_ARM_SWITCH: + return OSD_MESSAGE_STR(OSD_MSG_DISARM_1ST); + case ARMING_DISABLED_HARDWARE_FAILURE: + { + if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_GYRO_FAILURE); + } + if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_ACC_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_MAG_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_BARO_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_GPS_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_RANGEFINDER_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_PITOT_FAIL); + } + } + return OSD_MESSAGE_STR(OSD_MSG_HW_FAIL); + case ARMING_DISABLED_BOXFAILSAFE: + return OSD_MESSAGE_STR(OSD_MSG_FS_EN); + case ARMING_DISABLED_BOXKILLSWITCH: + return OSD_MESSAGE_STR(OSD_MSG_KILL_SW_EN); + case ARMING_DISABLED_RC_LINK: + return OSD_MESSAGE_STR(OSD_MSG_NO_RC_LINK); + case ARMING_DISABLED_THROTTLE: + return OSD_MESSAGE_STR(OSD_MSG_THROTTLE_NOT_LOW); + case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED: + return OSD_MESSAGE_STR(OSD_MSG_ROLLPITCH_OFFCENTER); + case ARMING_DISABLED_SERVO_AUTOTRIM: + return OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM_ACTIVE); + case ARMING_DISABLED_OOM: + return OSD_MESSAGE_STR(OSD_MSG_NOT_ENOUGH_MEMORY); + case ARMING_DISABLED_INVALID_SETTING: + return OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); + case ARMING_DISABLED_CLI: + return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); + case ARMING_DISABLED_PWM_OUTPUT_ERROR: + return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); + case ARMING_DISABLED_NO_PREARM: + return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + // Cases without message + case ARMING_DISABLED_LANDING_DETECTED: + FALLTHROUGH; + case ARMING_DISABLED_CMS_MENU: + FALLTHROUGH; + case ARMING_DISABLED_OSD_MENU: + FALLTHROUGH; + case ARMING_DISABLED_ALL_FLAGS: + FALLTHROUGH; + case ARMED: + FALLTHROUGH; + case SIMULATOR_MODE_HITL: + FALLTHROUGH; + case SIMULATOR_MODE_SITL: + FALLTHROUGH; + case WAS_EVER_ARMED: + break; + } + return NULL; } static const char * osdFailsafePhaseMessage(void) { - // See failsafe.h for each phase explanation - switch (failsafePhase()) { - case FAILSAFE_RETURN_TO_HOME: - // XXX: Keep this in sync with OSD_FLYMODE. - return OSD_MESSAGE_STR(OSD_MSG_RTH_FS); - case FAILSAFE_LANDING: - // This should be considered an emergengy landing - return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); - case FAILSAFE_RX_LOSS_MONITORING: - // Only reachable from FAILSAFE_LANDED, which performs - // a disarm. Since aircraft has been disarmed, we no - // longer show failsafe details. - FALLTHROUGH; - case FAILSAFE_LANDED: - // Very brief, disarms and transitions into - // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents - // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM, - // so we'll show the user how to re-arm in when - // that flag is the reason to prevent arming. - FALLTHROUGH; - case FAILSAFE_RX_LOSS_IDLE: - // This only happens when user has chosen NONE as FS - // procedure. The recovery messages should be enough. - FALLTHROUGH; - case FAILSAFE_IDLE: - // Failsafe not active - FALLTHROUGH; - case FAILSAFE_RX_LOSS_DETECTED: - // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED - // or the FS procedure immediately. - FALLTHROUGH; - case FAILSAFE_RX_LOSS_RECOVERED: - // Exiting failsafe - break; - } - return NULL; + // See failsafe.h for each phase explanation + switch (failsafePhase()) { + case FAILSAFE_RETURN_TO_HOME: + // XXX: Keep this in sync with OSD_FLYMODE. + return OSD_MESSAGE_STR(OSD_MSG_RTH_FS); + case FAILSAFE_LANDING: + // This should be considered an emergengy landing + return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); + case FAILSAFE_RX_LOSS_MONITORING: + // Only reachable from FAILSAFE_LANDED, which performs + // a disarm. Since aircraft has been disarmed, we no + // longer show failsafe details. + FALLTHROUGH; + case FAILSAFE_LANDED: + // Very brief, disarms and transitions into + // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents + // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM, + // so we'll show the user how to re-arm in when + // that flag is the reason to prevent arming. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_IDLE: + // This only happens when user has chosen NONE as FS + // procedure. The recovery messages should be enough. + FALLTHROUGH; + case FAILSAFE_IDLE: + // Failsafe not active + FALLTHROUGH; + case FAILSAFE_RX_LOSS_DETECTED: + // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED + // or the FS procedure immediately. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_RECOVERED: + // Exiting failsafe + break; + } + return NULL; } static const char * osdFailsafeInfoMessage(void) { - if (failsafeIsReceivingRxData()) { - // User must move sticks to exit FS mode - return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); - } - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); + if (failsafeIsReceivingRxData()) { + // User must move sticks to exit FS mode + return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); + } + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); } return NULL; } @@ -995,72 +995,72 @@ static const char * divertingToSafehomeMessage(void) static const char * navigationStateMessage(void) { - switch (NAV_Status.state) { - case MW_NAV_STATE_NONE: - break; - case MW_NAV_STATE_RTH_START: - return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); - case MW_NAV_STATE_RTH_CLIMB: - return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); - case MW_NAV_STATE_RTH_ENROUTE: - if (posControl.flags.rthTrackbackActive) { - return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK); - } else { - return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); - } - case MW_NAV_STATE_HOLD_INFINIT: - // Used by HOLD flight modes. No information to add. - break; - case MW_NAV_STATE_HOLD_TIMED: - // "HOLDING WP FOR xx S" Countdown added in osdGetSystemMessage - break; - case MW_NAV_STATE_WP_ENROUTE: - // "TO WP" + WP countdown added in osdGetSystemMessage - break; - case MW_NAV_STATE_PROCESS_NEXT: - return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP); - case MW_NAV_STATE_DO_JUMP: - // Not used - break; - case MW_NAV_STATE_LAND_START: - // Not used - break; - case MW_NAV_STATE_EMERGENCY_LANDING: - return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING); - case MW_NAV_STATE_LAND_IN_PROGRESS: - return OSD_MESSAGE_STR(OSD_MSG_LANDING); - case MW_NAV_STATE_HOVER_ABOVE_HOME: - if (STATE(FIXED_WING_LEGACY)) { + switch (NAV_Status.state) { + case MW_NAV_STATE_NONE: + break; + case MW_NAV_STATE_RTH_START: + return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); + case MW_NAV_STATE_RTH_CLIMB: + return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); + case MW_NAV_STATE_RTH_ENROUTE: + if (posControl.flags.rthTrackbackActive) { + return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK); + } else { + return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); + } + case MW_NAV_STATE_HOLD_INFINIT: + // Used by HOLD flight modes. No information to add. + break; + case MW_NAV_STATE_HOLD_TIMED: + // "HOLDING WP FOR xx S" Countdown added in osdGetSystemMessage + break; + case MW_NAV_STATE_WP_ENROUTE: + // "TO WP" + WP countdown added in osdGetSystemMessage + break; + case MW_NAV_STATE_PROCESS_NEXT: + return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP); + case MW_NAV_STATE_DO_JUMP: + // Not used + break; + case MW_NAV_STATE_LAND_START: + // Not used + break; + case MW_NAV_STATE_EMERGENCY_LANDING: + return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING); + case MW_NAV_STATE_LAND_IN_PROGRESS: + return OSD_MESSAGE_STR(OSD_MSG_LANDING); + case MW_NAV_STATE_HOVER_ABOVE_HOME: + if (STATE(FIXED_WING_LEGACY)) { #if defined(USE_SAFE_HOME) - if (safehome_applied) { - return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); - } + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); + } #endif - return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); - } - return OSD_MESSAGE_STR(OSD_MSG_HOVERING); - case MW_NAV_STATE_LANDED: - return OSD_MESSAGE_STR(OSD_MSG_LANDED); - case MW_NAV_STATE_LAND_SETTLE: - return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); - case MW_NAV_STATE_LAND_START_DESCENT: - // Not used - break; - } - return NULL; + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); + } + return OSD_MESSAGE_STR(OSD_MSG_HOVERING); + case MW_NAV_STATE_LANDED: + return OSD_MESSAGE_STR(OSD_MSG_LANDED); + case MW_NAV_STATE_LAND_SETTLE: + return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); + case MW_NAV_STATE_LAND_START_DESCENT: + // Not used + break; + } + return NULL; } static void osdFormatMessage(char *buff, size_t size, const char *message, bool isCenteredText) { - // String is always filled with Blanks - memset(buff, SYM_BLANK, size); - if (message) { - size_t messageLength = strlen(message); - int rem = isCenteredText ? MAX(0, (int)size - (int)messageLength) : 0; - strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); - } - // Ensure buff is zero terminated - buff[size] = '\0'; + // String is always filled with Blanks + memset(buff, SYM_BLANK, size); + if (message) { + size_t messageLength = strlen(message); + int rem = isCenteredText ? MAX(0, (int)size - (int)messageLength) : 0; + strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); + } + // Ensure buff is zero terminated + buff[size] = '\0'; } /** @@ -1069,25 +1069,25 @@ static void osdFormatMessage(char *buff, size_t size, const char *message, bool **/ static void osdFormatBatteryChargeSymbol(char *buff) { - uint8_t p = calculateBatteryPercentage(); - p = (100 - p) / 16.6; - buff[0] = SYM_BATT_FULL + p; + uint8_t p = calculateBatteryPercentage(); + p = (100 - p) / 16.6; + buff[0] = SYM_BATT_FULL + p; } static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr) { - const batteryState_e batteryState = getBatteryState(); + const batteryState_e batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { - TEXT_ATTRIBUTES_ADD_BLINK(*attr); - } + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + TEXT_ATTRIBUTES_ADD_BLINK(*attr); + } } void osdCrosshairPosition(uint8_t *x, uint8_t *y) { - *x = osdDisplayPort->cols / 2; - *y = osdDisplayPort->rows / 2; - *y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down + *x = osdDisplayPort->cols / 2; + *y = osdDisplayPort->rows / 2; + *y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down } /** @@ -1096,13 +1096,13 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) */ 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]); + 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; + if (!usingScaledThrottle && !usingRCThrottle) + usingScaledThrottle = true; - return usingScaledThrottle; + return usingScaledThrottle; } /** @@ -1111,26 +1111,26 @@ bool osdUsingScaledThrottle(void) **/ 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 (navigationIsControllingThrottle()) { - buff[0] = SYM_AUTO_THR0; - buff[1] = SYM_AUTO_THR1; - if (isFixedWingAutoThrottleManuallyIncreased()) { - TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); - } - useScaled = true; - } + if (useScaled) { + buff[0] = SYM_SCALE; + } else { + buff[0] = SYM_BLANK; + } + buff[1] = SYM_THR; + 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); - } + if (powerLimiterIsLimiting()) { + TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); + } #endif - tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); + tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); } /** @@ -1138,110 +1138,110 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes **/ static void osdFormatGVar(char *buff, uint8_t index) { - buff[0] = 'G'; - buff[1] = '0'+index; - buff[2] = ':'; - #ifdef USE_PROGRAMMING_FRAMEWORK - osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); - #endif + buff[0] = 'G'; + buff[1] = '0'+index; + buff[2] = ':'; + #ifdef USE_PROGRAMMING_FRAMEWORK + osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); + #endif } #if defined(USE_ESC_SENSOR) static void osdFormatRpm(char *buff, uint32_t rpm) { - buff[0] = SYM_RPM; - if (rpm) { - if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { - uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); - osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); - buff[osdConfig()->esc_rpm_precision] = 'K'; - buff[osdConfig()->esc_rpm_precision+1] = '\0'; - } - else { - switch(osdConfig()->esc_rpm_precision) { - case 6: - tfp_sprintf(buff + 1, "%6lu", rpm); - break; - case 5: - tfp_sprintf(buff + 1, "%5lu", rpm); - break; - case 4: - tfp_sprintf(buff + 1, "%4lu", rpm); - break; - case 3: - default: - tfp_sprintf(buff + 1, "%3lu", rpm); - break; - } + buff[0] = SYM_RPM; + if (rpm) { + if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { + uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); + osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); + buff[osdConfig()->esc_rpm_precision] = 'K'; + buff[osdConfig()->esc_rpm_precision+1] = '\0'; + } + else { + switch(osdConfig()->esc_rpm_precision) { + case 6: + tfp_sprintf(buff + 1, "%6lu", rpm); + break; + case 5: + tfp_sprintf(buff + 1, "%5lu", rpm); + break; + case 4: + tfp_sprintf(buff + 1, "%4lu", rpm); + break; + case 3: + default: + tfp_sprintf(buff + 1, "%3lu", rpm); + break; + } - } - } - else { - uint8_t buffPos = 1; - while (buffPos <=( osdConfig()->esc_rpm_precision)) { - strcpy(buff + buffPos++, "-"); - } - } + } + } + else { + uint8_t buffPos = 1; + while (buffPos <=( osdConfig()->esc_rpm_precision)) { + strcpy(buff + buffPos++, "-"); + } + } } #endif int32_t osdGetAltitude(void) { - return getEstimatedActualPosition(Z); + return getEstimatedActualPosition(Z); } static inline int32_t osdGetAltitudeMsl(void) { - return getEstimatedActualPosition(Z)+GPS_home.alt; + return getEstimatedActualPosition(Z)+GPS_home.alt; } uint16_t osdGetRemainingGlideTime(void) { - float value = getEstimatedActualVelocity(Z); - static pt1Filter_t glideTimeFilterState; - const timeMs_t curTimeMs = millis(); - static timeMs_t glideTimeUpdatedMs; + float value = getEstimatedActualVelocity(Z); + static pt1Filter_t glideTimeFilterState; + const timeMs_t curTimeMs = millis(); + static timeMs_t glideTimeUpdatedMs; - value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); - glideTimeUpdatedMs = curTimeMs; + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); + glideTimeUpdatedMs = curTimeMs; - if (value < 0) { - value = osdGetAltitude() / abs((int)value); - } else { - value = 0; - } + if (value < 0) { + value = osdGetAltitude() / abs((int)value); + } else { + value = 0; + } - return (uint16_t)roundf(value); + return (uint16_t)roundf(value); } static bool osdIsHeadingValid(void) { - return isImuHeadingValid(); + return isImuHeadingValid(); } int16_t osdGetHeading(void) { - return attitude.values.yaw; + return attitude.values.yaw; } int16_t osdGetPanServoOffset(void) { - int8_t servoIndex = osdConfig()->pan_servo_index; - int16_t servoPosition = servo[servoIndex]; - int16_t servoMiddle = servoParams(servoIndex)->middle; - return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); + int8_t servoIndex = osdConfig()->pan_servo_index; + int16_t servoPosition = servo[servoIndex]; + int16_t servoMiddle = servoParams(servoIndex)->middle; + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); } // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle) { - while (angle < 0) { - angle += 360; - } - while (angle >= 360) { - angle -= 360; - } - return angle; + while (angle < 0) { + angle += 360; + } + while (angle >= 360) { + angle -= 360; + } + return angle; } #if defined(USE_GPS) @@ -1255,141 +1255,141 @@ int osdGetHeadingAngle(int angle) * erasing all screen on each redraw. */ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t centerSym, - uint32_t poiDistance, int16_t poiDirection, uint16_t poiSymbol, - uint16_t *drawn, uint32_t *usedScale) + uint32_t poiDistance, int16_t poiDirection, uint16_t poiSymbol, + uint16_t *drawn, uint32_t *usedScale) { - // TODO: These need to be tested with several setups. We might - // need to make them configurable. - const int hMargin = 5; - const int vMargin = 3; + // TODO: These need to be tested with several setups. We might + // need to make them configurable. + const int hMargin = 5; + const int vMargin = 3; - // TODO: Get this from the display driver? - const int charWidth = 12; - const int charHeight = 18; + // TODO: Get this from the display driver? + const int charWidth = 12; + const int charHeight = 18; - uint8_t minX = hMargin; - uint8_t maxX = osdDisplayPort->cols - 1 - hMargin; - uint8_t minY = vMargin; - uint8_t maxY = osdDisplayPort->rows - 1 - vMargin; - uint8_t midX = osdDisplayPort->cols / 2; - uint8_t midY = osdDisplayPort->rows / 2; + uint8_t minX = hMargin; + uint8_t maxX = osdDisplayPort->cols - 1 - hMargin; + uint8_t minY = vMargin; + uint8_t maxY = osdDisplayPort->rows - 1 - vMargin; + uint8_t midX = osdDisplayPort->cols / 2; + uint8_t midY = osdDisplayPort->rows / 2; - // Fixed marks - displayWriteChar(osdDisplayPort, midX, midY, centerSym); + // Fixed marks + displayWriteChar(osdDisplayPort, midX, midY, centerSym); - // First, erase the previous drawing. - if (OSD_VISIBLE(*drawn)) { - displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK); - *drawn = 0; - } + // First, erase the previous drawing. + if (OSD_VISIBLE(*drawn)) { + displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK); + *drawn = 0; + } - uint32_t initialScale; - const unsigned scaleMultiplier = 2; - // We try to reduce the scale when the POI will be around half the distance - // between the center and the closers map edge, to avoid too much jumping - const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; + uint32_t initialScale; + const unsigned scaleMultiplier = 2; + // We try to reduce the scale when the POI will be around half the distance + // between the center and the closers map edge, to avoid too much jumping + const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - initialScale = 16; // 16m ~= 0.01miles - break; - case OSD_UNIT_GA: - initialScale = 18; // 18m ~= 0.01 nautical miles - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - initialScale = 10; // 10m as initial scale - break; - } + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + initialScale = 16; // 16m ~= 0.01miles + break; + case OSD_UNIT_GA: + initialScale = 18; // 18m ~= 0.01 nautical miles + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + initialScale = 10; // 10m as initial scale + break; + } - // Try to keep the same scale when getting closer until we draw over the center point - uint32_t scale = initialScale; - if (*usedScale) { - scale = *usedScale; - if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) { - scale /= scaleMultiplier; - } - } + // Try to keep the same scale when getting closer until we draw over the center point + uint32_t scale = initialScale; + if (*usedScale) { + scale = *usedScale; + if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) { + scale /= scaleMultiplier; + } + } - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX)) { - int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); - float poiAngle = DEGREES_TO_RADIANS(directionToPoi); - float poiSin = sin_approx(poiAngle); - float poiCos = cos_approx(poiAngle); + int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); + float poiAngle = DEGREES_TO_RADIANS(directionToPoi); + float poiSin = sin_approx(poiAngle); + float poiCos = cos_approx(poiAngle); - // Now start looking for a valid scale that lets us draw everything - int ii; - for (ii = 0; ii < 50; ii++) { - // Calculate location of the aircraft in map - int points = poiDistance / ((float)scale / charHeight); + // Now start looking for a valid scale that lets us draw everything + int ii; + for (ii = 0; ii < 50; ii++) { + // Calculate location of the aircraft in map + int points = poiDistance / ((float)scale / charHeight); - float pointsX = points * poiSin; - int poiX = midX - roundf(pointsX / charWidth); - if (poiX < minX || poiX > maxX) { - scale *= scaleMultiplier; - continue; - } + float pointsX = points * poiSin; + int poiX = midX - roundf(pointsX / charWidth); + if (poiX < minX || poiX > maxX) { + scale *= scaleMultiplier; + continue; + } - float pointsY = points * poiCos; - int poiY = midY + roundf(pointsY / charHeight); - if (poiY < minY || poiY > maxY) { - scale *= scaleMultiplier; - continue; - } + float pointsY = points * poiCos; + int poiY = midY + roundf(pointsY / charHeight); + if (poiY < minY || poiY > maxY) { + scale *= scaleMultiplier; + continue; + } - if (poiX == midX && poiY == midY) { - // We're over the map center symbol, so we would be drawing - // over it even if we increased the scale. Alternate between - // drawing the center symbol or drawing the POI. - if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - break; - } - } else { + if (poiX == midX && poiY == midY) { + // We're over the map center symbol, so we would be drawing + // over it even if we increased the scale. Alternate between + // drawing the center symbol or drawing the POI. + if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + break; + } + } else { - uint16_t c; - if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { - // Something else written here, increase scale. If the display doesn't support reading - // back characters, we assume there's nothing. - // - // If we're close to the center, decrease scale. Otherwise increase it. - uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2); - uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2); - if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX && - poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY && - scale > scaleMultiplier) { + uint16_t c; + if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { + // Something else written here, increase scale. If the display doesn't support reading + // back characters, we assume there's nothing. + // + // If we're close to the center, decrease scale. Otherwise increase it. + uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2); + uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2); + if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX && + poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY && + scale > scaleMultiplier) { - scale /= scaleMultiplier; - } else { - scale *= scaleMultiplier; - } - continue; - } - } + scale /= scaleMultiplier; + } else { + scale *= scaleMultiplier; + } + continue; + } + } - // Draw the point on the map - if (poiSymbol == SYM_ARROW_UP) { - // Drawing aircraft, rotate - int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading); - poiSymbol += mapHeading * 2 / 45; - } - displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol); + // Draw the point on the map + if (poiSymbol == SYM_ARROW_UP) { + // Drawing aircraft, rotate + int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading); + poiSymbol += mapHeading * 2 / 45; + } + displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol); - // Update saved location - *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG; - break; - } - } + // Update saved location + *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG; + break; + } + } - *usedScale = scale; + *usedScale = scale; - // Update global map data for scale and reference - osdMapData.scale = scale; - osdMapData.referenceSymbol = referenceSym; + // Update global map data for scale and reference + osdMapData.scale = scale; + osdMapData.referenceSymbol = referenceSym; } /* Draws a map with the home in the center and the craft moving around. @@ -1397,7 +1397,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen */ static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale) { - osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale); + osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale); } /* Draws a map with the aircraft in the center and the home moving around. @@ -1405,3015 +1405,3113 @@ static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t */ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) { - int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading()); - int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180); - osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); + int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading()); + int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180); + osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) { - uint8_t tmp; - tmp = data ^ (uint8_t)(crcAccum & 0xff); - tmp ^= (tmp << 4); - crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); - return crcAccum; + uint8_t tmp; + tmp = data ^ (uint8_t)(crcAccum & 0xff); + tmp ^= (tmp << 4); + crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + return crcAccum; } static void osdDisplayTelemetry(void) { - uint32_t trk_data; - uint16_t trk_crc = 0; - char trk_buffer[31]; - static int16_t trk_elevation = 127; - static uint16_t trk_bearing = 0; + uint32_t trk_data; + uint16_t trk_crc = 0; + char trk_buffer[31]; + static int16_t trk_elevation = 127; + static uint16_t trk_bearing = 0; - if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX)){ - if (GPS_distanceToHome > 5) { - trk_bearing = GPS_directionToHome; - trk_bearing += 360 + 180; - trk_bearing %= 360; - int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); - float at = atan2(alt, GPS_distanceToHome); - trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad - trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 - if (trk_elevation < 0) { - trk_elevation = 0; - } - } - } - } - else{ - trk_elevation = 127; - trk_bearing = 0; - } + if (ARMING_FLAG(ARMED)) { + if (STATE(GPS_FIX)){ + if (GPS_distanceToHome > 5) { + trk_bearing = GPS_directionToHome; + trk_bearing += 360 + 180; + trk_bearing %= 360; + int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); + float at = atan2(alt, GPS_distanceToHome); + trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad + trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 + if (trk_elevation < 0) { + trk_elevation = 0; + } + } + } + } + else{ + trk_elevation = 127; + trk_bearing = 0; + } - trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet - trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. - trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 - trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 - trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 - trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 - trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF + trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet + trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. + trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 + trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 + trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 + trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 + trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF - for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. - if (trk_data & (uint32_t)1 << t_ctr){ - trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; - } - else{ - trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; - } - } - trk_buffer[30] = 0; - displayWrite(osdDisplayPort, 0, 0, trk_buffer); - if (osdConfig()->telemetry>1){ - displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible - } + for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. + if (trk_data & (uint32_t)1 << t_ctr){ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; + } + else{ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; + } + } + trk_buffer[30] = 0; + displayWrite(osdDisplayPort, 0, 0, trk_buffer); + if (osdConfig()->telemetry>1){ + displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible + } } #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { - strcpy(buff, label); - for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; - uint8_t decimals = showDecimal ? 1 : 0; - osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); - buff[9] = ' '; - osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); - buff[14] = ' '; - osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); - buff[19] = ' '; - osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); - buff[24] = '\0'; + strcpy(buff, label); + for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; + uint8_t decimals = showDecimal ? 1 : 0; + osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); + buff[9] = ' '; + osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); + buff[14] = ' '; + osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); + buff[19] = ' '; + osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); + buff[24] = '\0'; } static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals) { - char buff[7]; - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + char buff[7]; + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatBatteryChargeSymbol(buff); - buff[1] = '\0'; - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + osdFormatBatteryChargeSymbol(buff); + buff[1] = '\0'; + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - digits = MIN(digits, 5); - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); - buff[digits] = SYM_VOLT; - buff[digits+1] = '\0'; - const batteryState_e batteryVoltageState = checkBatteryVoltageState(); - if (batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + digits = MIN(digits, 5); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + buff[digits] = SYM_VOLT; + buff[digits+1] = '\0'; + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + if (batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); } static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF) { - textAttributes_t elemAttr; - char buff[4]; + textAttributes_t elemAttr; + char buff[4]; - const pid8_t *pid = &pidBank()->pid[pidIndex]; - pidType_e pidType = pidIndexGetType(pidIndex); + const pid8_t *pid = &pidBank()->pid[pidIndex]; + pidType_e pidType = pidIndexGetType(pidIndex); - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - if (pidType == PID_TYPE_NONE) { - // PID is not used in this configuration. Draw dashes. - // XXX: Keep this in sync with the %3d format and spacing used below - displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -"); - return; - } + if (pidType == PID_TYPE_NONE) { + // PID is not used in this configuration. Draw dashes. + // XXX: Keep this in sync with the %3d format and spacing used below + displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -"); + return; + } - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); - 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->P); + 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); - 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->I); + 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); - 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->D); + 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); - 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); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->FF); + 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); } static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) { - textAttributes_t elemAttr; - char buff[4]; + textAttributes_t elemAttr; + char buff[4]; - const pid8_t *pid = &pidBank()->pid[pidIndex]; - pidType_e pidType = pidIndexGetType(pidIndex); + const pid8_t *pid = &pidBank()->pid[pidIndex]; + pidType_e pidType = pidIndexGetType(pidIndex); - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - if (pidType == PID_TYPE_NONE) { - // PID is not used in this configuration. Draw dashes. - // XXX: Keep this in sync with the %3d format and spacing used below - displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - -"); - return; - } + if (pidType == PID_TYPE_NONE) { + // PID is not used in this configuration. Draw dashes. + // XXX: Keep this in sync with the %3d format and spacing used below + displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - -"); + return; + } - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); - 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->P); + 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); - 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->I); + 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); - 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", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + 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); } static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) { - char buff[8]; - textAttributes_t elemAttr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + char buff[8]; + textAttributes_t elemAttr; + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); - if (isAdjustmentFunctionSelected(adjFunc)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + if (isAdjustmentFunctionSelected(adjFunc)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); } int8_t getGeoWaypointNumber(int8_t waypointIndex) { - static int8_t lastWaypointIndex = 1; - static int8_t geoWaypointIndex; + static int8_t lastWaypointIndex = 1; + static int8_t geoWaypointIndex; - if (waypointIndex != lastWaypointIndex) { - lastWaypointIndex = geoWaypointIndex = waypointIndex; - for (uint8_t i = posControl.startWpIndex; i <= waypointIndex; i++) { - if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || - posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || - posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { - geoWaypointIndex -= 1; - } - } - } + if (waypointIndex != lastWaypointIndex) { + lastWaypointIndex = geoWaypointIndex = waypointIndex; + for (uint8_t i = posControl.startWpIndex; i <= waypointIndex; i++) { + if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || + posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || + posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { + geoWaypointIndex -= 1; + } + } + } - return geoWaypointIndex - posControl.startWpIndex + 1; + return geoWaypointIndex - posControl.startWpIndex + 1; } void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { - int8_t ptr = 0; + int8_t ptr = 0; - if (osdConfig()->osd_switch_indicators_align_left) { - for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { - buff[ptr] = swName[ptr]; - } + if (osdConfig()->osd_switch_indicators_align_left) { + for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { + buff[ptr] = swName[ptr]; + } - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } - } else { - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + } else { + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } - for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { - buff[ptr] = swName[ptr-1]; - } + for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { + buff[ptr] = swName[ptr-1]; + } - ptr++; - } + ptr++; + } - buff[ptr] = '\0'; + buff[ptr] = '\0'; } static bool osdDrawSingleElement(uint8_t item) { - uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; - if (!OSD_VISIBLE(pos)) { - return false; - } - uint8_t elemPosX = OSD_X(pos); - uint8_t elemPosY = OSD_Y(pos); - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - char buff[32] = {0}; + uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; + if (!OSD_VISIBLE(pos)) { + return false; + } + uint8_t elemPosX = OSD_X(pos); + uint8_t elemPosY = OSD_Y(pos); + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + char buff[32] = {0}; - switch (item) { - case OSD_RSSI_VALUE: - { - uint16_t osdRssi = osdConvertRSSI(); - buff[0] = SYM_RSSI; - tfp_sprintf(buff + 1, "%2d", osdRssi); - if (osdRssi < osdConfig()->rssi_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + switch (item) { + case OSD_RSSI_VALUE: + { + uint16_t osdRssi = osdConvertRSSI(); + buff[0] = SYM_RSSI; + tfp_sprintf(buff + 1, "%2d", osdRssi); + if (osdRssi < osdConfig()->rssi_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; + case OSD_MAIN_BATT_VOLTAGE: { + uint8_t base_digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + return true; + } - case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; + case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { + uint8_t base_digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + return true; + } - case OSD_CURRENT_DRAW: { - osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); - buff[3] = SYM_AMP; - buff[4] = '\0'; + case OSD_CURRENT_DRAW: { + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; - uint8_t current_alarm = osdConfig()->current_alarm; - if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_MAH_DRAWN: { - uint8_t mah_digits = osdConfig()->mAh_used_precision; // Initialize to config value - bool bfcompat = false; // Assume BFCOMPAT is off + case OSD_MAH_DRAWN: { + uint8_t mah_digits = osdConfig()->mAh_used_precision; // Initialize to config value + bool bfcompat = false; // Assume BFCOMPAT is off #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if (isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + bfcompat = true; + } #endif - if (bfcompat) { - //BFcompat 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 10Ah+ packs - buff[5] = SYM_MAH; - buff[6] = '\0'; - } else { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { - // Shown in Ah - buff[mah_digits] = SYM_AH; - } else { - // Shown in mAh - buff[mah_digits] = SYM_MAH; - } - buff[mah_digits + 1] = '\0'; - } + if (bfcompat) { + //BFcompat 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 10Ah+ packs + buff[5] = SYM_MAH; + buff[6] = '\0'; + } else { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { + // Shown in Ah + buff[mah_digits] = SYM_AH; + } else { + // Shown in mAh + buff[mah_digits] = SYM_MAH; + } + buff[mah_digits + 1] = '\0'; + } - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - break; - } + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + break; + } - case OSD_WH_DRAWN: - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - buff[3] = SYM_WH; - buff[4] = '\0'; - break; + case OSD_WH_DRAWN: + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + buff[3] = SYM_WH; + buff[4] = '\0'; + break; - case OSD_BATTERY_REMAINING_CAPACITY: + case OSD_BATTERY_REMAINING_CAPACITY: - if (currentBatteryProfile->capacity.value == 0) - tfp_sprintf(buff, " NA"); - else if (!batteryWasFullWhenPluggedIn()) - tfp_sprintf(buff, " NF"); - else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); - else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH - osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + if (currentBatteryProfile->capacity.value == 0) + tfp_sprintf(buff, " NA"); + else if (!batteryWasFullWhenPluggedIn()) + tfp_sprintf(buff, " NF"); + else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) + tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); + else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH + osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); - buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; - buff[5] = '\0'; + buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[5] = '\0'; - if (batteryUsesCapacityThresholds()) { - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - } + if (batteryUsesCapacityThresholds()) { + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + } - break; + break; - case OSD_BATTERY_REMAINING_PERCENT: - osdFormatBatteryChargeSymbol(buff); - tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - break; + case OSD_BATTERY_REMAINING_PERCENT: + osdFormatBatteryChargeSymbol(buff); + tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + break; - case OSD_POWER_SUPPLY_IMPEDANCE: - if (isPowerSupplyImpedanceValid()) - tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); - else - strcpy(buff, "---"); - buff[3] = SYM_MILLIOHM; - buff[4] = '\0'; - break; + case OSD_POWER_SUPPLY_IMPEDANCE: + if (isPowerSupplyImpedanceValid()) + tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); + else + strcpy(buff, "---"); + buff[3] = SYM_MILLIOHM; + buff[4] = '\0'; + break; #ifdef USE_GPS - case OSD_GPS_SATS: - buff[0] = SYM_SAT_L; - buff[1] = SYM_SAT_R; - tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); - if (!STATE(GPS_FIX)) { - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); - } - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; + case OSD_GPS_SATS: + buff[0] = SYM_SAT_L; + buff[1] = SYM_SAT_R; + tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); + if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; - case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); - break; + case OSD_GPS_SPEED: + osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); + break; - case OSD_GPS_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_speed, false, true); - break; + case OSD_GPS_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_speed, false, true); + break; - case OSD_3D_SPEED: - osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); - break; + case OSD_3D_SPEED: + osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); + break; - case OSD_3D_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); - break; + case OSD_3D_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); + break; - case OSD_GLIDESLOPE: - { - float horizontalSpeed = gpsSol.groundSpeed; - float sinkRate = -getEstimatedActualVelocity(Z); - static pt1Filter_t gsFilterState; - const timeMs_t currentTimeMs = millis(); - static timeMs_t gsUpdatedTimeMs; - float glideSlope = horizontalSpeed / sinkRate; - glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); - gsUpdatedTimeMs = currentTimeMs; + case OSD_GLIDESLOPE: + { + float horizontalSpeed = gpsSol.groundSpeed; + float sinkRate = -getEstimatedActualVelocity(Z); + static pt1Filter_t gsFilterState; + const timeMs_t currentTimeMs = millis(); + static timeMs_t gsUpdatedTimeMs; + float glideSlope = horizontalSpeed / sinkRate; + glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); + gsUpdatedTimeMs = currentTimeMs; - buff[0] = SYM_GLIDESLOPE; - if (glideSlope > 0.0f && glideSlope < 100.0f) { - osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = '\0'; - break; - } + buff[0] = SYM_GLIDESLOPE; + if (glideSlope > 0.0f && glideSlope < 100.0f) { + osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = '\0'; + break; + } - case OSD_GPS_LAT: - osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); - break; + case OSD_GPS_LAT: + osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); + break; - case OSD_GPS_LON: - osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon); - break; + case OSD_GPS_LON: + osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon); + break; - case OSD_HOME_DIR: - { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { - if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); - } - else - { - int16_t panHomeDirOffset = 0; - if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ - panHomeDirOffset = osdGetPanServoOffset(); - } - int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); - int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset; - osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); - } - } else { - // No home or no fix or unknown heading, blink. - // If we're unarmed, show the arrow pointing up so users can see the arrow - // while configuring the OSD. If we're armed, show a '-' indicating that - // we don't know the direction to home. - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr); - } - return true; - } + case OSD_HOME_DIR: + { + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); + } + else + { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); + } + } else { + // No home or no fix or unknown heading, blink. + // If we're unarmed, show the arrow pointing up so users can see the arrow + // while configuring the OSD. If we're armed, show a '-' indicating that + // we don't know the direction to home. + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr); + } + return true; + } - case OSD_HOME_HEADING_ERROR: - { - buff[0] = SYM_HOME; - buff[1] = SYM_HEADING; + case OSD_HOME_HEADING_ERROR: + { + buff[0] = SYM_HOME; + buff[1] = SYM_HEADING; - 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); - } else { - strcpy(buff + 2, "----"); - } + 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); + } else { + strcpy(buff + 2, "----"); + } - buff[6] = SYM_DEGREES; - buff[7] = '\0'; - break; - } + buff[6] = SYM_DEGREES; + buff[7] = '\0'; + break; + } - case OSD_HOME_DIST: - { - buff[0] = SYM_HOME; - uint32_t distance_to_home_cm = GPS_distanceToHome * 100; - osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); + case OSD_HOME_DIST: + { + buff[0] = SYM_HOME; + uint32_t distance_to_home_cm = GPS_distanceToHome * 100; + osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); - uint16_t dist_alarm = osdConfig()->dist_alarm; - if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + uint16_t dist_alarm = osdConfig()->dist_alarm; + if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; - case OSD_TRIP_DIST: - buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); - break; + case OSD_TRIP_DIST: + buff[0] = SYM_TOTAL; + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); + break; - case OSD_GROUND_COURSE: - { - buff[0] = SYM_GROUND_COURSE; - if (osdIsHeadingValid()) { - tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + case OSD_GROUND_COURSE: + { + buff[0] = SYM_GROUND_COURSE; + if (osdIsHeadingValid()) { + tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_COURSE_HOLD_ERROR: - { - if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } + case OSD_COURSE_HOLD_ERROR: + { + if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } - buff[0] = SYM_HEADING; + buff[0] = SYM_HEADING; - if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) { - buff[1] = buff[2] = buff[3] = '-'; - } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); - if (ABS(herr) > 99) - strcpy(buff + 1, ">99"); - else - tfp_sprintf(buff + 1, "%3d", herr); - } + if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) { + buff[1] = buff[2] = buff[3] = '-'; + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); + if (ABS(herr) > 99) + strcpy(buff + 1, ">99"); + else + tfp_sprintf(buff + 1, "%3d", herr); + } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_COURSE_HOLD_ADJUSTMENT: - { - int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); + case OSD_COURSE_HOLD_ADJUSTMENT: + { + int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); - if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } + if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } - buff[0] = SYM_HEADING; + buff[0] = SYM_HEADING; - 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); - } + 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); + } - buff[5] = SYM_DEGREES; - buff[6] = '\0'; - break; - } + buff[5] = SYM_DEGREES; + buff[6] = '\0'; + break; + } - case OSD_CROSS_TRACK_ERROR: - { - if (isWaypointNavTrackingActive()) { - buff[0] = SYM_CROSS_TRACK_ERROR; - osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); - } else { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } - break; - } + case OSD_CROSS_TRACK_ERROR: + { + if (isWaypointNavTrackingActive()) { + buff[0] = SYM_CROSS_TRACK_ERROR; + osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); + } else { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + break; + } - case OSD_GPS_HDOP: - { - buff[0] = SYM_HDP_L; - buff[1] = SYM_HDP_R; - int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE; - uint8_t digits = 2U; + case OSD_GPS_HDOP: + { + buff[0] = SYM_HDP_L; + buff[1] = SYM_HDP_R; + int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE; + uint8_t digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - digits = 3U; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + digits = 3U; + } #endif - osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); - break; - } + osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); + break; + } - case OSD_MAP_NORTH: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawHomeMap(0, 'N', &drawn, &scale); - return true; - } - case OSD_MAP_TAKEOFF: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale); - return true; - } - case OSD_RADAR: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawRadar(&drawn, &scale); - return true; - } + case OSD_MAP_NORTH: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawHomeMap(0, 'N', &drawn, &scale); + return true; + } + case OSD_MAP_TAKEOFF: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale); + return true; + } + case OSD_RADAR: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawRadar(&drawn, &scale); + return true; + } #endif // GPS - case OSD_ALTITUDE: - { - int32_t alt = osdGetAltitude(); - osdFormatAltitudeSymbol(buff, alt); + case OSD_ALTITUDE: + { + int32_t alt = osdGetAltitude(); + osdFormatAltitudeSymbol(buff, alt); - uint16_t alt_alarm = osdConfig()->alt_alarm; - uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm; - if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) || - (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) { + uint16_t alt_alarm = osdConfig()->alt_alarm; + uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm; + if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) || + (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_ALTITUDE_MSL: - { - int32_t alt = osdGetAltitudeMsl(); - osdFormatAltitudeSymbol(buff, alt); - break; - } + case OSD_ALTITUDE_MSL: + { + int32_t alt = osdGetAltitudeMsl(); + osdFormatAltitudeSymbol(buff, alt); + break; + } #ifdef USE_RANGEFINDER - case OSD_RANGEFINDER: - { - int32_t range = rangefinderGetLatestRawAltitude(); - if (range < 0) { - buff[0] = '-'; - buff[1] = '-'; - buff[2] = '-'; - } else { - osdFormatDistanceSymbol(buff, range, 1); - } - } - break; + case OSD_RANGEFINDER: + { + int32_t range = rangefinderGetLatestRawAltitude(); + if (range < 0) { + buff[0] = '-'; + buff[1] = '-'; + buff[2] = '-'; + } else { + osdFormatDistanceSymbol(buff, range, 1); + } + } + break; #endif - case OSD_ONTIME: - { - osdFormatOnTime(buff); - break; - } + case OSD_ONTIME: + { + osdFormatOnTime(buff); + break; + } - case OSD_FLYTIME: - { - osdFormatFlyTime(buff, &elemAttr); - break; - } + case OSD_FLYTIME: + { + osdFormatFlyTime(buff, &elemAttr); + break; + } - case OSD_ONTIME_FLYTIME: - { - if (ARMING_FLAG(ARMED)) { - osdFormatFlyTime(buff, &elemAttr); - } else { - osdFormatOnTime(buff); - } - break; - } + case OSD_ONTIME_FLYTIME: + { + if (ARMING_FLAG(ARMED)) { + osdFormatFlyTime(buff, &elemAttr); + } else { + osdFormatOnTime(buff); + } + break; + } - case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: - { - /*static int32_t updatedTimeSeconds = 0;*/ - static int32_t timeSeconds = -1; + case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: + { + /*static int32_t updatedTimeSeconds = 0;*/ + static int32_t timeSeconds = -1; #if defined(USE_ADC) && defined(USE_GPS) - static timeUs_t updatedTimestamp = 0; - timeUs_t currentTimeUs = micros(); - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR - timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); + timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); #else - timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); + timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); #endif - updatedTimestamp = currentTimeUs; - } + updatedTimestamp = currentTimeUs; + } #endif - if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { - buff[0] = SYM_FLIGHT_MINS_REMAINING; - strcpy(buff + 1, "--:--"); + if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { + buff[0] = SYM_FLIGHT_MINS_REMAINING; + strcpy(buff + 1, "--:--"); #if defined(USE_ADC) && defined(USE_GPS) - updatedTimestamp = 0; + updatedTimestamp = 0; #endif - } else if (timeSeconds == -2) { - // Wind is too strong to come back with cruise throttle - buff[0] = SYM_FLIGHT_MINS_REMAINING; - buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL; - buff[3] = ':'; - buff[6] = '\0'; - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else { - osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING); - if (timeSeconds == 0) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + } else if (timeSeconds == -2) { + // Wind is too strong to come back with cruise throttle + buff[0] = SYM_FLIGHT_MINS_REMAINING; + buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL; + buff[3] = ':'; + buff[6] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING); + if (timeSeconds == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; - case OSD_REMAINING_DISTANCE_BEFORE_RTH:; - static int32_t distanceMeters = -1; + case OSD_REMAINING_DISTANCE_BEFORE_RTH:; + static int32_t distanceMeters = -1; #if defined(USE_ADC) && defined(USE_GPS) - static timeUs_t updatedTimestamp = 0; - timeUs_t currentTimeUs = micros(); - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR - distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); + distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); #else - distanceMeters = calculateRemainingDistanceBeforeRTH(false); + distanceMeters = calculateRemainingDistanceBeforeRTH(false); #endif - updatedTimestamp = currentTimeUs; - } + updatedTimestamp = currentTimeUs; + } #endif - //buff[0] = SYM_TRIP_DIST; - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); - if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[4] = SYM_BLANK; - buff[5] = '\0'; - strcpy(buff + 1, "---"); - } else if (distanceMeters == -2) { - // Wind is too strong to come back with cruise throttle - buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; - switch ((osd_unit_e)osdConfig()->units){ - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - buff[4] = SYM_DIST_MI; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - buff[4] = SYM_DIST_KM; - break; - case OSD_UNIT_GA: - buff[4] = SYM_DIST_NM; - break; - } - buff[5] = '\0'; - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else { - osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); - if (distanceMeters == 0) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; + //buff[0] = SYM_TRIP_DIST; + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); + if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { + buff[4] = SYM_BLANK; + buff[5] = '\0'; + strcpy(buff + 1, "---"); + } else if (distanceMeters == -2) { + // Wind is too strong to come back with cruise throttle + buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; + switch ((osd_unit_e)osdConfig()->units){ + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + buff[4] = SYM_DIST_MI; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + buff[4] = SYM_DIST_KM; + break; + case OSD_UNIT_GA: + buff[4] = SYM_DIST_NM; + break; + } + buff[5] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); + if (distanceMeters == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; - case OSD_FLYMODE: - { - char *p = "ACRO"; + case OSD_FLYMODE: + { + char *p = "ACRO"; - if (FLIGHT_MODE(FAILSAFE_MODE)) - p = "!FS!"; - else if (FLIGHT_MODE(MANUAL_MODE)) - p = "MANU"; - else if (FLIGHT_MODE(TURTLE_MODE)) - p = "TURT"; - else if (FLIGHT_MODE(NAV_RTH_MODE)) - p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; - else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) - p = "LOTR"; - else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) - p = "HOLD"; - else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) - p = "CRUZ"; - else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) - p = "CRSH"; - else if (FLIGHT_MODE(NAV_WP_MODE)) - 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, ACRO, etc... - // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. - p = " AH "; - } - else if (FLIGHT_MODE(ANGLE_MODE)) - p = "ANGL"; - else if (FLIGHT_MODE(HORIZON_MODE)) - p = "HOR "; + if (FLIGHT_MODE(FAILSAFE_MODE)) + p = "!FS!"; + else if (FLIGHT_MODE(MANUAL_MODE)) + p = "MANU"; + else if (FLIGHT_MODE(TURTLE_MODE)) + p = "TURT"; + else if (FLIGHT_MODE(NAV_RTH_MODE)) + p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) + p = "LOTR"; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + p = "HOLD"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) + p = "CRUZ"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + p = "CRSH"; + else if (FLIGHT_MODE(NAV_WP_MODE)) + 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, ACRO, etc... + // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. + p = " AH "; + } + else if (FLIGHT_MODE(ANGLE_MODE)) + p = "ANGL"; + else if (FLIGHT_MODE(HORIZON_MODE)) + p = "HOR "; - displayWrite(osdDisplayPort, elemPosX, elemPosY, p); - return true; - } + displayWrite(osdDisplayPort, elemPosX, elemPosY, p); + return true; + } - case OSD_CRAFT_NAME: - osdFormatCraftName(buff); - break; + case OSD_CRAFT_NAME: + osdFormatCraftName(buff); + break; - case OSD_PILOT_NAME: - osdFormatPilotName(buff); - break; + case OSD_PILOT_NAME: + osdFormatPilotName(buff); + break; - case OSD_THROTTLE_POS: - { - osdFormatThrottlePosition(buff, false, &elemAttr); - break; - } + case OSD_THROTTLE_POS: + { + osdFormatThrottlePosition(buff, false, &elemAttr); + break; + } - case OSD_VTX_CHANNEL: - { - vtxDeviceOsdInfo_t osdInfo; - vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); + case OSD_VTX_CHANNEL: + { + vtxDeviceOsdInfo_t osdInfo; + vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); - if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); - return true; - } - break; + tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); + return true; + } + break; - case OSD_VTX_POWER: - { - vtxDeviceOsdInfo_t osdInfo; - vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); + case OSD_VTX_POWER: + { + vtxDeviceOsdInfo_t osdInfo; + vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "%c", SYM_VTX_POWER); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + tfp_sprintf(buff, "%c", SYM_VTX_POWER); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); - if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - return true; - } + tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + return true; + } #if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_RSSI_DBM: - { - 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%c", rssi, SYM_DBM, ' '); - } - if (!failsafeIsReceivingRxData()){ - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } - case OSD_CRSF_LQ: - { - buff[0] = SYM_LQ; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); - } - break; - case OSD_CRSF_LQ_TYPE2: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%s:%3d", " ", 0); - } else { - tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); - } - break; - case OSD_CRSF_LQ_TYPE3: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); - } - break; - } - if (!failsafeIsReceivingRxData()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + case OSD_CRSF_RSSI_DBM: + { + 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%c", rssi, SYM_DBM, ' '); + } + if (!failsafeIsReceivingRxData()){ + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_CRSF_LQ: + { + buff[0] = SYM_LQ; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + } + break; + case OSD_CRSF_LQ_TYPE2: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%s:%3d", " ", 0); + } else { + tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); + } + break; + case OSD_CRSF_LQ_TYPE3: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + } + break; + } + if (!failsafeIsReceivingRxData()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_CRSF_SNR_DB: - { - static pt1Filter_t snrFilterState; - static timeMs_t snrUpdated = 0; - int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); - snrUpdated = millis(); + case OSD_CRSF_SNR_DB: + { + static pt1Filter_t snrFilterState; + static timeMs_t snrUpdated = 0; + int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); + snrUpdated = millis(); - const char* showsnr = "-20"; - const char* hidesnr = " "; - if (snrFiltered > osdConfig()->snr_alarm) { - if (cmsInMenu) { - buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); - } else { - buff[0] = SYM_BLANK; - tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); - } - } else if (snrFiltered <= osdConfig()->snr_alarm) { - buff[0] = SYM_SNR; - if (snrFiltered <= -10) { - tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); - } else { - tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); - } - } - break; - } + const char* showsnr = "-20"; + const char* hidesnr = " "; + if (snrFiltered > osdConfig()->snr_alarm) { + if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); + } else { + buff[0] = SYM_BLANK; + tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + } + } else if (snrFiltered <= osdConfig()->snr_alarm) { + buff[0] = SYM_SNR; + if (snrFiltered <= -10) { + tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); + } else { + tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); + } + } + break; + } - case OSD_CRSF_TX_POWER: - { - if (!failsafeIsReceivingRxData()) - tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); - else - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); - break; - } + case OSD_CRSF_TX_POWER: + { + if (!failsafeIsReceivingRxData()) + tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); + else + tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + break; + } #endif - case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair + case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair - osdCrosshairPosition(&elemPosX, &elemPosY); - osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); + osdCrosshairPosition(&elemPosX, &elemPosY); + osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { - osdHudDrawHoming(elemPosX, elemPosY); - } + if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + osdHudDrawHoming(elemPosX, elemPosY); + } - if (STATE(GPS_FIX) && isImuHeadingValid()) { + if (STATE(GPS_FIX) && isImuHeadingValid()) { - if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { - osdHudClear(); - } + if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { + osdHudClear(); + } - // -------- POI : Home point + // -------- POI : Home point - if (osdConfig()->hud_homepoint) { // Display the home point (H) - osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0); - } + if (osdConfig()->hud_homepoint) { // Display the home point (H) + osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0); + } - // -------- POI : Nearby aircrafts from ESP32 radar + // -------- POI : Nearby aircrafts from ESP32 radar - if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar - for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) { - if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped - fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE); - radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters + if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar + for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) { + if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE); + radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters - if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) { - radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In ° - radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100; - osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq); - } - } - } - } + if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) { + radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In ° + radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100; + osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq); + } + } + } + } - // -------- POI : Next waypoints from navigation + // -------- POI : Next waypoints from navigation - if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints - gpsLocation_t wp2; - int j; + if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints + gpsLocation_t wp2; + int j; - for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top - j = posControl.activeWaypointIndex + i; - if (j > posControl.startWpIndex + posControl.waypointCount - 1) { // limit to max WP index for mission - break; - } - if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { - wp2.lat = posControl.waypointList[j].lat; - wp2.lon = posControl.waypointList[j].lon; - wp2.alt = posControl.waypointList[j].alt; - fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); - int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); - j = getGeoWaypointNumber(j); - while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0) - osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i); - } - } - } - } + for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top + j = posControl.activeWaypointIndex + i; + if (j > posControl.startWpIndex + posControl.waypointCount - 1) { // limit to max WP index for mission + break; + } + if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { + wp2.lat = posControl.waypointList[j].lat; + wp2.lon = posControl.waypointList[j].lon; + wp2.alt = posControl.waypointList[j].alt; + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); + int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); + j = getGeoWaypointNumber(j); + while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0) + osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i); + } + } + } + } - return true; - break; + return true; + break; - case OSD_ATTITUDE_ROLL: - buff[0] = SYM_ROLL_LEVEL; - if (ABS(attitude.values.roll) >= 1) - buff[0] += (attitude.values.roll < 0 ? -1 : 1); - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); - break; + case OSD_ATTITUDE_ROLL: + buff[0] = SYM_ROLL_LEVEL; + if (ABS(attitude.values.roll) >= 1) + buff[0] += (attitude.values.roll < 0 ? -1 : 1); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); + break; - case OSD_ATTITUDE_PITCH: - if (ABS(attitude.values.pitch) < 1) - buff[0] = 'P'; - else if (attitude.values.pitch > 0) - buff[0] = SYM_PITCH_DOWN; - else if (attitude.values.pitch < 0) - buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); - break; + case OSD_ATTITUDE_PITCH: + if (ABS(attitude.values.pitch) < 1) + buff[0] = 'P'; + else if (attitude.values.pitch > 0) + buff[0] = SYM_PITCH_DOWN; + else if (attitude.values.pitch < 0) + buff[0] = SYM_PITCH_UP; + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); + break; - case OSD_ARTIFICIAL_HORIZON: - { - float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + case OSD_ARTIFICIAL_HORIZON: + { + float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); + float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); - pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0; - pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim()); - if (osdConfig()->ahi_reverse_roll) { - rollAngle = -rollAngle; - } - osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(), - OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle); - osdDrawSingleElement(OSD_HORIZON_SIDEBARS); - osdDrawSingleElement(OSD_CROSSHAIRS); + pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0; + pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim()); + if (osdConfig()->ahi_reverse_roll) { + rollAngle = -rollAngle; + } + osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(), + OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle); + osdDrawSingleElement(OSD_HORIZON_SIDEBARS); + osdDrawSingleElement(OSD_CROSSHAIRS); - return true; - } + return true; + } - case OSD_HORIZON_SIDEBARS: - { - osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas()); - return true; - } + case OSD_HORIZON_SIDEBARS: + { + osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas()); + return true; + } #if defined(USE_BARO) || defined(USE_GPS) - case OSD_VARIO: - { - float zvel = getEstimatedActualVelocity(Z); - osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel); - return true; - } + case OSD_VARIO: + { + float zvel = getEstimatedActualVelocity(Z); + osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel); + return true; + } - case OSD_VARIO_NUM: - { - int16_t value = getEstimatedActualVelocity(Z); - char sym; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - // Convert to centifeet/s - value = CENTIMETERS_TO_CENTIFEET(value); - sym = SYM_FTS; - break; - case OSD_UNIT_GA: - // Convert to centi-100feet/min - value = CENTIMETERS_TO_FEET(value * 60); - sym = SYM_100FTM; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // Already in cm/s - sym = SYM_MS; - break; - } + case OSD_VARIO_NUM: + { + int16_t value = getEstimatedActualVelocity(Z); + char sym; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // Convert to centifeet/s + value = CENTIMETERS_TO_CENTIFEET(value); + sym = SYM_FTS; + break; + case OSD_UNIT_GA: + // Convert to centi-100feet/min + value = CENTIMETERS_TO_FEET(value * 60); + sym = SYM_100FTM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // Already in cm/s + sym = SYM_MS; + break; + } - osdFormatCentiNumber(buff, value, 0, 1, 0, 3); - buff[3] = sym; - buff[4] = '\0'; - break; - } - case OSD_CLIMB_EFFICIENCY: - { - // amperage is in centi amps (10mA), vertical speed is in cms/s. We want - // Ah/dist only to show when vertical speed > 1m/s. - static pt1Filter_t veFilterState; - static timeUs_t vEfficiencyUpdated = 0; - int32_t value = 0; - timeUs_t currentTimeUs = micros(); - timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); - if (getEstimatedActualVelocity(Z) > 0) { - if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD - value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); + osdFormatCentiNumber(buff, value, 0, 1, 0, 3); + buff[3] = sym; + buff[4] = '\0'; + break; + } + case OSD_CLIMB_EFFICIENCY: + { + // amperage is in centi amps (10mA), vertical speed is in cms/s. We want + // Ah/dist only to show when vertical speed > 1m/s. + static pt1Filter_t veFilterState; + static timeUs_t vEfficiencyUpdated = 0; + int32_t value = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); + if (getEstimatedActualVelocity(Z) > 0) { + if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD + value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); - vEfficiencyUpdated = currentTimeUs; - } else { - value = veFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - // mAh/foot - if (efficiencyValid) { - osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", 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; - buff[4] = SYM_AH_V_FT_1; - buff[5] = '\0'; - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // mAh/metre - if (efficiencyValid) { - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", 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; - buff[4] = SYM_AH_V_M_1; - buff[5] = '\0'; - } - break; - } - break; - } - case OSD_GLIDE_TIME_REMAINING: - { - uint16_t glideTime = osdGetRemainingGlideTime(); - buff[0] = SYM_GLIDE_MINS; - if (glideTime > 0) { - // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide - // time will be longer than 99 minutes. If it is, it will show 99:^^ - if (glideTime > (99 * 60) + 59) { - tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); - buff[4] = SYM_DIRECTION; - buff[5] = SYM_DIRECTION; - } else { - tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); - } - } else { - tfp_sprintf(buff + 1, "%s", "--:--"); - } - buff[6] = '\0'; - break; - } - case OSD_GLIDE_RANGE: - { - uint16_t glideSeconds = osdGetRemainingGlideTime(); - buff[0] = SYM_GLIDE_DIST; - if (glideSeconds > 0) { - uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); - } else { - tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); - buff[5] = '\0'; - } - break; - } + vEfficiencyUpdated = currentTimeUs; + } else { + value = veFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // mAh/foot + if (efficiencyValid) { + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", 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; + buff[4] = SYM_AH_V_FT_1; + buff[5] = '\0'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // mAh/metre + if (efficiencyValid) { + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", 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; + buff[4] = SYM_AH_V_M_1; + buff[5] = '\0'; + } + break; + } + break; + } + case OSD_GLIDE_TIME_REMAINING: + { + uint16_t glideTime = osdGetRemainingGlideTime(); + buff[0] = SYM_GLIDE_MINS; + if (glideTime > 0) { + // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide + // time will be longer than 99 minutes. If it is, it will show 99:^^ + if (glideTime > (99 * 60) + 59) { + tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); + buff[4] = SYM_DIRECTION; + buff[5] = SYM_DIRECTION; + } else { + tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); + } + } else { + tfp_sprintf(buff + 1, "%s", "--:--"); + } + buff[6] = '\0'; + break; + } + case OSD_GLIDE_RANGE: + { + uint16_t glideSeconds = osdGetRemainingGlideTime(); + buff[0] = SYM_GLIDE_DIST; + if (glideSeconds > 0) { + uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + } else { + tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); + buff[5] = '\0'; + } + break; + } #endif - case OSD_SWITCH_INDICATOR_0: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_0: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_1: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_1: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_2: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_2: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_3: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_3: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); + break; - case OSD_PAN_SERVO_CENTRED: - { - int16_t panOffset = osdGetPanServoOffset(); - const timeMs_t panServoTimeNow = millis(); - static timeMs_t panServoTimeOffCentre = 0; + case OSD_PAN_SERVO_CENTRED: + { + int16_t panOffset = osdGetPanServoOffset(); + const timeMs_t panServoTimeNow = millis(); + static timeMs_t panServoTimeOffCentre = 0; - if (panOffset < 0) { - if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset >= -osdConfig()->pan_servo_offcentre_warning) { - if (panServoTimeOffCentre == 0) { - panServoTimeOffCentre = panServoTimeNow; - } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } else { - panServoTimeOffCentre = 0; - } + if (panOffset < 0) { + if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset >= -osdConfig()->pan_servo_offcentre_warning) { + if (panServoTimeOffCentre == 0) { + panServoTimeOffCentre = panServoTimeNow; + } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } else { + panServoTimeOffCentre = 0; + } - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); - } else if (panOffset > 0) { - if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset <= osdConfig()->pan_servo_offcentre_warning) { - if (panServoTimeOffCentre == 0) { - panServoTimeOffCentre = panServoTimeNow; - } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } else { - panServoTimeOffCentre = 0; - } + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); + } else if (panOffset > 0) { + if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset <= osdConfig()->pan_servo_offcentre_warning) { + if (panServoTimeOffCentre == 0) { + panServoTimeOffCentre = panServoTimeNow; + } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } else { + panServoTimeOffCentre = 0; + } - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); - } else { - panServoTimeOffCentre = 0; + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); + } else { + panServoTimeOffCentre = 0; - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); - } + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); + } - return true; - } - break; + return true; + } + break; - case OSD_ACTIVE_PROFILE: - tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - break; + case OSD_ACTIVE_PROFILE: + tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; - case OSD_ROLL_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); - return true; + case OSD_ROLL_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); + return true; - case OSD_PITCH_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF); - return true; + case OSD_PITCH_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF); + return true; - case OSD_YAW_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF); - return true; + case OSD_YAW_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF); + return true; - case OSD_LEVEL_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); - return true; + case OSD_LEVEL_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); + return true; - case OSD_POS_XY_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); - return true; + case OSD_POS_XY_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); + return true; - case OSD_POS_Z_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); - return true; + case OSD_POS_Z_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); + return true; - case OSD_VEL_XY_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); - return true; + case OSD_VEL_XY_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); + return true; - case OSD_VEL_Z_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); - return true; + case OSD_VEL_Z_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); + return true; - case OSD_HEADING_P: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); - return true; + case OSD_HEADING_P: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); + return true; - case OSD_BOARD_ALIGN_ROLL: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); - return true; + case OSD_BOARD_ALIGN_ROLL: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); + return true; - case OSD_BOARD_ALIGN_PITCH: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); - return true; + case OSD_BOARD_ALIGN_PITCH: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); + return true; - case OSD_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); - return true; + case OSD_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); + return true; - case OSD_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); - return true; + case OSD_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); + return true; - case OSD_THROTTLE_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); - return true; + case OSD_THROTTLE_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); + return true; - case OSD_PITCH_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); + case OSD_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_ROLL_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); + case OSD_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); - return true; + case OSD_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; - case OSD_MANUAL_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); - return true; + case OSD_MANUAL_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); + return true; - case OSD_MANUAL_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); - return true; + case OSD_MANUAL_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); + return true; - case OSD_MANUAL_PITCH_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); + case OSD_MANUAL_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_MANUAL_ROLL_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); + case OSD_MANUAL_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_MANUAL_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); - return true; + case OSD_MANUAL_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; - case OSD_NAV_FW_CRUISE_THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); - return true; + case OSD_NAV_FW_CRUISE_THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + return true; - case OSD_NAV_FW_PITCH2THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); - return true; + case OSD_NAV_FW_PITCH2THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); + return true; - case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); - return true; + case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); + return true; - case OSD_FW_ALT_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees - break; - } + case OSD_FW_ALT_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees + break; + } - case OSD_FW_POS_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees - osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true); - break; - } + case OSD_FW_POS_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees + osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true); + break; + } - case OSD_MC_VEL_Z_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs - break; - } + case OSD_MC_VEL_Z_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs + break; + } - case OSD_MC_VEL_X_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2 - break; - } + case OSD_MC_VEL_X_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2 + break; + } - case OSD_MC_VEL_Y_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2 - break; - } + case OSD_MC_VEL_Y_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2 + break; + } - case OSD_MC_POS_XYZ_P_OUTPUTS: - { - 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)); - buff[9] = ' '; - tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); - buff[14] = ' '; - tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); - buff[19] = '\0'; - break; - } + case OSD_MC_POS_XYZ_P_OUTPUTS: + { + 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)); + buff[9] = ' '; + tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); + buff[14] = ' '; + tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); + buff[19] = '\0'; + break; + } - case OSD_POWER: - { - bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; + case OSD_POWER: + { + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; - uint8_t current_alarm = osdConfig()->current_alarm; - if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_AIR_SPEED: - { - #ifdef USE_PITOT - buff[0] = SYM_AIR; + case OSD_AIR_SPEED: + { + #ifdef USE_PITOT + buff[0] = SYM_AIR; - if (pitotIsHealthy()) - { - const float airspeed_estimate = getAirspeedEstimate(); - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); - if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || - (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - else - { - strcpy(buff + 1, " X!"); - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - #else - return false; - #endif - break; - } + if (pitotIsHealthy()) + { + const float airspeed_estimate = getAirspeedEstimate(); + osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || + (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + else + { + strcpy(buff + 1, " X!"); + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + #else + return false; + #endif + break; + } - case OSD_AIR_MAX_SPEED: - { - #ifdef USE_PITOT - buff[0] = SYM_MAX; - buff[1] = SYM_AIR; - osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); - #else - return false; - #endif - break; - } + case OSD_AIR_MAX_SPEED: + { + #ifdef USE_PITOT + buff[0] = SYM_MAX; + buff[1] = SYM_AIR; + osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); + #else + return false; + #endif + break; + } - case OSD_RTC_TIME: - { - // RTC not configured will show 00:00 - dateTime_t dateTime; - rtcGetDateTimeLocal(&dateTime); - buff[0] = SYM_CLOCK; - tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds); - break; - } + case OSD_RTC_TIME: + { + // RTC not configured will show 00:00 + dateTime_t dateTime; + rtcGetDateTimeLocal(&dateTime); + buff[0] = SYM_CLOCK; + tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds); + break; + } - case OSD_MESSAGES: - { - elemAttr = osdGetSystemMessage(buff, OSD_MESSAGE_LENGTH, true); - break; - } + case OSD_MESSAGES: + { + elemAttr = osdGetSystemMessage(buff, OSD_MESSAGE_LENGTH, true); + break; + } - case OSD_VERSION: - { - tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - break; - } + case OSD_VERSION: + { + tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; + } - case OSD_MAIN_BATT_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; + case OSD_MAIN_BATT_CELL_VOLTAGE: + { + uint8_t base_digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), base_digits, 2); + return true; + } - case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; + case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: + { + uint8_t base_digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), base_digits, 2); + return true; + } - case OSD_SCALED_THROTTLE_POS: - { - osdFormatThrottlePosition(buff, true, &elemAttr); - break; - } + case OSD_SCALED_THROTTLE_POS: + { + osdFormatThrottlePosition(buff, true, &elemAttr); + break; + } - case OSD_HEADING: - { - buff[0] = SYM_HEADING; - if (osdIsHeadingValid()) { - int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); - if (h < 0) { - h += 360; - } - tfp_sprintf(&buff[1], "%3d", h); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + case OSD_HEADING: + { + buff[0] = SYM_HEADING; + if (osdIsHeadingValid()) { + int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); + if (h < 0) { + h += 360; + } + tfp_sprintf(&buff[1], "%3d", h); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_HEADING_GRAPH: - { - if (osdIsHeadingValid()) { - osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading()); - return true; - } else { - buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE; - buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE; - buff[OSD_HEADING_GRAPH_WIDTH] = '\0'; - } - break; - } + case OSD_HEADING_GRAPH: + { + if (osdIsHeadingValid()) { + osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading()); + return true; + } else { + buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE; + buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE; + buff[OSD_HEADING_GRAPH_WIDTH] = '\0'; + } + break; + } - case OSD_EFFICIENCY_MAH_PER_KM: - { - // amperage is in centi amps, speed is in cms/s. We want - // mah/km. Only show when ground speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; - bool moreThanAh = false; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - uint8_t digits = 3U; + case OSD_EFFICIENCY_MAH_PER_KM: + { + // amperage is in centi amps, speed is in cms/s. We want + // mah/km. Only show when ground speed > 1m/s. + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + bool moreThanAh = false; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + uint8_t digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber - digits = 4U; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber + digits = 4U; + } #endif - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, + 1, US2S(efficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; - buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode - buff[digits + 1] = SYM_MAH_MI_1; - buff[digits + 2] = '\0'; - } - break; - case OSD_UNIT_GA: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - 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'; - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - 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'; - } - break; - } - break; - } + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode + buff[digits + 1] = SYM_MAH_MI_1; + buff[digits + 2] = '\0'; + } + break; + case OSD_UNIT_GA: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + 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'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + 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'; + } + break; + } + break; + } - case OSD_EFFICIENCY_WH_PER_KM: - { - // amperage is in centi amps, speed is in cms/s. We want - // mWh/km. Only show when ground speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + case OSD_EFFICIENCY_WH_PER_KM: + { + // amperage is in centi amps, speed is in cms/s. We want + // mWh/km. Only show when ground speed > 1m/s. + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, + 1, US2S(efficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); - buff[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); - buff[3] = SYM_WH_NM; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); - buff[3] = SYM_WH_KM; - break; - } - buff[4] = '\0'; - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - break; - } + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); + buff[3] = SYM_WH_KM; + break; + } + buff[4] = '\0'; + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + break; + } - case OSD_GFORCE: - { - buff[0] = SYM_GFORCE; - osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); - if (GForce > osdConfig()->gforce_alarm * 100) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + case OSD_GFORCE: + { + buff[0] = SYM_GFORCE; + osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); + if (GForce > osdConfig()->gforce_alarm * 100) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_GFORCE_X: - case OSD_GFORCE_Y: - case OSD_GFORCE_Z: - { - float GForceValue = GForceAxis[item - OSD_GFORCE_X]; - buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; - osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); - if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } - case OSD_DEBUG: - { - /* - * Longest representable string is -2147483648 does not fit in the screen. - * Only 7 digits for negative and 8 digits for positive values allowed - */ - for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { - tfp_sprintf( - buff, - "[%u]=%8ld [%u]=%8ld", - bufferIndex, - (long)constrain(debug[bufferIndex], -9999999, 99999999), - bufferIndex+1, - (long)constrain(debug[bufferIndex+1], -9999999, 99999999) - ); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - } - break; - } + case OSD_GFORCE_X: + case OSD_GFORCE_Y: + case OSD_GFORCE_Z: + { + float GForceValue = GForceAxis[item - OSD_GFORCE_X]; + buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; + osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); + if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_DEBUG: + { + /* + * Longest representable string is -2147483648 does not fit in the screen. + * Only 7 digits for negative and 8 digits for positive values allowed + */ + for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { + tfp_sprintf( + buff, + "[%u]=%8ld [%u]=%8ld", + bufferIndex, + (long)constrain(debug[bufferIndex], -9999999, 99999999), + bufferIndex+1, + (long)constrain(debug[bufferIndex+1], -9999999, 99999999) + ); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + } + break; + } - case OSD_IMU_TEMPERATURE: - { - int16_t temperature; - const bool valid = getIMUTemperature(&temperature); - osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); - return true; - } + case OSD_IMU_TEMPERATURE: + { + int16_t temperature; + const bool valid = getIMUTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } - case OSD_BARO_TEMPERATURE: - { - int16_t temperature; - const bool valid = getBaroTemperature(&temperature); - osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); - return true; - } + case OSD_BARO_TEMPERATURE: + { + int16_t temperature; + const bool valid = getBaroTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } #ifdef USE_TEMPERATURE_SENSOR - case OSD_TEMP_SENSOR_0_TEMPERATURE: - case OSD_TEMP_SENSOR_1_TEMPERATURE: - case OSD_TEMP_SENSOR_2_TEMPERATURE: - case OSD_TEMP_SENSOR_3_TEMPERATURE: - case OSD_TEMP_SENSOR_4_TEMPERATURE: - case OSD_TEMP_SENSOR_5_TEMPERATURE: - case OSD_TEMP_SENSOR_6_TEMPERATURE: - case OSD_TEMP_SENSOR_7_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE); - return true; - } + case OSD_TEMP_SENSOR_0_TEMPERATURE: + case OSD_TEMP_SENSOR_1_TEMPERATURE: + case OSD_TEMP_SENSOR_2_TEMPERATURE: + case OSD_TEMP_SENSOR_3_TEMPERATURE: + case OSD_TEMP_SENSOR_4_TEMPERATURE: + case OSD_TEMP_SENSOR_5_TEMPERATURE: + case OSD_TEMP_SENSOR_6_TEMPERATURE: + case OSD_TEMP_SENSOR_7_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE); + return true; + } #endif /* ifdef USE_TEMPERATURE_SENSOR */ - case OSD_WIND_SPEED_HORIZONTAL: + case OSD_WIND_SPEED_HORIZONTAL: #ifdef USE_WIND_ESTIMATOR - { - bool valid = isEstimatedWindSpeedValid(); - float horizontalWindSpeed; - uint16_t angle; - horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); - int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); - buff[0] = SYM_WIND_HORIZONTAL; - buff[1] = SYM_DIRECTION + (windDirection*2 / 90); - osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid); - break; - } + { + bool valid = isEstimatedWindSpeedValid(); + float horizontalWindSpeed; + uint16_t angle; + horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); + int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); + buff[0] = SYM_WIND_HORIZONTAL; + buff[1] = SYM_DIRECTION + (windDirection*2 / 90); + osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid); + break; + } #else - return false; + return false; #endif - case OSD_WIND_SPEED_VERTICAL: + case OSD_WIND_SPEED_VERTICAL: #ifdef USE_WIND_ESTIMATOR - { - buff[0] = SYM_WIND_VERTICAL; - buff[1] = SYM_BLANK; - bool valid = isEstimatedWindSpeedValid(); - float verticalWindSpeed; - verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU - if (verticalWindSpeed < 0) { - buff[1] = SYM_AH_DIRECTION_DOWN; - verticalWindSpeed = -verticalWindSpeed; - } else { - buff[1] = SYM_AH_DIRECTION_UP; - } - osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid); - break; - } + { + buff[0] = SYM_WIND_VERTICAL; + buff[1] = SYM_BLANK; + bool valid = isEstimatedWindSpeedValid(); + float verticalWindSpeed; + verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU + if (verticalWindSpeed < 0) { + buff[1] = SYM_AH_DIRECTION_DOWN; + verticalWindSpeed = -verticalWindSpeed; + } else { + buff[1] = SYM_AH_DIRECTION_UP; + } + osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid); + break; + } #else - return false; + return false; #endif - case OSD_PLUS_CODE: - { - 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)) { - olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); - } else { - // +codes with > 8 digits have a + at the 9th digit - // and we only support 10 and up. - memset(buff, '-', digits + 1); - buff[8] = '+'; - buff[digits + 1] = '\0'; - } - // Optionally trim digits from the left - memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved); - buff[digits + 1 - digitsRemoved] = '\0'; - break; - } + case OSD_PLUS_CODE: + { + 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)) { + olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); + } else { + // +codes with > 8 digits have a + at the 9th digit + // and we only support 10 and up. + memset(buff, '-', digits + 1); + buff[8] = '+'; + buff[digits + 1] = '\0'; + } + // Optionally trim digits from the left + memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved); + buff[digits + 1 - digitsRemoved] = '\0'; + break; + } - case OSD_AZIMUTH: - { + case OSD_AZIMUTH: + { - buff[0] = SYM_AZIMUTH; - if (osdIsHeadingValid()) { - int16_t h = GPS_directionToHome; - if (h < 0) { - h += 360; - } - if (h >= 180) - h = h - 180; - else - h = h + 180; + buff[0] = SYM_AZIMUTH; + if (osdIsHeadingValid()) { + int16_t h = GPS_directionToHome; + if (h < 0) { + h += 360; + } + if (h >= 180) + h = h - 180; + else + h = h + 180; - tfp_sprintf(&buff[1], "%3d", h); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + tfp_sprintf(&buff[1], "%3d", h); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_MAP_SCALE: - { - float scaleToUnit; - int scaleUnitDivisor; - char symUnscaled; - char symScaled; - int maxDecimals; + case OSD_MAP_SCALE: + { + float scaleToUnit; + int scaleUnitDivisor; + char symUnscaled; + char symScaled; + int maxDecimals; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() - scaleUnitDivisor = 0; - symUnscaled = SYM_MI; - symScaled = SYM_MI; - maxDecimals = 2; - break; - case OSD_UNIT_GA: - scaleToUnit = 100 / 1852.0010f; // scale to 0.01mi for osdFormatCentiNumber() - scaleUnitDivisor = 0; - symUnscaled = SYM_NM; - symScaled = SYM_NM; - maxDecimals = 2; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() - scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m - symUnscaled = SYM_M; - symScaled = SYM_KM; - maxDecimals = 0; - break; - } - buff[0] = SYM_SCALE; - if (osdMapData.scale > 0) { - bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); - buff[4] = scaled ? symScaled : symUnscaled; - // Make sure this is cleared if the map stops being drawn - osdMapData.scale = 0; - } else { - memset(&buff[1], '-', 4); - } - buff[5] = '\0'; - break; - } - case OSD_MAP_REFERENCE: - { - char referenceSymbol; - if (osdMapData.referenceSymbol) { - referenceSymbol = osdMapData.referenceSymbol; - // Make sure this is cleared if the map stops being drawn - osdMapData.referenceSymbol = 0; - } else { - referenceSymbol = '-'; - } - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); - displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); - return true; - } + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_MI; + symScaled = SYM_MI; + maxDecimals = 2; + break; + case OSD_UNIT_GA: + scaleToUnit = 100 / 1852.0010f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_NM; + symScaled = SYM_NM; + maxDecimals = 2; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() + scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m + symUnscaled = SYM_M; + symScaled = SYM_KM; + maxDecimals = 0; + break; + } + buff[0] = SYM_SCALE; + if (osdMapData.scale > 0) { + bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); + buff[4] = scaled ? symScaled : symUnscaled; + // Make sure this is cleared if the map stops being drawn + osdMapData.scale = 0; + } else { + memset(&buff[1], '-', 4); + } + buff[5] = '\0'; + break; + } + case OSD_MAP_REFERENCE: + { + char referenceSymbol; + if (osdMapData.referenceSymbol) { + referenceSymbol = osdMapData.referenceSymbol; + // Make sure this is cleared if the map stops being drawn + osdMapData.referenceSymbol = 0; + } else { + referenceSymbol = '-'; + } + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); + displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); + return true; + } - case OSD_GVAR_0: - { - osdFormatGVar(buff, 0); - break; - } - case OSD_GVAR_1: - { - osdFormatGVar(buff, 1); - break; - } - case OSD_GVAR_2: - { - osdFormatGVar(buff, 2); - break; - } - case OSD_GVAR_3: - { - osdFormatGVar(buff, 3); - break; - } + case OSD_GVAR_0: + { + osdFormatGVar(buff, 0); + break; + } + case OSD_GVAR_1: + { + osdFormatGVar(buff, 1); + break; + } + case OSD_GVAR_2: + { + osdFormatGVar(buff, 2); + break; + } + case OSD_GVAR_3: + { + osdFormatGVar(buff, 3); + break; + } #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - case OSD_RC_SOURCE: - { - const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD"; - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr); - return true; - } + case OSD_RC_SOURCE: + { + const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD"; + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr); + return true; + } #endif #if defined(USE_ESC_SENSOR) - case OSD_ESC_RPM: - { - escSensorData_t * escSensor = escSensorGetData(); - if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { - osdFormatRpm(buff, escSensor->rpm); - } - else { - osdFormatRpm(buff, 0); - } - break; - } - case OSD_ESC_TEMPERATURE: - { - escSensorData_t * escSensor = escSensorGetData(); - bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE; - osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max); - return true; - } + case OSD_ESC_RPM: + { + escSensorData_t * escSensor = escSensorGetData(); + if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { + osdFormatRpm(buff, escSensor->rpm); + } + else { + osdFormatRpm(buff, 0); + } + break; + } + case OSD_ESC_TEMPERATURE: + { + escSensorData_t * escSensor = escSensorGetData(); + bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE; + osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max); + return true; + } #endif - case OSD_TPA: - { - char buff[4]; - textAttributes_t attr; + case OSD_TPA: + { + char buff[4]; + textAttributes_t attr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); - attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); - if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { - TEXT_ATTRIBUTES_ADD_BLINK(attr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); - displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); - attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); - if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { - TEXT_ATTRIBUTES_ADD_BLINK(attr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); - return true; - } - case OSD_TPA_TIME_CONSTANT: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); - return true; - } - case OSD_FW_LEVEL_TRIM: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, getFixedWingLevelTrim(), 3, 1, ADJUSTMENT_FW_LEVEL_TRIM); - return true; - } + return true; + } + case OSD_TPA_TIME_CONSTANT: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); + return true; + } + case OSD_FW_LEVEL_TRIM: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, getFixedWingLevelTrim(), 3, 1, ADJUSTMENT_FW_LEVEL_TRIM); + return true; + } - case OSD_NAV_FW_CONTROL_SMOOTHNESS: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); - return true; - } + case OSD_NAV_FW_CONTROL_SMOOTHNESS: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); + return true; + } #ifdef USE_MULTI_MISSION - case OSD_NAV_WP_MULTI_MISSION_INDEX: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "WP NO", 0, navConfig()->general.waypoint_multi_mission_index, 1, 0, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX); - return true; - } + case OSD_NAV_WP_MULTI_MISSION_INDEX: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "WP NO", 0, navConfig()->general.waypoint_multi_mission_index, 1, 0, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX); + return true; + } #endif - case OSD_MISSION: - { - if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) { - char buf[5]; - switch (posControl.wpMissionPlannerStatus) { - case WP_PLAN_WAIT: - strcpy(buf, "WAIT"); - break; - case WP_PLAN_SAVE: - strcpy(buf, "SAVE"); - break; - case WP_PLAN_OK: - strcpy(buf, " OK "); - break; - case WP_PLAN_FULL: - strcpy(buf, "FULL"); - } - tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex); - } else if (posControl.wpPlannerActiveWPIndex){ - tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active - } + case OSD_MISSION: + { + if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) { + char buf[5]; + switch (posControl.wpMissionPlannerStatus) { + case WP_PLAN_WAIT: + strcpy(buf, "WAIT"); + break; + case WP_PLAN_SAVE: + strcpy(buf, "SAVE"); + break; + case WP_PLAN_OK: + strcpy(buf, " OK "); + break; + case WP_PLAN_FULL: + strcpy(buf, "FULL"); + } + tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex); + } else if (posControl.wpPlannerActiveWPIndex){ + tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active + } #ifdef USE_MULTI_MISSION - else { - if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ - // Limit field size when Armed, only show selected mission - tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex); - } else if (posControl.multiMissionCount) { - if (navConfig()->general.waypoint_multi_mission_index != posControl.loadedMultiMissionIndex) { - tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount); - } else { - if (posControl.waypointListValid && posControl.waypointCount > 0) { - tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - } else { - tfp_sprintf(buff, "M0/%u> 0WP", posControl.multiMissionCount); - } - } - } else { // no multi mission loaded - show active WP count from other source - tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount); - } - } + else { + if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ + // Limit field size when Armed, only show selected mission + tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex); + } else if (posControl.multiMissionCount) { + if (navConfig()->general.waypoint_multi_mission_index != posControl.loadedMultiMissionIndex) { + tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount); + } else { + if (posControl.waypointListValid && posControl.waypointCount > 0) { + tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + } else { + tfp_sprintf(buff, "M0/%u> 0WP", posControl.multiMissionCount); + } + } + } else { // no multi mission loaded - show active WP count from other source + tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount); + } + } #endif - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - return true; - } + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + return true; + } #ifdef USE_POWER_LIMITS - case OSD_PLIMIT_REMAINING_BURST_TIME: - osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); - buff[3] = 'S'; - buff[4] = '\0'; - break; + case OSD_PLIMIT_REMAINING_BURST_TIME: + osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); + buff[3] = 'S'; + buff[4] = '\0'; + break; - case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: - if (currentBatteryProfile->powerLimits.continuousCurrent) { - osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); - buff[3] = SYM_AMP; - buff[4] = '\0'; + case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: + if (currentBatteryProfile->powerLimits.continuousCurrent) { + osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; - if (powerLimiterIsLimitingCurrent()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + if (powerLimiterIsLimitingCurrent()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; #ifdef USE_ADC - case OSD_PLIMIT_ACTIVE_POWER_LIMIT: - { - if (currentBatteryProfile->powerLimits.continuousPower) { - bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; + case OSD_PLIMIT_ACTIVE_POWER_LIMIT: + { + if (currentBatteryProfile->powerLimits.continuousPower) { + bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; - if (powerLimiterIsLimitingPower()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; - } + if (powerLimiterIsLimitingPower()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; + } #endif // USE_ADC #endif // USE_POWER_LIMITS - default: - return false; - } + default: + return false; + } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - return true; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + return true; } uint8_t osdIncElementIndex(uint8_t elementIndex) { - ++elementIndex; + ++elementIndex; - if (elementIndex == OSD_ARTIFICIAL_HORIZON) { // always drawn last so skip - elementIndex++; - } + if (elementIndex == OSD_ARTIFICIAL_HORIZON) { // always drawn last so skip + elementIndex++; + } #ifndef USE_TEMPERATURE_SENSOR - if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) { - elementIndex = OSD_ALTITUDE_MSL; - } + if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) { + elementIndex = OSD_ALTITUDE_MSL; + } #endif - if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))) { - if (elementIndex == OSD_POWER) { - elementIndex = OSD_GPS_LON; - } - if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { - elementIndex = OSD_LEVEL_PIDS; - } + if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))) { + if (elementIndex == OSD_POWER) { + elementIndex = OSD_GPS_LON; + } + if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { + elementIndex = OSD_LEVEL_PIDS; + } #ifdef USE_POWER_LIMITS - if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { - elementIndex = OSD_GLIDESLOPE; - } + if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { + elementIndex = OSD_GLIDESLOPE; + } #endif - } + } #ifndef USE_POWER_LIMITS - if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { - elementIndex = OSD_GLIDESLOPE; - } + if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { + elementIndex = OSD_GLIDESLOPE; + } #endif - if (!feature(FEATURE_CURRENT_METER)) { - if (elementIndex == OSD_CURRENT_DRAW) { - elementIndex = OSD_GPS_SPEED; - } - if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { - elementIndex = OSD_BATTERY_REMAINING_PERCENT; - } - if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { - elementIndex = OSD_TRIP_DIST; - } - if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) { - elementIndex = OSD_HOME_HEADING_ERROR; - } - if (elementIndex == OSD_CLIMB_EFFICIENCY) { - elementIndex = OSD_NAV_WP_MULTI_MISSION_INDEX; - } - } + if (!feature(FEATURE_CURRENT_METER)) { + if (elementIndex == OSD_CURRENT_DRAW) { + elementIndex = OSD_GPS_SPEED; + } + if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { + elementIndex = OSD_BATTERY_REMAINING_PERCENT; + } + if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { + elementIndex = OSD_TRIP_DIST; + } + if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) { + elementIndex = OSD_HOME_HEADING_ERROR; + } + if (elementIndex == OSD_CLIMB_EFFICIENCY) { + elementIndex = OSD_NAV_WP_MULTI_MISSION_INDEX; + } + } - if (!STATE(ESC_SENSOR_ENABLED)) { - if (elementIndex == OSD_ESC_RPM) { - elementIndex = OSD_AZIMUTH; - } - } + if (!STATE(ESC_SENSOR_ENABLED)) { + if (elementIndex == OSD_ESC_RPM) { + elementIndex = OSD_AZIMUTH; + } + } - if (!feature(FEATURE_GPS)) { - if (elementIndex == OSD_GPS_HDOP || elementIndex == OSD_TRIP_DIST || elementIndex == OSD_3D_SPEED || elementIndex == OSD_MISSION || - elementIndex == OSD_AZIMUTH || elementIndex == OSD_BATTERY_REMAINING_CAPACITY || elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { - elementIndex++; - } - if (elementIndex == OSD_HEADING_GRAPH && !sensors(SENSOR_MAG)) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_WH_DRAWN : OSD_BATTERY_REMAINING_PERCENT; - } - if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { - elementIndex = OSD_ATTITUDE_PITCH; - } - if (elementIndex == OSD_GPS_SPEED) { - elementIndex = OSD_ALTITUDE; - } - if (elementIndex == OSD_GPS_LON) { - elementIndex = sensors(SENSOR_MAG) ? OSD_HEADING : OSD_VARIO; - } - if (elementIndex == OSD_MAP_NORTH) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE : OSD_LEVEL_PIDS; - } - if (elementIndex == OSD_PLUS_CODE) { - elementIndex = OSD_GFORCE; - } - if (elementIndex == OSD_GLIDESLOPE) { - elementIndex = OSD_AIR_MAX_SPEED; - } - if (elementIndex == OSD_GLIDE_RANGE) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; - } - if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { - elementIndex = OSD_PILOT_NAME; - } - } + if (!feature(FEATURE_GPS)) { + if (elementIndex == OSD_GPS_HDOP || elementIndex == OSD_TRIP_DIST || elementIndex == OSD_3D_SPEED || elementIndex == OSD_MISSION || + elementIndex == OSD_AZIMUTH || elementIndex == OSD_BATTERY_REMAINING_CAPACITY || elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { + elementIndex++; + } + if (elementIndex == OSD_HEADING_GRAPH && !sensors(SENSOR_MAG)) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_WH_DRAWN : OSD_BATTERY_REMAINING_PERCENT; + } + if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { + elementIndex = OSD_ATTITUDE_PITCH; + } + if (elementIndex == OSD_GPS_SPEED) { + elementIndex = OSD_ALTITUDE; + } + if (elementIndex == OSD_GPS_LON) { + elementIndex = sensors(SENSOR_MAG) ? OSD_HEADING : OSD_VARIO; + } + if (elementIndex == OSD_MAP_NORTH) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE : OSD_LEVEL_PIDS; + } + if (elementIndex == OSD_PLUS_CODE) { + elementIndex = OSD_GFORCE; + } + if (elementIndex == OSD_GLIDESLOPE) { + elementIndex = OSD_AIR_MAX_SPEED; + } + if (elementIndex == OSD_GLIDE_RANGE) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; + } + if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { + elementIndex = OSD_PILOT_NAME; + } + } - if (!sensors(SENSOR_ACC)) { - if (elementIndex == OSD_CROSSHAIRS) { - elementIndex = OSD_ONTIME; - } - if (elementIndex == OSD_GFORCE) { - elementIndex = OSD_RC_SOURCE; - } - } + if (!sensors(SENSOR_ACC)) { + if (elementIndex == OSD_CROSSHAIRS) { + elementIndex = OSD_ONTIME; + } + if (elementIndex == OSD_GFORCE) { + elementIndex = OSD_RC_SOURCE; + } + } - if (elementIndex == OSD_ITEM_COUNT) { - elementIndex = 0; - } - return elementIndex; + if (elementIndex == OSD_ITEM_COUNT) { + elementIndex = 0; + } + return elementIndex; } void osdDrawNextElement(void) { - static uint8_t elementIndex = 0; - // Flag for end of loop, also prevents infinite loop when no elements are enabled - uint8_t index = elementIndex; - do { - elementIndex = osdIncElementIndex(elementIndex); - } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); + static uint8_t elementIndex = 0; + // Flag for end of loop, also prevents infinite loop when no elements are enabled + uint8_t index = elementIndex; + do { + elementIndex = osdIncElementIndex(elementIndex); + } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon + tracking telemtry last - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - if (osdConfig()->telemetry>0){ - osdDisplayTelemetry(); - } + // Draw artificial horizon + tracking telemtry last + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, - .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, - .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, - .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, - .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, - .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, - .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, - .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, - .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, - .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, - .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, - .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, - .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, - .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, + .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, + .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, + .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, + .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, + .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, + .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, + .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, + .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, + .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, + .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, + .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, + .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, + .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, #ifdef USE_BARO - .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, - .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, + .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, + .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif #ifdef USE_SERIALRX_CRSF - .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, - .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, - .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, - .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, - .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, - .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, + .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, + .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, + .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, + .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, + .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, + .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR - .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, + .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, #endif #ifdef USE_PITOT - .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT, - .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, + .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT, + .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, #endif - .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, - .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, - .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT, + .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, + .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, + .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT, - .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, - .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, - .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, - .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, - .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, - .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT, - .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, - .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, - .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, - .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, - .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, - .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, - .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, - .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, - .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, - .hud_radar_alt_difference_display_time = SETTING_OSD_HUD_RADAR_ALT_DIFFERENCE_DISPLAY_TIME_DEFAULT, - .hud_radar_distance_display_time = SETTING_OSD_HUD_RADAR_DISTANCE_DISPLAY_TIME_DEFAULT, - .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, - .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, - .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, - .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, - .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT, - .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT, - .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT, - .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT, - .ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT, - .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, - .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, - .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, - .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, - .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, - .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, - .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, - .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, - .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, - .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, - .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, - .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, - .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, - .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, - .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, - .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, - .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, - .units = SETTING_OSD_UNITS_DEFAULT, - .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, + .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, + .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, + .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, + .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, + .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT, + .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, + .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, + .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, + .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, + .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, + .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, + .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, + .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, + .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, + .hud_radar_alt_difference_display_time = SETTING_OSD_HUD_RADAR_ALT_DIFFERENCE_DISPLAY_TIME_DEFAULT, + .hud_radar_distance_display_time = SETTING_OSD_HUD_RADAR_DISTANCE_DISPLAY_TIME_DEFAULT, + .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, + .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, + .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, + .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, + .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT, + .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT, + .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT, + .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT, + .ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT, + .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, + .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, + .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, + .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, + .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, + .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, + .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, + .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, + .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, + .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, + .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, + .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, + .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, + .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, + .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, + .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, + .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, + .units = SETTING_OSD_UNITS_DEFAULT, + .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, #ifdef USE_WIND_ESTIMATOR - .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, + .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, #endif - .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT, + .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT, - .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, + .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, - .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, - .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, + .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, + .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, - .ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT, - .ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT, - .ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT, - .ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT, - .ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT, + .ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT, + .ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT, + .ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT, + .ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT, + .ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT, - .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, + .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, - .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, - .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT, - .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT + .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, + .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT, + .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) { - osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG; - //line 2 - osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); - osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); - osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); - osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2); + osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG; + //line 2 + osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); + osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); + osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); + 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_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); - osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2); - osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3); - osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); - osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); - osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7); - osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8); + osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; + 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); + osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2); + osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3); + osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); + osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); + osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7); + osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8); - osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7); - osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8); + osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7); + osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8); - // avoid OSD_VARIO under OSD_CROSSHAIRS - osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5); - // OSD_VARIO_NUM at the right of OSD_VARIO - osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); - osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); - osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); - osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); + // avoid OSD_VARIO under OSD_CROSSHAIRS + osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5); + // OSD_VARIO_NUM at the right of OSD_VARIO + osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); + osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); + osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); + osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); - osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); - osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); - osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); + osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); + osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); + osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); + osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); + osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); #endif - osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); - osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9); - osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7); - osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6); + osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); + osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9); + osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7); + osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6); - osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10); - osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); + osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10); + osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); - osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); - // Put this on top of the latitude, since it's very unlikely - // that users will want to use both at the same time. - osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12); - osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12); + osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); + // Put this on top of the latitude, since it's very unlikely + // that users will want to use both at the same time. + osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12); + osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12); - osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); - osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11); - osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); - osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); - osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11); + osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); + osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); + osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); - osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2); - osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11); + osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2); + osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11); - osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); - osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); + osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); + osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); - osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); + osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); - osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_GVAR_0] = OSD_POS(1, 1); - osdLayoutsConfig->item_pos[0][OSD_GVAR_1] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); - osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_GVAR_0] = OSD_POS(1, 1); + osdLayoutsConfig->item_pos[0][OSD_GVAR_1] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); + osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); #if defined(USE_ESC_SENSOR) - osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); + osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); #endif #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); + osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); #endif #ifdef USE_POWER_LIMITS - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6); #endif - // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? - osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? + osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; - for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) { - for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) { - osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG; - } - } + for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) { + for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) { + osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG; + } + } } static void osdSetNextRefreshIn(uint32_t timeMs) { - resumeRefreshAt = micros() + timeMs * 1000; - refreshWaitForResumeCmdRelease = true; + resumeRefreshAt = micros() + timeMs * 1000; + refreshWaitForResumeCmdRelease = true; } static void osdCompleteAsyncInitialization(void) { - if (!displayIsReady(osdDisplayPort)) { - // Update the display. - // XXX: Rename displayDrawScreen() and associated functions - // to displayUpdate() - displayDrawScreen(osdDisplayPort); - return; - } + if (!displayIsReady(osdDisplayPort)) { + // Update the display. + // XXX: Rename displayDrawScreen() and associated functions + // to displayUpdate() + displayDrawScreen(osdDisplayPort); + return; + } - osdDisplayIsReady = true; + osdDisplayIsReady = true; #if defined(USE_CANVAS) - if (osdConfig()->force_grid) { - osdDisplayHasCanvas = false; - } else { - osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); - } + if (osdConfig()->force_grid) { + osdDisplayHasCanvas = false; + } else { + osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); + } #endif - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - displayClearScreen(osdDisplayPort); + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); - uint8_t y = 1; - displayFontMetadata_t metadata; - bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); - LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", - fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); + uint8_t y = 1; + displayFontMetadata_t metadata; + bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); + LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", + fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); - if (fontHasMetadata && metadata.charCount > 256) { - hasExtendedFont = true; - unsigned logo_c = SYM_LOGO_START; - unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); - for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { - for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { - displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); - } - y++; - } - y++; - } else if (!fontHasMetadata) { - const char *m = "INVALID FONT"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); - y = 4; - } + if (fontHasMetadata && metadata.charCount > 256) { + hasExtendedFont = true; + unsigned logo_c = SYM_LOGO_START; + unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); + for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { + for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { + displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); + } + y++; + } + y++; + } else if (!fontHasMetadata) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); + y = 4; + } - if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { - const char *m = "INVALID FONT VERSION"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); - } + if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT VERSION"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } - char string_buffer[30]; - tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); - uint8_t xPos = osdDisplayIsHD() ? 15 : 5; - displayWrite(osdDisplayPort, xPos, y++, string_buffer); + char string_buffer[30]; + tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); + uint8_t xPos = osdDisplayIsHD() ? 15 : 5; + displayWrite(osdDisplayPort, xPos, y++, string_buffer); #ifdef USE_CMS - displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); + displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); #endif #ifdef USE_STATS - uint8_t statNameX = osdDisplayIsHD() ? 14 : 4; - uint8_t statValueX = osdDisplayIsHD() ? 34 : 24; + uint8_t statNameX = osdDisplayIsHD() ? 14 : 4; + uint8_t statValueX = osdDisplayIsHD() ? 34 : 24; - if (statsConfig()->stats_enabled) { - displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:"); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); - string_buffer[5] = SYM_MI; - break; - default: - case OSD_UNIT_GA: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); - string_buffer[5] = SYM_NM; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); - string_buffer[5] = SYM_KM; - break; - } - string_buffer[6] = '\0'; - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); + if (statsConfig()->stats_enabled) { + displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:"); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + string_buffer[5] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + string_buffer[5] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + string_buffer[5] = SYM_KM; + break; + } + string_buffer[6] = '\0'; + displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:"); - uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60)); - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); + displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:"); + uint32_t tot_mins = statsConfig()->stats_total_time / 60; + tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60)); + displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); #ifdef USE_ADC - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:"); - osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); - strcat(string_buffer, "\xAB"); // SYM_WH - displayWrite(osdDisplayPort, statValueX-4, y, string_buffer); + if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:"); + osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); + strcat(string_buffer, "\xAB"); // SYM_WH + displayWrite(osdDisplayPort, statValueX-4, y, string_buffer); - displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:"); - if (statsConfig()->stats_total_dist) { - uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_NM; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_KM; - break; - } - } else { - string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; - } - string_buffer[4] = '\0'; - displayWrite(osdDisplayPort, statValueX-3, y, string_buffer); - } + displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:"); + if (statsConfig()->stats_total_dist) { + uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_NM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + break; + } + } else { + string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; + } + string_buffer[4] = '\0'; + displayWrite(osdDisplayPort, statValueX-3, y, string_buffer); + } #endif // USE_ADC - } + } #endif - displayCommitTransaction(osdDisplayPort); - displayResync(osdDisplayPort); - osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); + displayCommitTransaction(osdDisplayPort); + displayResync(osdDisplayPort); + osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); } void osdInit(displayPort_t *osdDisplayPortToUse) { - if (!osdDisplayPortToUse) - return; + if (!osdDisplayPortToUse) + return; - BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); + BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); - osdDisplayPort = osdDisplayPortToUse; + osdDisplayPort = osdDisplayPortToUse; #ifdef USE_CMS - cmsDisplayPortRegister(osdDisplayPort); + cmsDisplayPortRegister(osdDisplayPort); #endif - armState = ARMING_FLAG(ARMED); - osdCompleteAsyncInitialization(); + armState = ARMING_FLAG(ARMED); + osdCompleteAsyncInitialization(); } static void osdResetStats(void) { - stats.max_current = 0; - stats.max_power = 0; - stats.max_speed = 0; - stats.max_3D_speed = 0; - stats.max_air_speed = 0; - stats.min_voltage = 5000; - stats.min_rssi = 99; - stats.min_lq = 300; - stats.min_rssi_dbm = 0; - stats.max_altitude = 0; + stats.max_current = 0; + stats.max_power = 0; + stats.max_speed = 0; + stats.max_3D_speed = 0; + stats.max_air_speed = 0; + stats.min_voltage = 5000; + stats.min_rssi = 99; + stats.min_lq = 300; + stats.min_rssi_dbm = 0; + stats.max_altitude = 0; } static void osdUpdateStats(void) { - int32_t value; + int32_t value; - if (feature(FEATURE_GPS)) { - value = osdGet3DSpeed(); - const float airspeed_estimate = getAirspeedEstimate(); + if (feature(FEATURE_GPS)) { + value = osdGet3DSpeed(); + const float airspeed_estimate = getAirspeedEstimate(); - if (stats.max_3D_speed < value) - stats.max_3D_speed = value; + if (stats.max_3D_speed < value) + stats.max_3D_speed = value; - if (stats.max_speed < gpsSol.groundSpeed) - stats.max_speed = gpsSol.groundSpeed; + if (stats.max_speed < gpsSol.groundSpeed) + stats.max_speed = gpsSol.groundSpeed; - if (stats.max_air_speed < airspeed_estimate) - stats.max_air_speed = airspeed_estimate; + if (stats.max_air_speed < airspeed_estimate) + stats.max_air_speed = airspeed_estimate; - if (stats.max_distance < GPS_distanceToHome) - stats.max_distance = GPS_distanceToHome; - } + if (stats.max_distance < GPS_distanceToHome) + stats.max_distance = GPS_distanceToHome; + } - value = getBatteryVoltage(); - if (stats.min_voltage > value) - stats.min_voltage = value; + value = getBatteryVoltage(); + if (stats.min_voltage > value) + stats.min_voltage = value; - value = abs(getAmperage()); - if (stats.max_current < value) - stats.max_current = value; + value = abs(getAmperage()); + if (stats.max_current < value) + stats.max_current = value; - value = labs(getPower()); - if (stats.max_power < value) - stats.max_power = value; + value = labs(getPower()); + if (stats.max_power < value) + stats.max_power = value; - value = osdConvertRSSI(); - if (stats.min_rssi > value) - stats.min_rssi = value; + value = osdConvertRSSI(); + if (stats.min_rssi > value) + stats.min_rssi = value; - value = osdGetCrsfLQ(); - if (stats.min_lq > value) - stats.min_lq = value; + value = osdGetCrsfLQ(); + if (stats.min_lq > value) + stats.min_lq = value; - if (!failsafeIsReceivingRxData()) - stats.min_lq = 0; + if (!failsafeIsReceivingRxData()) + stats.min_lq = 0; - value = osdGetCrsfdBm(); - if (stats.min_rssi_dbm > value) - stats.min_rssi_dbm = value; + value = osdGetCrsfdBm(); + if (stats.min_rssi_dbm > value) + stats.min_rssi_dbm = value; - stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); + stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); } static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) { - const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; - uint8_t top = 1; // Start one line down leaving space at the top of the screen. - size_t multiValueLengthOffset = 0; + const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; + uint8_t top = 1; // Start one line down leaving space at the top of the screen. + size_t multiValueLengthOffset = 0; - const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; - const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20; - char buff[10]; + const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; + const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20; + char buff[10]; - if (page > 1) - page = 0; + if (page > 1) + page = 0; - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - displayClearScreen(osdDisplayPort); + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); - } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); - } else if (page == 1) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); - } + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); + } else if (page == 0) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + } else if (page == 1) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); + } - if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + if (isSinglePageStatsCompatible || page == 0) { + if (feature(FEATURE_GPS)) { + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); - osdFormatDistanceStr(buff, stats.max_distance*100); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); + osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); - osdFormatDistanceStr(buff, getTotalTravelDistance()); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); + osdFormatDistanceStr(buff, getTotalTravelDistance()); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); - osdFormatAltitudeStr(buff, stats.max_altitude); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); + osdFormatAltitudeStr(buff, stats.max_altitude); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); - itoa(stats.min_rssi, buff, 10); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff), "%/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); + itoa(stats.min_rssi, buff, 10); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff), "%/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); - } - - if (isSinglePageStatsCompatible || page == 1) { - if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); - } - tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + } + + if (isSinglePageStatsCompatible || page == 1) { + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + } + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_AMP); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + if (feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); - } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_WH); - } - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + } else { + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH); + } + displayWrite(osdDisplayPort, statValuesX, top++, buff); - int32_t totalDistance = getTotalTravelDistance(); - bool moreThanAh = false; - bool efficiencyValid = totalDistance >= 10000; - if (feature(FEATURE_GPS)) { - displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - uint8_t digits = 3U; // Total number of digits (including decimal point) - #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so no switch to scaled decimal occurs above 99 - digits = 4U; - } - #endif - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_GA: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_NM_0; - buff[4] = SYM_MAH_NM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - } - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } - } + int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; + if (feature(FEATURE_GPS)) { + displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; + } + #endif + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + } + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } + } - const float max_gforce = accGetMeasuredMaxG(); - displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); - const float acc_extremes_min = acc_extremes[Z].min; - const float acc_extremes_max = acc_extremes[Z].max; - displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); - osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const float acc_extremes_min = acc_extremes[Z].min; + const float acc_extremes_max = acc_extremes[Z].max; + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } - if (savingSettings == true) { - displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); - } else if (notify_settings_saved > 0) { - if (millis() > notify_settings_saved) { - notify_settings_saved = 0; - } else { - displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); - } - } + if (savingSettings == true) { + displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); + } else if (notify_settings_saved > 0) { + if (millis() > notify_settings_saved) { + notify_settings_saved = 0; + } else { + displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); + } + } - displayCommitTransaction(osdDisplayPort); + displayCommitTransaction(osdDisplayPort); } // HD arming screen. based on the minimum HD OSD grid size of 50 x 18 static void osdShowHDArmScreen(void) { - dateTime_t dt; - char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[30]; - char *date; - char *time; + dateTime_t dt; + char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[30]; + char *date; + char *time; + + uint8_t armScreenRow = 2; // Start at row 2 + + // Draw Logo(s) + uint8_t logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); + // uint8_t logoColOffset = floor((osdDisplayPort->cols - ((using pilot logo) ? (SYM_LOGO_WIDTH * 2) : SYM_LOGO_WIDTH)) / 2.0f); + + // Draw INAV logo + unsigned logo_c = SYM_LOGO_START; + uint8_t logo_x = logoColOffset; + uint8_t logoRow = armScreenRow; + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + + // if (using pilot logo) { + logo_c = SYM_PILOT_LOGO_LRG_START; + logo_x = (logoColOffset * 2) + ((osdDisplayPort->cols % 2 == 0) ? 0 : 1); // Add extra 1 px space between logos, if the OSD has an odd number of columns + logoRow = armScreenRow; + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + // } // using pilot logo + + armScreenRow = logoRow + 2; + + strcpy(buf, "ARMED"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + armScreenRow += 2; + + if (strlen(systemConfig()->craftName) > 0) { + osdFormatCraftName(craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + } + + if (posControl.waypointListValid && posControl.waypointCount > 0) { +#ifdef USE_MULTI_MISSION + tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); +#else + strcpy(buf, "*MISSION LOADED*"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); +#endif + } + armScreenRow++; + +#if defined(USE_GPS) + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + if (osdConfig()->osd_home_position_arm_screen){ + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } + armScreenRow++; +#if defined (USE_SAFE_HOME) + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + char buf2[12]; // format the distance first + osdFormatDistanceStr(buf2, safehome_distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message above the ARMED message to make it obvious (3rd parameter - row 7) + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, 7, buf, elemAttr); + } +#endif + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; + } + } +#endif + + if (rtcGetDateTime(&dt)) { + dateTimeFormatLocal(buf, &dt); + dateTimeSplitFormatted(buf, &date, &time); + + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, armScreenRow, date); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, armScreenRow++, time); + armScreenRow += 2; + } + + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow, versionBuf); } static void osdShowSDArmScreen(void) { - dateTime_t dt; - char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[30]; - char *date; - char *time; - // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + dateTime_t dt; + char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[30]; + char *date; + char *time; + // We need 12 visible rows, start row never < first fully visible row 1 + uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; - strcpy(buf, "ARMED"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 2; + strcpy(buf, "ARMED"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + y += 2; - if (strlen(systemConfig()->craftName) > 0) { - osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf ); - y += 1; - } - if (posControl.waypointListValid && posControl.waypointCount > 0) { + if (strlen(systemConfig()->craftName) > 0) { + osdFormatCraftName(craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf ); + y += 1; + } + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION - tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); #else - strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + strcpy(buf, "*MISSION LOADED*"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); #endif - } - y += 1; + } + y += 1; #if defined(USE_GPS) - if (feature(FEATURE_GPS)) { - if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); - } - y += 4; + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + if (osdConfig()->osd_home_position_arm_screen){ + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + } + y += 4; #if defined (USE_SAFE_HOME) - if (safehome_distance) { // safehome found during arming - if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { - strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { char buf2[12]; // format the distance first osdFormatDistanceStr(buf2, safehome_distance); @@ -4422,246 +4520,246 @@ static void osdShowSDArmScreen(void) textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message above the ARMED message to make it obvious displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); - } + } #endif - } else { - strcpy(buf, "!NO HOME POSITION!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 1; - } - } + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + y += 1; + } + } #endif - if (rtcGetDateTime(&dt)) { - dateTimeFormatLocal(buf, &dt); - dateTimeSplitFormatted(buf, &date, &time); + if (rtcGetDateTime(&dt)) { + dateTimeFormatLocal(buf, &dt); + dateTimeSplitFormatted(buf, &date, &time); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time); - y += 3; - } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time); + y += 3; + } - tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); } // called when motors armed static void osdShowArmed(void) { - displayClearScreen(osdDisplayPort); + displayClearScreen(osdDisplayPort); - if (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS) { - osdShowHDArmScreen(); - } else { - osdShowSDArmScreen(); - } + if (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS) { + osdShowHDArmScreen(); + } else { + osdShowSDArmScreen(); + } } static void osdFilterData(timeUs_t currentTimeUs) { - static timeUs_t lastRefresh = 0; - float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); + static timeUs_t lastRefresh = 0; + float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); - GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; - if (lastRefresh) { - GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); - } else { - pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); - pt1FilterReset(&GForceFilter, GForce); + if (lastRefresh) { + GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); + } else { + pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); + pt1FilterReset(&GForceFilter, GForce); - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { - pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); - pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); - } - } + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { + pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); + pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); + } + } - lastRefresh = currentTimeUs; + lastRefresh = currentTimeUs; } // Detect when the user is holding the roll stick to the right static bool osdIsPageUpStickCommandHeld(void) { - static int pageUpHoldCount = 1; + static int pageUpHoldCount = 1; - bool keyHeld = false; + bool keyHeld = false; - if (IS_HI(ROLL)) { - keyHeld = true; - } + if (IS_HI(ROLL)) { + keyHeld = true; + } - if (!keyHeld) { - pageUpHoldCount = 1; - } else { - ++pageUpHoldCount; - } + if (!keyHeld) { + pageUpHoldCount = 1; + } else { + ++pageUpHoldCount; + } - if (pageUpHoldCount > 20) { - pageUpHoldCount = 1; - return true; - } + if (pageUpHoldCount > 20) { + pageUpHoldCount = 1; + return true; + } - return false; + return false; } // Detect when the user is holding the roll stick to the left static bool osdIsPageDownStickCommandHeld(void) { - static int pageDownHoldCount = 1; + static int pageDownHoldCount = 1; - bool keyHeld = false; - if (IS_LO(ROLL)) { - keyHeld = true; - } + bool keyHeld = false; + if (IS_LO(ROLL)) { + keyHeld = true; + } - if (!keyHeld) { - pageDownHoldCount = 1; - } else { - ++pageDownHoldCount; - } + if (!keyHeld) { + pageDownHoldCount = 1; + } else { + ++pageDownHoldCount; + } - if (pageDownHoldCount > 20) { - pageDownHoldCount = 1; - return true; - } + if (pageDownHoldCount > 20) { + pageDownHoldCount = 1; + return true; + } - return false; + return false; } static void osdRefresh(timeUs_t currentTimeUs) { - osdFilterData(currentTimeUs); + osdFilterData(currentTimeUs); #ifdef USE_CMS - if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #else - if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #endif - displayClearScreen(osdDisplayPort); - armState = ARMING_FLAG(ARMED); - return; - } + displayClearScreen(osdDisplayPort); + armState = ARMING_FLAG(ARMED); + return; + } - bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); - static uint8_t statsCurrentPage = 0; - static bool statsDisplayed = false; - static bool statsAutoPagingEnabled = true; + bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); + static uint8_t statsCurrentPage = 0; + static bool statsDisplayed = false; + static bool statsAutoPagingEnabled = true; - // Detect arm/disarm - if (armState != ARMING_FLAG(ARMED)) { - if (ARMING_FLAG(ARMED)) { - // Display the "Arming" screen - statsDisplayed = false; - osdResetStats(); - osdShowArmed(); - uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + // Detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) { + // Display the "Arming" screen + statsDisplayed = false; + osdResetStats(); + osdShowArmed(); + uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; #if defined(USE_SAFE_HOME) - if (safehome_distance) - delay *= 3; + if (safehome_distance) + delay *= 3; #endif - osdSetNextRefreshIn(delay); - } else { - // Display the "Stats" screen - statsDisplayed = true; - statsCurrentPage = 0; - statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); - } + osdSetNextRefreshIn(delay); + } else { + // Display the "Stats" screen + statsDisplayed = true; + statsCurrentPage = 0; + statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); + } - armState = ARMING_FLAG(ARMED); - } + armState = ARMING_FLAG(ARMED); + } - // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens - if (resumeRefreshAt) { - - // Handle events only when the "Stats" screen is being displayed. - if (statsDisplayed) { + // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens + if (resumeRefreshAt) { + + // Handle events only when the "Stats" screen is being displayed. + if (statsDisplayed) { - // Manual paging stick commands are only applicable to multi-page stats. - // ****************************** - // 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. - // ****************************** - // 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. - bool manualPageUpRequested = 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()) { - manualPageUpRequested = true; - statsAutoPagingEnabled = false; - } else if (osdIsPageDownStickCommandHeld()) { - manualPageDownRequested = true; - statsAutoPagingEnabled = false; - } - } + // Manual paging stick commands are only applicable to multi-page stats. + // ****************************** + // 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. + // ****************************** + // 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. + bool manualPageUpRequested = 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()) { + manualPageUpRequested = true; + statsAutoPagingEnabled = false; + } else if (osdIsPageDownStickCommandHeld()) { + manualPageDownRequested = true; + statsAutoPagingEnabled = false; + } + } - if (statsAutoPagingEnabled) { - // Alternate screens for multi-page stats. - // Also, refreshes screen at swap interval for single-page stats. - if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { - if (statsCurrentPage == 0) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - statsCurrentPage = 1; - } - } else { - if (statsCurrentPage == 1) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - statsCurrentPage = 0; - } - } - } else { - // Process manual page change events for multi-page stats. - if (manualPageUpRequested) { - osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; - } else if (manualPageDownRequested) { - osdShowStats(statsSinglePageCompatible, 0); - statsCurrentPage = 0; - } - } - } + if (statsAutoPagingEnabled) { + // Alternate screens for multi-page stats. + // Also, refreshes screen at swap interval for single-page stats. + if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { + if (statsCurrentPage == 0) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 1; + } + } else { + if (statsCurrentPage == 1) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 0; + } + } + } else { + // Process manual page change events for multi-page stats. + if (manualPageUpRequested) { + osdShowStats(statsSinglePageCompatible, 1); + statsCurrentPage = 1; + } else if (manualPageDownRequested) { + osdShowStats(statsSinglePageCompatible, 0); + statsCurrentPage = 0; + } + } + } - // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. - if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { - // Time elapsed or canceled by stick commands. - // Exit to normal OSD operation. - displayClearScreen(osdDisplayPort); - resumeRefreshAt = 0; - statsDisplayed = false; - } else { - // Continue "Splash", "Armed" or "Stats" screens. - displayHeartbeat(osdDisplayPort); - } - - return; - } + // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. + if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { + // Time elapsed or canceled by stick commands. + // Exit to normal OSD operation. + displayClearScreen(osdDisplayPort); + resumeRefreshAt = 0; + statsDisplayed = false; + } else { + // Continue "Splash", "Armed" or "Stats" screens. + displayHeartbeat(osdDisplayPort); + } + + return; + } #ifdef USE_CMS - if (!displayIsGrabbed(osdDisplayPort)) { - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - if (fullRedraw) { - displayClearScreen(osdDisplayPort); - fullRedraw = false; - } - osdDrawNextElement(); - displayHeartbeat(osdDisplayPort); - displayCommitTransaction(osdDisplayPort); + if (!displayIsGrabbed(osdDisplayPort)) { + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + if (fullRedraw) { + displayClearScreen(osdDisplayPort); + fullRedraw = false; + } + osdDrawNextElement(); + displayHeartbeat(osdDisplayPort); + displayCommitTransaction(osdDisplayPort); #ifdef OSD_CALLS_CMS - } else { - cmsUpdate(currentTimeUs); + } else { + cmsUpdate(currentTimeUs); #endif - } + } #endif } @@ -4670,318 +4768,318 @@ static void osdRefresh(timeUs_t currentTimeUs) */ void osdUpdate(timeUs_t currentTimeUs) { - static uint32_t counter = 0; + static uint32_t counter = 0; - // don't touch buffers if DMA transaction is in progress - if (displayIsTransferInProgress(osdDisplayPort)) { - return; - } + // don't touch buffers if DMA transaction is in progress + if (displayIsTransferInProgress(osdDisplayPort)) { + return; + } - if (!osdDisplayIsReady) { - osdCompleteAsyncInitialization(); - return; - } + if (!osdDisplayIsReady) { + osdCompleteAsyncInitialization(); + return; + } #if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0 - // Check if the layout has changed. Higher numbered - // boxes take priority. - unsigned activeLayout; - if (layoutOverride >= 0) { - activeLayout = layoutOverride; - // Check for timed override, it will go into effect on - // the next OSD iteration - if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) { - layoutOverrideUntil = 0; - layoutOverride = -1; - } - } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) { - activeLayout = 0; - } else { + // Check if the layout has changed. Higher numbered + // boxes take priority. + unsigned activeLayout; + if (layoutOverride >= 0) { + activeLayout = layoutOverride; + // Check for timed override, it will go into effect on + // the next OSD iteration + if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) { + layoutOverrideUntil = 0; + layoutOverride = -1; + } + } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) { + activeLayout = 0; + } else { #if OSD_ALTERNATE_LAYOUT_COUNT > 2 - if (IS_RC_MODE_ACTIVE(BOXOSDALT3)) - activeLayout = 3; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT3)) + activeLayout = 3; + else #endif #if OSD_ALTERNATE_LAYOUT_COUNT > 1 - if (IS_RC_MODE_ACTIVE(BOXOSDALT2)) - activeLayout = 2; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT2)) + activeLayout = 2; + else #endif - if (IS_RC_MODE_ACTIVE(BOXOSDALT1)) - activeLayout = 1; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT1)) + activeLayout = 1; + else #ifdef USE_PROGRAMMING_FRAMEWORK - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT)) - activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); - else + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT)) + activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); + else #endif - activeLayout = 0; - } - if (currentLayout != activeLayout) { - currentLayout = activeLayout; - osdStartFullRedraw(); - } + activeLayout = 0; + } + if (currentLayout != activeLayout) { + currentLayout = activeLayout; + osdStartFullRedraw(); + } #endif #define DRAW_FREQ_DENOM 4 #define STATS_FREQ_DENOM 50 - counter++; + counter++; - if ((counter % STATS_FREQ_DENOM) == 0) { - osdUpdateStats(); - } + if ((counter % STATS_FREQ_DENOM) == 0) { + osdUpdateStats(); + } - if ((counter % DRAW_FREQ_DENOM) == 0) { - // redraw values in buffer - osdRefresh(currentTimeUs); - } else { - // rest of time redraw screen - displayDrawScreen(osdDisplayPort); - } + if ((counter % DRAW_FREQ_DENOM) == 0) { + // redraw values in buffer + osdRefresh(currentTimeUs); + } else { + // rest of time redraw screen + displayDrawScreen(osdDisplayPort); + } #ifdef USE_CMS - // do not allow ARM if we are in menu - if (displayIsGrabbed(osdDisplayPort)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); - } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); - } + // do not allow ARM if we are in menu + if (displayIsGrabbed(osdDisplayPort)) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); + } #endif } void osdStartFullRedraw(void) { - fullRedraw = true; + fullRedraw = true; } void osdOverrideLayout(int layout, timeMs_t duration) { - layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1); - if (layoutOverride >= 0 && duration > 0) { - layoutOverrideUntil = millis() + duration; - } else { - layoutOverrideUntil = 0; - } + layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1); + if (layoutOverride >= 0 && duration > 0) { + layoutOverrideUntil = millis() + duration; + } else { + layoutOverrideUntil = 0; + } } int osdGetActiveLayout(bool *overridden) { - if (overridden) { - *overridden = layoutOverride >= 0; - } - return currentLayout; + if (overridden) { + *overridden = layoutOverride >= 0; + } + return currentLayout; } bool osdItemIsFixed(osd_items_e item) { - return item == OSD_CROSSHAIRS || - item == OSD_ARTIFICIAL_HORIZON || - item == OSD_HORIZON_SIDEBARS; + return item == OSD_CROSSHAIRS || + item == OSD_ARTIFICIAL_HORIZON || + item == OSD_HORIZON_SIDEBARS; } displayPort_t *osdGetDisplayPort(void) { - return osdDisplayPort; + return osdDisplayPort; } displayCanvas_t *osdGetDisplayPortCanvas(void) { #if defined(USE_CANVAS) - if (osdDisplayHasCanvas) { - return &osdCanvas; - } + if (osdDisplayHasCanvas) { + return &osdCanvas; + } #endif - return NULL; + return NULL; } timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){ - uint8_t i = 0; - float factor = 1.0f; - while (i < messageCount) { - if ((float)strlen(messages[i]) / 15.0f > factor) { - factor = (float)strlen(messages[i]) / 15.0f; - } - i++; - } - return osdConfig()->system_msg_display_time * factor; + uint8_t i = 0; + float factor = 1.0f; + while (i < messageCount) { + if ((float)strlen(messages[i]) / 15.0f > factor) { + factor = (float)strlen(messages[i]) / 15.0f; + } + i++; + } + return osdConfig()->system_msg_display_time * factor; } textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText) { - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - if (buff != NULL) { - const char *message = NULL; - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - // We might have up to 5 messages to show. - const char *messages[5]; - unsigned messageCount = 0; - const char *failsafeInfoMessage = NULL; - const char *invertedInfoMessage = NULL; + if (buff != NULL) { + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + // We might have up to 5 messages to show. + const char *messages[5]; + unsigned messageCount = 0; + const char *failsafeInfoMessage = NULL; + const char *invertedInfoMessage = NULL; - if (ARMING_FLAG(ARMED)) { - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - 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); - } - 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) { - // Countdown display for remaining Waypoints - char buf[6]; - osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); - tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); - messages[messageCount++] = messageBuf; - } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { - if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); - } else { - // WP hold time countdown in seconds - timeMs_t currentTime = millis(); - int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); - holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; + if (ARMING_FLAG(ARMED)) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + 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); + } + 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) { + // Countdown display for remaining Waypoints + char buf[6]; + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); + messages[messageCount++] = messageBuf; + } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); + } else { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); + holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; - tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); - messages[messageCount++] = messageBuf; - } - } else { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } - } + messages[messageCount++] = messageBuf; + } + } else { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } + } #if defined(USE_SAFE_HOME) - const char *safehomeMessage = divertingToSafehomeMessage(); - if (safehomeMessage) { - messages[messageCount++] = safehomeMessage; - } + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } #endif - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too - const char *failsafePhaseMessage = osdFailsafePhaseMessage(); - failsafeInfoMessage = osdFailsafeInfoMessage(); + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + failsafeInfoMessage = osdFailsafeInfoMessage(); - if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; - } - if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; - } - } - } 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/ACRO - // when it doesn't require ANGLE mode (required only in FW - // right now). If if requires ANGLE, its display is handled - // by OSD_FLYMODE. - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); - if (FLIGHT_MODE(MANUAL_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); - } - } - if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) { - 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); - } - } - } - } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { - unsigned invalidIndex; + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + } + } 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/ACRO + // when it doesn't require ANGLE mode (required only in FW + // right now). If if requires ANGLE, its display is handled + // by OSD_FLYMODE. + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); + if (FLIGHT_MODE(MANUAL_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); + } + } + if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) { + 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); + } + } + } + } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + unsigned invalidIndex; - // Check if we're unable to arm for some reason - if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&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 { + } else { - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - messages[messageCount++] = invertedInfoMessage; + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); + messages[messageCount++] = invertedInfoMessage; - // Show the reason for not arming - messages[messageCount++] = osdArmingDisabledReasonMessage(); + // Show the reason for not arming + messages[messageCount++] = osdArmingDisabledReasonMessage(); - } - } else if (!ARMING_FLAG(ARMED)) { - if (isWaypointListValid()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); - } - } + } + } else if (!ARMING_FLAG(ARMED)) { + if (isWaypointListValid()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); + } + } - /* Messages that are shown regardless of Arming state */ + /* Messages that are shown regardless of Arming state */ - if (savingSettings == true) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); - } else if (notify_settings_saved > 0) { - if (millis() > notify_settings_saved) { - notify_settings_saved = 0; - } else { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED); - } - } + if (savingSettings == true) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); + } else if (notify_settings_saved > 0) { + if (millis() > notify_settings_saved) { + notify_settings_saved = 0; + } else { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED); + } + } #ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE); - } + if (systemConfig()->groundTestMode) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE); + } #endif - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; - if (message == failsafeInfoMessage) { - // failsafeInfoMessage is not useful for recovering - // a lost model, but might help avoiding a crash. - // Blink to grab user attention. - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (message == invertedInfoMessage) { - TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } - // We're shoing either failsafePhaseMessage or - // navStateMessage. Don't BLINK here since - // having this text available might be crucial - // during a lost aircraft recovery and blinking - // will cause it to be missing from some frames. - } + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; + if (message == failsafeInfoMessage) { + // failsafeInfoMessage is not useful for recovering + // a lost model, but might help avoiding a crash. + // Blink to grab user attention. + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == invertedInfoMessage) { + TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + // We're shoing either failsafePhaseMessage or + // navStateMessage. Don't BLINK here since + // having this text available might be crucial + // during a lost aircraft recovery and blinking + // will cause it to be missing from some frames. + } - osdFormatMessage(buff, buff_size, message, isCenteredText); - } - return elemAttr; + osdFormatMessage(buff, buff_size, message, isCenteredText); + } + return elemAttr; } -#endif // OSD +#endif // OSD \ No newline at end of file From 6e58a94cf3a9230096cffb21c6816b92eebcd47a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 27 Aug 2023 23:28:04 +0100 Subject: [PATCH 14/62] Change logic + add gyro check for MR --- src/main/fc/fc_core.c | 38 ++++++++++++++------------------ src/main/navigation/navigation.c | 20 ++++++++++++----- 2 files changed, 30 insertions(+), 28 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 60c658809e..a00e797242 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -119,8 +119,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; -timeMs_t emergInflightRearmTimeout = 0; -timeMs_t mcEmergRearmStabiliseTimeout = 0; +timeMs_t emergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -435,10 +434,6 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { - emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; - } - DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -508,24 +503,24 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { - /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ - + /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */ timeMs_t currentTimeMs = millis(); - if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + emergRearmStabiliseTimeout = 0; + + if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) || + (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) { return false; } - if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm - if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - return false; // MR doesn't appear to be flying so don't allow emergency rearm - } else { - mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR - } + if (isProbablyStillFlying()) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + return true; + } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + return true; } - return true; + return false; // craft doesn't appear to be flying, don't allow emergency rearm } void tryArm(void) @@ -564,7 +559,6 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; - emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -667,9 +661,9 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude - bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); - bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; + // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR) + bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; bool canUseHorizonMode = true; if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 759ddd42f3..eba38c68b1 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,6 +61,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" +#include "sensors/gyro.h" #include "programming/global_variables.h" @@ -2808,13 +2809,16 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - + if (STATE(LANDING_DETECTED)) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; + } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming + landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2854,10 +2858,14 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - // Multirotor flight sanity checked after disarm so always true here - bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + bool inFlightSanityCheck; + if (STATE(MULTIROTOR)) { + inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f; + } else { + inFlightSanityCheck = isGPSHeadingValid(); + } - return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; + return landingDetectorIsActive && inFlightSanityCheck; } /*----------------------------------------------------------- From 1338fb4a54fcf891a175c5285ca48bfe991b34dd Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 28 Aug 2023 12:47:11 +0100 Subject: [PATCH 15/62] Update fc_core.h --- src/main/fc/fc_core.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 1d34e31742..37d0bbda79 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,7 +43,6 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); -bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); From 2d2195d841b31ace59b94831595a9e22cd366f25 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 29 Aug 2023 11:41:39 +0100 Subject: [PATCH 16/62] fix landing detector active state logic --- src/main/fc/fc_core.c | 2 ++ src/main/navigation/navigation.c | 13 +++++++------ src/main/navigation/navigation.h | 1 + 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a00e797242..92705ab691 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -558,6 +558,8 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight + lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eba38c68b1..63c5b9884a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,16 +2809,12 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (STATE(LANDING_DETECTED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; - } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming - landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2851,6 +2847,11 @@ void resetLandingDetector(void) posControl.flags.resetLandingDetector = true; } +void resetLandingDetectorActiveState(void) +{ + landingDetectorIsActive = false; +} + bool isFlightDetected(void) { return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9ffe532e6c..3fd213a245 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -611,6 +611,7 @@ float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); bool isProbablyStillFlying(void); +void resetLandingDetectorActiveState(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); From 89d62544b7b62f0a0e8d2c3679703caea15ff622 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 30 Aug 2023 18:49:29 +0100 Subject: [PATCH 17/62] More updates - Added use pilot logo option to CLI - Converted osd.h to use tabs --- docs/Settings.md | 10 + src/main/fc/settings.yaml | 6 + src/main/io/osd.c | 1 + src/main/io/osd.h | 687 +++++++++++++++++++------------------- 4 files changed, 360 insertions(+), 344 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 048e8de9d2..bfbb1e7e12 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4722,6 +4722,16 @@ IMPERIAL, METRIC, UK --- +### osd_use_pilot_logo + +Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511 + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### osd_video_system Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT` diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b11169b6ac..c63972fefd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3480,6 +3480,12 @@ groups: max: 6 default_value: 4 + - name: osd_use_pilot_logo + description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511 + field: use_pilot_logo + type: bool + default_value: OFF + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f6289520bc..49cdee582d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3673,6 +3673,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .use_pilot_logo = SETTINGS_OSD_USE_PILOT_LOGO_DEFAULT #ifdef USE_WIND_ESTIMATOR .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2dad5ceb81..afe651a8d6 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -32,19 +32,19 @@ // 00vb yyyy yyxx xxxx // (visible)(blink)(yCoord)(xCoord) -#define OSD_VISIBLE_FLAG 0x2000 -#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) +#define OSD_VISIBLE_FLAG 0x2000 +#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) -#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) -#define OSD_X(x) ((x) & 0x3F) -#define OSD_Y(x) (((x) >> 6) & 0x3F) -#define OSD_POS_MAX 0xFFF +#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) +#define OSD_X(x) ((x) & 0x3F) +#define OSD_Y(x) (((x) >> 6) & 0x3F) +#define OSD_POS_MAX 0xFFF // For DJI compatibility #define OSD_VISIBLE_FLAG_SD 0x0800 -#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) +#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) -#define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) +#define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) #define OSD_HOMING_LIM_H1 6 #define OSD_HOMING_LIM_H2 16 @@ -54,286 +54,286 @@ #define OSD_HOMING_LIM_V3 15 // Message defines to be use in OSD and/or telemetry exports -#define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" -#define OSD_MSG_TURN_ARM_SW_OFF "TURN ARM SWITCH OFF" -#define OSD_MSG_DISABLED_BY_FS "DISABLED BY FAILSAFE" -#define OSD_MSG_AIRCRAFT_UNLEVEL "AIRCRAFT IS NOT LEVEL" -#define OSD_MSG_SENSORS_CAL "SENSORS CALIBRATING" -#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" -#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" -#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" -#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" -#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" -#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" -#define OSD_MSG_DISARM_1ST "DISABLE ARM SWITCH FIRST" -#define OSD_MSG_GYRO_FAILURE "GYRO FAILURE" -#define OSD_MSG_ACC_FAIL "ACCELEROMETER FAILURE" -#define OSD_MSG_MAG_FAIL "COMPASS FAILURE" -#define OSD_MSG_BARO_FAIL "BAROMETER FAILURE" -#define OSD_MSG_GPS_FAIL "GPS FAILURE" -#define OSD_MSG_RANGEFINDER_FAIL "RANGE FINDER FAILURE" -#define OSD_MSG_PITOT_FAIL "PITOT METER FAILURE" -#define OSD_MSG_HW_FAIL "HARDWARE FAILURE" -#define OSD_MSG_FS_EN "FAILSAFE MODE ENABLED" -#define OSD_MSG_KILL_SW_EN "KILLSWITCH MODE ENABLED" -#define OSD_MSG_NO_RC_LINK "NO RC LINK" -#define OSD_MSG_THROTTLE_NOT_LOW "THROTTLE IS NOT LOW" -#define OSD_MSG_ROLLPITCH_OFFCENTER "ROLLPITCH NOT CENTERED" -#define OSD_MSG_AUTOTRIM_ACTIVE "AUTOTRIM IS ACTIVE" -#define OSD_MSG_NOT_ENOUGH_MEMORY "NOT ENOUGH MEMORY" -#define OSD_MSG_INVALID_SETTING "INVALID SETTING" -#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" -#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" -#define OSD_MSG_NO_PREARM "NO PREARM" -#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" -#define OSD_MSG_RTH_FS "(RTH)" -#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" -#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" -#define OSD_MSG_STARTING_RTH "STARTING RTH" -#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" -#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" -#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" -#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" -#define OSD_MSG_WP_LANDED "WP END>LANDED" -#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" -#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" -#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" -#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" -#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" -#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" -#define OSD_MSG_LANDING "LANDING" -#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" -#define OSD_MSG_HOVERING "HOVERING" -#define OSD_MSG_LANDED "LANDED" -#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" -#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" -#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" -#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" -#define OSD_MSG_AUTOTRIM "(AUTOTRIM)" -#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" -#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" -#define OSD_MSG_AUTOLEVEL "(AUTO LEVEL TRIM)" -#define OSD_MSG_HEADFREE "(HEADFREE)" -#define OSD_MSG_NAV_SOARING "(SOARING)" -#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" -#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **" -#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **" +#define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" +#define OSD_MSG_TURN_ARM_SW_OFF "TURN ARM SWITCH OFF" +#define OSD_MSG_DISABLED_BY_FS "DISABLED BY FAILSAFE" +#define OSD_MSG_AIRCRAFT_UNLEVEL "AIRCRAFT IS NOT LEVEL" +#define OSD_MSG_SENSORS_CAL "SENSORS CALIBRATING" +#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" +#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" +#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" +#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" +#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" +#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" +#define OSD_MSG_DISARM_1ST "DISABLE ARM SWITCH FIRST" +#define OSD_MSG_GYRO_FAILURE "GYRO FAILURE" +#define OSD_MSG_ACC_FAIL "ACCELEROMETER FAILURE" +#define OSD_MSG_MAG_FAIL "COMPASS FAILURE" +#define OSD_MSG_BARO_FAIL "BAROMETER FAILURE" +#define OSD_MSG_GPS_FAIL "GPS FAILURE" +#define OSD_MSG_RANGEFINDER_FAIL "RANGE FINDER FAILURE" +#define OSD_MSG_PITOT_FAIL "PITOT METER FAILURE" +#define OSD_MSG_HW_FAIL "HARDWARE FAILURE" +#define OSD_MSG_FS_EN "FAILSAFE MODE ENABLED" +#define OSD_MSG_KILL_SW_EN "KILLSWITCH MODE ENABLED" +#define OSD_MSG_NO_RC_LINK "NO RC LINK" +#define OSD_MSG_THROTTLE_NOT_LOW "THROTTLE IS NOT LOW" +#define OSD_MSG_ROLLPITCH_OFFCENTER "ROLLPITCH NOT CENTERED" +#define OSD_MSG_AUTOTRIM_ACTIVE "AUTOTRIM IS ACTIVE" +#define OSD_MSG_NOT_ENOUGH_MEMORY "NOT ENOUGH MEMORY" +#define OSD_MSG_INVALID_SETTING "INVALID SETTING" +#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" +#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" +#define OSD_MSG_NO_PREARM "NO PREARM" +#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" +#define OSD_MSG_RTH_FS "(RTH)" +#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" +#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" +#define OSD_MSG_STARTING_RTH "STARTING RTH" +#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" +#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" +#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" +#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" +#define OSD_MSG_WP_LANDED "WP END>LANDED" +#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" +#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" +#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" +#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" +#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" +#define OSD_MSG_LANDING "LANDING" +#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" +#define OSD_MSG_HOVERING "HOVERING" +#define OSD_MSG_LANDED "LANDED" +#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" +#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" +#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" +#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" +#define OSD_MSG_AUTOTRIM "(AUTOTRIM)" +#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" +#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" +#define OSD_MSG_AUTOLEVEL "(AUTO LEVEL TRIM)" +#define OSD_MSG_HEADFREE "(HEADFREE)" +#define OSD_MSG_NAV_SOARING "(SOARING)" +#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **" +#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **" #ifdef USE_DEV_TOOLS -#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" +#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" #endif #if defined(USE_SAFE_HOME) -#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" -#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" +#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif typedef enum { - OSD_RSSI_VALUE, - OSD_MAIN_BATT_VOLTAGE, - OSD_CROSSHAIRS, - OSD_ARTIFICIAL_HORIZON, - OSD_HORIZON_SIDEBARS, - OSD_ONTIME, - OSD_FLYTIME, - OSD_FLYMODE, - OSD_CRAFT_NAME, - OSD_THROTTLE_POS, - OSD_VTX_CHANNEL, - OSD_CURRENT_DRAW, - OSD_MAH_DRAWN, - OSD_GPS_SPEED, - OSD_GPS_SATS, - OSD_ALTITUDE, - OSD_ROLL_PIDS, - OSD_PITCH_PIDS, - OSD_YAW_PIDS, - OSD_POWER, - OSD_GPS_LON, - OSD_GPS_LAT, - OSD_HOME_DIR, - OSD_HOME_DIST, - OSD_HEADING, - OSD_VARIO, - OSD_VARIO_NUM, - OSD_AIR_SPEED, - OSD_ONTIME_FLYTIME, - OSD_RTC_TIME, - OSD_MESSAGES, - OSD_GPS_HDOP, - OSD_MAIN_BATT_CELL_VOLTAGE, - OSD_SCALED_THROTTLE_POS, - OSD_HEADING_GRAPH, - OSD_EFFICIENCY_MAH_PER_KM, - OSD_WH_DRAWN, - OSD_BATTERY_REMAINING_CAPACITY, - OSD_BATTERY_REMAINING_PERCENT, - OSD_EFFICIENCY_WH_PER_KM, - OSD_TRIP_DIST, - OSD_ATTITUDE_PITCH, - OSD_ATTITUDE_ROLL, - OSD_MAP_NORTH, - OSD_MAP_TAKEOFF, - OSD_RADAR, - OSD_WIND_SPEED_HORIZONTAL, - OSD_WIND_SPEED_VERTICAL, - OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, - OSD_REMAINING_DISTANCE_BEFORE_RTH, - OSD_HOME_HEADING_ERROR, - OSD_COURSE_HOLD_ERROR, - OSD_COURSE_HOLD_ADJUSTMENT, - OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, - OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, - OSD_POWER_SUPPLY_IMPEDANCE, - OSD_LEVEL_PIDS, - OSD_POS_XY_PIDS, - OSD_POS_Z_PIDS, - OSD_VEL_XY_PIDS, - OSD_VEL_Z_PIDS, - OSD_HEADING_P, - OSD_BOARD_ALIGN_ROLL, - OSD_BOARD_ALIGN_PITCH, - OSD_RC_EXPO, - OSD_RC_YAW_EXPO, - OSD_THROTTLE_EXPO, - OSD_PITCH_RATE, - OSD_ROLL_RATE, - OSD_YAW_RATE, - OSD_MANUAL_RC_EXPO, - OSD_MANUAL_RC_YAW_EXPO, - OSD_MANUAL_PITCH_RATE, - OSD_MANUAL_ROLL_RATE, - OSD_MANUAL_YAW_RATE, - OSD_NAV_FW_CRUISE_THR, - OSD_NAV_FW_PITCH2THR, - OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, - OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI. - OSD_FW_ALT_PID_OUTPUTS, - OSD_FW_POS_PID_OUTPUTS, - OSD_MC_VEL_X_PID_OUTPUTS, - OSD_MC_VEL_Y_PID_OUTPUTS, - OSD_MC_VEL_Z_PID_OUTPUTS, - OSD_MC_POS_XYZ_P_OUTPUTS, - OSD_3D_SPEED, - OSD_IMU_TEMPERATURE, - OSD_BARO_TEMPERATURE, - OSD_TEMP_SENSOR_0_TEMPERATURE, - OSD_TEMP_SENSOR_1_TEMPERATURE, - OSD_TEMP_SENSOR_2_TEMPERATURE, - OSD_TEMP_SENSOR_3_TEMPERATURE, - OSD_TEMP_SENSOR_4_TEMPERATURE, - OSD_TEMP_SENSOR_5_TEMPERATURE, - OSD_TEMP_SENSOR_6_TEMPERATURE, - OSD_TEMP_SENSOR_7_TEMPERATURE, - OSD_ALTITUDE_MSL, - OSD_PLUS_CODE, - OSD_MAP_SCALE, - OSD_MAP_REFERENCE, - OSD_GFORCE, - OSD_GFORCE_X, - OSD_GFORCE_Y, - OSD_GFORCE_Z, - OSD_RC_SOURCE, - OSD_VTX_POWER, - OSD_ESC_RPM, - OSD_ESC_TEMPERATURE, - OSD_AZIMUTH, - OSD_CRSF_RSSI_DBM, - OSD_CRSF_LQ, - OSD_CRSF_SNR_DB, - OSD_CRSF_TX_POWER, - OSD_GVAR_0, - OSD_GVAR_1, - OSD_GVAR_2, - OSD_GVAR_3, - OSD_TPA, - OSD_NAV_FW_CONTROL_SMOOTHNESS, - OSD_VERSION, - OSD_RANGEFINDER, - OSD_PLIMIT_REMAINING_BURST_TIME, - OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, - OSD_PLIMIT_ACTIVE_POWER_LIMIT, - OSD_GLIDESLOPE, - OSD_GPS_MAX_SPEED, - OSD_3D_MAX_SPEED, - OSD_AIR_MAX_SPEED, - OSD_ACTIVE_PROFILE, - OSD_MISSION, - OSD_SWITCH_INDICATOR_0, - OSD_SWITCH_INDICATOR_1, - OSD_SWITCH_INDICATOR_2, - OSD_SWITCH_INDICATOR_3, - OSD_TPA_TIME_CONSTANT, - OSD_FW_LEVEL_TRIM, - OSD_GLIDE_TIME_REMAINING, - OSD_GLIDE_RANGE, - OSD_CLIMB_EFFICIENCY, - OSD_NAV_WP_MULTI_MISSION_INDEX, - OSD_GROUND_COURSE, // 140 - OSD_CROSS_TRACK_ERROR, - OSD_PILOT_NAME, - OSD_PAN_SERVO_CENTRED, - OSD_ITEM_COUNT // MUST BE LAST + OSD_RSSI_VALUE, + OSD_MAIN_BATT_VOLTAGE, + OSD_CROSSHAIRS, + OSD_ARTIFICIAL_HORIZON, + OSD_HORIZON_SIDEBARS, + OSD_ONTIME, + OSD_FLYTIME, + OSD_FLYMODE, + OSD_CRAFT_NAME, + OSD_THROTTLE_POS, + OSD_VTX_CHANNEL, + OSD_CURRENT_DRAW, + OSD_MAH_DRAWN, + OSD_GPS_SPEED, + OSD_GPS_SATS, + OSD_ALTITUDE, + OSD_ROLL_PIDS, + OSD_PITCH_PIDS, + OSD_YAW_PIDS, + OSD_POWER, + OSD_GPS_LON, + OSD_GPS_LAT, + OSD_HOME_DIR, + OSD_HOME_DIST, + OSD_HEADING, + OSD_VARIO, + OSD_VARIO_NUM, + OSD_AIR_SPEED, + OSD_ONTIME_FLYTIME, + OSD_RTC_TIME, + OSD_MESSAGES, + OSD_GPS_HDOP, + OSD_MAIN_BATT_CELL_VOLTAGE, + OSD_SCALED_THROTTLE_POS, + OSD_HEADING_GRAPH, + OSD_EFFICIENCY_MAH_PER_KM, + OSD_WH_DRAWN, + OSD_BATTERY_REMAINING_CAPACITY, + OSD_BATTERY_REMAINING_PERCENT, + OSD_EFFICIENCY_WH_PER_KM, + OSD_TRIP_DIST, + OSD_ATTITUDE_PITCH, + OSD_ATTITUDE_ROLL, + OSD_MAP_NORTH, + OSD_MAP_TAKEOFF, + OSD_RADAR, + OSD_WIND_SPEED_HORIZONTAL, + OSD_WIND_SPEED_VERTICAL, + OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, + OSD_REMAINING_DISTANCE_BEFORE_RTH, + OSD_HOME_HEADING_ERROR, + OSD_COURSE_HOLD_ERROR, + OSD_COURSE_HOLD_ADJUSTMENT, + OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, + OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, + OSD_POWER_SUPPLY_IMPEDANCE, + OSD_LEVEL_PIDS, + OSD_POS_XY_PIDS, + OSD_POS_Z_PIDS, + OSD_VEL_XY_PIDS, + OSD_VEL_Z_PIDS, + OSD_HEADING_P, + OSD_BOARD_ALIGN_ROLL, + OSD_BOARD_ALIGN_PITCH, + OSD_RC_EXPO, + OSD_RC_YAW_EXPO, + OSD_THROTTLE_EXPO, + OSD_PITCH_RATE, + OSD_ROLL_RATE, + OSD_YAW_RATE, + OSD_MANUAL_RC_EXPO, + OSD_MANUAL_RC_YAW_EXPO, + OSD_MANUAL_PITCH_RATE, + OSD_MANUAL_ROLL_RATE, + OSD_MANUAL_YAW_RATE, + OSD_NAV_FW_CRUISE_THR, + OSD_NAV_FW_PITCH2THR, + OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, + OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI. + OSD_FW_ALT_PID_OUTPUTS, + OSD_FW_POS_PID_OUTPUTS, + OSD_MC_VEL_X_PID_OUTPUTS, + OSD_MC_VEL_Y_PID_OUTPUTS, + OSD_MC_VEL_Z_PID_OUTPUTS, + OSD_MC_POS_XYZ_P_OUTPUTS, + OSD_3D_SPEED, + OSD_IMU_TEMPERATURE, + OSD_BARO_TEMPERATURE, + OSD_TEMP_SENSOR_0_TEMPERATURE, + OSD_TEMP_SENSOR_1_TEMPERATURE, + OSD_TEMP_SENSOR_2_TEMPERATURE, + OSD_TEMP_SENSOR_3_TEMPERATURE, + OSD_TEMP_SENSOR_4_TEMPERATURE, + OSD_TEMP_SENSOR_5_TEMPERATURE, + OSD_TEMP_SENSOR_6_TEMPERATURE, + OSD_TEMP_SENSOR_7_TEMPERATURE, + OSD_ALTITUDE_MSL, + OSD_PLUS_CODE, + OSD_MAP_SCALE, + OSD_MAP_REFERENCE, + OSD_GFORCE, + OSD_GFORCE_X, + OSD_GFORCE_Y, + OSD_GFORCE_Z, + OSD_RC_SOURCE, + OSD_VTX_POWER, + OSD_ESC_RPM, + OSD_ESC_TEMPERATURE, + OSD_AZIMUTH, + OSD_CRSF_RSSI_DBM, + OSD_CRSF_LQ, + OSD_CRSF_SNR_DB, + OSD_CRSF_TX_POWER, + OSD_GVAR_0, + OSD_GVAR_1, + OSD_GVAR_2, + OSD_GVAR_3, + OSD_TPA, + OSD_NAV_FW_CONTROL_SMOOTHNESS, + OSD_VERSION, + OSD_RANGEFINDER, + OSD_PLIMIT_REMAINING_BURST_TIME, + OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, + OSD_PLIMIT_ACTIVE_POWER_LIMIT, + OSD_GLIDESLOPE, + OSD_GPS_MAX_SPEED, + OSD_3D_MAX_SPEED, + OSD_AIR_MAX_SPEED, + OSD_ACTIVE_PROFILE, + OSD_MISSION, + OSD_SWITCH_INDICATOR_0, + OSD_SWITCH_INDICATOR_1, + OSD_SWITCH_INDICATOR_2, + OSD_SWITCH_INDICATOR_3, + OSD_TPA_TIME_CONSTANT, + OSD_FW_LEVEL_TRIM, + OSD_GLIDE_TIME_REMAINING, + OSD_GLIDE_RANGE, + OSD_CLIMB_EFFICIENCY, + OSD_NAV_WP_MULTI_MISSION_INDEX, + OSD_GROUND_COURSE, // 140 + OSD_CROSS_TRACK_ERROR, + OSD_PILOT_NAME, + OSD_PAN_SERVO_CENTRED, + OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; typedef enum { - OSD_UNIT_IMPERIAL, - OSD_UNIT_METRIC, - OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph - OSD_UNIT_UK, // Show everything in imperial, temperature in C - OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C + OSD_UNIT_IMPERIAL, + OSD_UNIT_METRIC, + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C + OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C - OSD_UNIT_MAX = OSD_UNIT_GA, + OSD_UNIT_MAX = OSD_UNIT_GA, } osd_unit_e; typedef enum { - OSD_STATS_ENERGY_UNIT_MAH, - OSD_STATS_ENERGY_UNIT_WH, + OSD_STATS_ENERGY_UNIT_MAH, + OSD_STATS_ENERGY_UNIT_WH, } osd_stats_energy_unit_e; typedef enum { - OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, - OSD_STATS_MIN_VOLTAGE_UNIT_CELL, + OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, + OSD_STATS_MIN_VOLTAGE_UNIT_CELL, } osd_stats_min_voltage_unit_e; typedef enum { - OSD_CROSSHAIRS_STYLE_DEFAULT, - OSD_CROSSHAIRS_STYLE_AIRCRAFT, - OSD_CROSSHAIRS_STYLE_TYPE3, - OSD_CROSSHAIRS_STYLE_TYPE4, - OSD_CROSSHAIRS_STYLE_TYPE5, - OSD_CROSSHAIRS_STYLE_TYPE6, - OSD_CROSSHAIRS_STYLE_TYPE7, + OSD_CROSSHAIRS_STYLE_DEFAULT, + OSD_CROSSHAIRS_STYLE_AIRCRAFT, + OSD_CROSSHAIRS_STYLE_TYPE3, + OSD_CROSSHAIRS_STYLE_TYPE4, + OSD_CROSSHAIRS_STYLE_TYPE5, + OSD_CROSSHAIRS_STYLE_TYPE6, + OSD_CROSSHAIRS_STYLE_TYPE7, } osd_crosshairs_style_e; typedef enum { - OSD_SIDEBAR_SCROLL_NONE, - OSD_SIDEBAR_SCROLL_ALTITUDE, - OSD_SIDEBAR_SCROLL_SPEED, - OSD_SIDEBAR_SCROLL_HOME_DISTANCE, + OSD_SIDEBAR_SCROLL_NONE, + OSD_SIDEBAR_SCROLL_ALTITUDE, + OSD_SIDEBAR_SCROLL_SPEED, + OSD_SIDEBAR_SCROLL_HOME_DISTANCE, - OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE, + OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE, } osd_sidebar_scroll_e; typedef enum { - OSD_ALIGN_LEFT, - OSD_ALIGN_RIGHT + OSD_ALIGN_LEFT, + OSD_ALIGN_RIGHT } osd_alignment_e; typedef enum { - OSD_AHI_STYLE_DEFAULT, - OSD_AHI_STYLE_LINE, + OSD_AHI_STYLE_DEFAULT, + OSD_AHI_STYLE_LINE, } osd_ahi_style_e; typedef enum { - OSD_CRSF_LQ_TYPE1, - OSD_CRSF_LQ_TYPE2, - OSD_CRSF_LQ_TYPE3 + OSD_CRSF_LQ_TYPE1, + OSD_CRSF_LQ_TYPE2, + OSD_CRSF_LQ_TYPE3 } osd_crsf_lq_format_e; typedef struct osdLayoutsConfig_s { - // Layouts - uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; + // Layouts + uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; } osdLayoutsConfig_t; PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); @@ -341,112 +341,111 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); #define OSD_SWITCH_INDICATOR_NAME_LENGTH 4 typedef struct osdConfig_s { - // Alarms - uint8_t rssi_alarm; // rssi % - uint16_t time_alarm; // fly minutes - uint16_t alt_alarm; // positive altitude in m - uint16_t dist_alarm; // home distance in m - uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t current_alarm; // current consumption in A - int16_t imu_temp_alarm_min; - int16_t imu_temp_alarm_max; - int16_t esc_temp_alarm_min; - int16_t esc_temp_alarm_max; - float gforce_alarm; - float gforce_axis_alarm_min; - float gforce_axis_alarm_max; + // Alarms + uint8_t rssi_alarm; // rssi % + uint16_t time_alarm; // fly minutes + uint16_t alt_alarm; // positive altitude in m + uint16_t dist_alarm; // home distance in m + uint16_t neg_alt_alarm; // abs(negative altitude) in m + uint8_t current_alarm; // current consumption in A + int16_t imu_temp_alarm_min; + int16_t imu_temp_alarm_max; + int16_t esc_temp_alarm_min; + int16_t esc_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int8_t snr_alarm; //CRSF SNR alarm in dB - int8_t link_quality_alarm; - int16_t rssi_dbm_alarm; // in dBm - int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% - int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% + int8_t snr_alarm; //CRSF SNR alarm in dB + int8_t link_quality_alarm; + int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO - int16_t baro_temp_alarm_min; - int16_t baro_temp_alarm_max; + int16_t baro_temp_alarm_min; + int16_t baro_temp_alarm_max; #endif #ifdef USE_TEMPERATURE_SENSOR - osd_alignment_e temp_label_align; + osd_alignment_e temp_label_align; #endif #ifdef USE_PITOT - float airspeed_alarm_min; - float airspeed_alarm_max; + float airspeed_alarm_min; + float airspeed_alarm_max; #endif - videoSystem_e video_system; - uint8_t row_shiftdown; - int16_t msp_displayport_fullframe_interval; + videoSystem_e video_system; + uint8_t row_shiftdown; + int16_t msp_displayport_fullframe_interval; - // Preferences - uint8_t main_voltage_decimals; - uint8_t ahi_reverse_roll; - uint8_t ahi_max_pitch; - uint8_t crosshairs_style; // from osd_crosshairs_style_e - int8_t horizon_offset; - int8_t camera_uptilt; - bool ahi_camera_uptilt_comp; - uint8_t camera_fov_h; - uint8_t camera_fov_v; - uint8_t hud_margin_h; - uint8_t hud_margin_v; - bool hud_homing; - bool hud_homepoint; - uint8_t hud_radar_disp; - uint16_t hud_radar_range_min; - uint16_t hud_radar_range_max; - uint8_t hud_radar_alt_difference_display_time; - uint8_t hud_radar_distance_display_time; - uint8_t hud_wp_disp; + // Preferences + uint8_t main_voltage_decimals; + uint8_t ahi_reverse_roll; + uint8_t ahi_max_pitch; + uint8_t crosshairs_style; // from osd_crosshairs_style_e + int8_t horizon_offset; + int8_t camera_uptilt; + bool ahi_camera_uptilt_comp; + uint8_t camera_fov_h; + uint8_t camera_fov_v; + uint8_t hud_margin_h; + uint8_t hud_margin_v; + bool hud_homing; + bool hud_homepoint; + uint8_t hud_radar_disp; + uint16_t hud_radar_range_min; + uint16_t hud_radar_range_max; + uint8_t hud_radar_alt_difference_display_time; + uint8_t hud_radar_distance_display_time; + uint8_t hud_wp_disp; - uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t sidebar_scroll_arrows; + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t sidebar_scroll_arrows; - uint8_t units; // from osd_unit_e - uint8_t stats_energy_unit; // from osd_stats_energy_unit_e - uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e - uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) + uint8_t units; // from osd_unit_e + uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) -#ifdef USE_WIND_ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance -#endif - - uint8_t coordinate_digits; - - bool osd_failsafe_switch_layout; - uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; - uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. - bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm - uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred - bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo - uint8_t crsf_lq_format; - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh - uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) - char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. - char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. - char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. - char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. +#ifdef USE_WIND_ ESTIMATOR + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance +#endif + uint8_t coordinate_digits; + bool osd_failsafe_switch_layout; + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_short; + uint8_t ahi_style; + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred + bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo + uint8_t crsf_lq_format; + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with/instead of the INAV logo } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From f779303a7a2554a873ca72e1b515e72653ec7100 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 31 Aug 2023 13:53:28 +0100 Subject: [PATCH 18/62] Added small pilot logo - Added small pilot logo initial code - Fixed error from tabs in osd.h --- src/main/io/osd.c | 9 ++++++++- src/main/io/osd.h | 5 +++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 49cdee582d..d24cc63d5b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2198,6 +2198,12 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatPilotName(buff); break; + case OSD_PILOT_LOGO: + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_PILOT_LOGO_SML_L); + displayWriteChar(osdDisplayPort, elemPosX+1, elemPosY, SYM_PILOT_LOGO_SML_C); + displayWriteChar(osdDisplayPort, elemPosX+2, elemPosY, SYM_PILOT_LOGO_SML_R); + break; + case OSD_THROTTLE_POS: { osdFormatThrottlePosition(buff, false, &elemAttr); @@ -3673,7 +3679,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, - .use_pilot_logo = SETTINGS_OSD_USE_PILOT_LOGO_DEFAULT + .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, #ifdef USE_WIND_ESTIMATOR .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, @@ -3746,6 +3752,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); + osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF diff --git a/src/main/io/osd.h b/src/main/io/osd.h index afe651a8d6..d6d0491a83 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -273,6 +273,7 @@ typedef enum { OSD_CROSS_TRACK_ERROR, OSD_PILOT_NAME, OSD_PAN_SERVO_CENTRED, + OSD_PILOT_LOGO, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -408,8 +409,8 @@ typedef struct osdConfig_s { uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) -#ifdef USE_WIND_ ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance +#ifdef USE_WIND_ESTIMATOR + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance #endif uint8_t coordinate_digits; bool osd_failsafe_switch_layout; From e0d8917fc782d292c3f3f9df46544a8c87029c23 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 3 Sep 2023 10:17:31 +0100 Subject: [PATCH 19/62] Corrected indexes for actual position in the font --- src/main/drivers/osd_symbols.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index a1b84d52b1..ed68b5f01b 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -258,10 +258,10 @@ #define SYM_SERVO_PAN_IS_OFFSET_L 0x1C7 // 455 Pan servo is offset left #define SYM_SERVO_PAN_IS_OFFSET_R 0x1C8 // 456 Pan servo is offset right -#define SYM_PILOT_LOGO_SML_L 0x1D6 // 470 small Pilot logo Left -#define SYM_PILOT_LOGO_SML_C 0x1D7 // 471 small Pilot logo Centre -#define SYM_PILOT_LOGO_SML_R 0x1D8 // 472 small Pilot logo Right -#define SYM_PILOT_LOGO_LRG_START 0x1D9 // 473 to 511, Pilot logo +#define SYM_PILOT_LOGO_SML_L 0x1D5 // 469 small Pilot logo Left +#define SYM_PILOT_LOGO_SML_C 0x1D6 // 470 small Pilot logo Centre +#define SYM_PILOT_LOGO_SML_R 0x1D7 // 471 small Pilot logo Right +#define SYM_PILOT_LOGO_LRG_START 0x1D8 // 472 to 511, Pilot logo #else From ff6b8a76e8bbeaf91b798bb820a9d3d91cdc8b69 Mon Sep 17 00:00:00 2001 From: Andrey Akimov Date: Mon, 4 Sep 2023 10:30:16 +0300 Subject: [PATCH 20/62] Timer N channel fix --- src/main/drivers/timer_impl_stdperiph.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 5cb3457c77..620d6f6993 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -301,7 +301,12 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + } + TIM_Cmd(timer, ENABLE); dmaInit(tch->dma, OWNER_TIMER, 0); @@ -382,7 +387,12 @@ bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, vo TIM_CtrlPWMOutputs(timer, ENABLE); TIM_ARRPreloadConfig(timer, ENABLE); - TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable); + } else { + TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable); + } + TIM_Cmd(timer, ENABLE); if (!tch->timCtx->dmaBurstRef) { From 896478f9ebd0641eb4e52e148875ef205cc0070d Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 4 Sep 2023 22:46:19 +0100 Subject: [PATCH 21/62] Updates - Added arm screen timeout as CLI settable option. Defaults to current 1.5 seconds. - Fixed boot screen stats. Bug would have existed since 3.0 - Initial code for swapping logos on the boot screen - Replaced pseudo code with actual logic for using pilot logo on HD arming screen - Used already existing logic for detecting if display is HD --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 16 ++++++++++++++++ src/main/io/osd.c | 26 ++++++++++++-------------- src/main/io/osd.h | 5 +++-- 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index bfbb1e7e12..1fd236b8d7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3972,6 +3972,16 @@ Value above which to make the OSD relative altitude indicator blink (meters) --- +### osd_arm_screen_display_time + +Amount of time to display the arm screen [ms] + +| Default | Min | Max | +| --- | --- | --- | +| 1500 | 1000 | 5000 | + +--- + ### osd_baro_temp_alarm_max Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c63972fefd..cb2c5b75e4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3486,59 +3486,75 @@ groups: type: bool default_value: OFF + - name: osd_arm_screen_display_time + description: Amount of time to display the arm screen [ms] + field: arm_screen_display_time + min: 1000 + max: 5000 + default_value: 1500 + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name type: string max: 5 default_value: "FLAP" + - name: osd_switch_indicator_one_name description: "Character to use for OSD switch incicator 1." field: osd_switch_indicator1_name type: string max: 5 default_value: "GEAR" + - name: osd_switch_indicator_two_name description: "Character to use for OSD switch incicator 2." field: osd_switch_indicator2_name type: string max: 5 default_value: "CAM" + - name: osd_switch_indicator_three_name description: "Character to use for OSD switch incicator 3." field: osd_switch_indicator3_name type: string max: 5 default_value: "LIGT" + - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." field: osd_switch_indicator0_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_one_channel description: "RC Channel to use for OSD switch indicator 1." field: osd_switch_indicator1_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_two_channel description: "RC Channel to use for OSD switch indicator 2." field: osd_switch_indicator2_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_three_channel description: "RC Channel to use for OSD switch indicator 3." field: osd_switch_indicator3_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicators_align_left description: "Align text to left of switch indicators" field: osd_switch_indicators_align_left type: bool default_value: ON + - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d24cc63d5b..8b7a57260f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -129,7 +129,6 @@ #define STATS_PAGE1 (checkStickPosition(ROL_LO)) #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms -#define ARMED_SCREEN_DISPLAY_TIME 1500 // ms #define STATS_SCREEN_DISPLAY_TIME 60000 // ms #define EFFICIENCY_UPDATE_INTERVAL (5 * 1000) @@ -3680,6 +3679,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, + .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, #ifdef USE_WIND_ESTIMATOR .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, @@ -3909,7 +3909,7 @@ static void osdCompleteAsyncInitialization(void) if (fontHasMetadata && metadata.charCount > 256) { hasExtendedFont = true; - unsigned logo_c = SYM_LOGO_START; + unsigned logo_c = (osdConfig()->use_pilot_logo) ? SYM_PILOT_LOGO_LRG_START : SYM_LOGO_START; unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { @@ -3920,8 +3920,7 @@ static void osdCompleteAsyncInitialization(void) y++; } else if (!fontHasMetadata) { const char *m = "INVALID FONT"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m); - y = 4; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); } if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { @@ -3934,13 +3933,12 @@ static void osdCompleteAsyncInitialization(void) uint8_t xPos = osdDisplayIsHD() ? 15 : 5; displayWrite(osdDisplayPort, xPos, y++, string_buffer); #ifdef USE_CMS - displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); + displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); #endif + #ifdef USE_STATS - - uint8_t statNameX = osdDisplayIsHD() ? 14 : 4; uint8_t statValueX = osdDisplayIsHD() ? 34 : 24; @@ -3970,15 +3968,15 @@ static void osdCompleteAsyncInitialization(void) displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:"); uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60)); + tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); #ifdef USE_ADC if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:"); osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); - strcat(string_buffer, "\xAB"); // SYM_WH displayWrite(osdDisplayPort, statValueX-4, y, string_buffer); + displayWriteChar(osdDisplayPort, statValueX, y, SYM_WH); displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:"); if (statsConfig()->stats_total_dist) { @@ -4014,7 +4012,7 @@ static void osdCompleteAsyncInitialization(void) displayCommitTransaction(osdDisplayPort); displayResync(osdDisplayPort); - osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); + osdSetNextRefreshIn(10000);//SPLASH_SCREEN_DISPLAY_TIME); // TEMPORARY - PUT BACK ONCE TESTING IS FINISHED! } void osdInit(displayPort_t *osdDisplayPortToUse) @@ -4390,7 +4388,7 @@ static void osdShowHDArmScreen(void) logoRow++; } - // if (using pilot logo) { + if (osdConfig()->use_pilot_logo) { logo_c = SYM_PILOT_LOGO_LRG_START; logo_x = (logoColOffset * 2) + ((osdDisplayPort->cols % 2 == 0) ? 0 : 1); // Add extra 1 px space between logos, if the OSD has an odd number of columns logoRow = armScreenRow; @@ -4400,7 +4398,7 @@ static void osdShowHDArmScreen(void) } logoRow++; } - // } // using pilot logo + } armScreenRow = logoRow + 2; @@ -4556,7 +4554,7 @@ static void osdShowArmed(void) { displayClearScreen(osdDisplayPort); - if (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS) { + if (osdDisplayIsHD()) { osdShowHDArmScreen(); } else { osdShowSDArmScreen(); @@ -4661,7 +4659,7 @@ static void osdRefresh(timeUs_t currentTimeUs) statsDisplayed = false; osdResetStats(); osdShowArmed(); - uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + uint32_t delay = osdConfig()->arm_screen_display_time; #if defined(USE_SAFE_HOME) if (safehome_distance) delay *= 3; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d6d0491a83..08b74cfe23 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -273,7 +273,7 @@ typedef enum { OSD_CROSS_TRACK_ERROR, OSD_PILOT_NAME, OSD_PAN_SERVO_CENTRED, - OSD_PILOT_LOGO, + OSD_PILOT_LOGO, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -446,7 +446,8 @@ typedef struct osdConfig_s { char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. - bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with/instead of the INAV logo + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with/instead of the INAV logo + uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From 46b1ece7b6b36a18c20be7ee9186496f0f4a0846 Mon Sep 17 00:00:00 2001 From: Andrey Akimov Date: Tue, 5 Sep 2023 12:35:53 +0300 Subject: [PATCH 22/62] use dma burst for MATEKF405TE --- src/main/target/MATEKF405TE/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index 93ab3345de..b00b77299e 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -178,5 +178,5 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT +#define USE_DSHOT_DMAR #define USE_ESC_SENSOR - From d0d13af8210f7edfdfa16d241ce56bfc99e8bb86 Mon Sep 17 00:00:00 2001 From: Andrey Akimov Date: Tue, 5 Sep 2023 13:50:16 +0300 Subject: [PATCH 23/62] Revert "use dma burst for MATEKF405TE" This reverts commit 46b1ece7b6b36a18c20be7ee9186496f0f4a0846. --- src/main/target/MATEKF405TE/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index b00b77299e..93ab3345de 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -178,5 +178,5 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT -#define USE_DSHOT_DMAR #define USE_ESC_SENSOR + From fa60cfda81a4d50591bea9d74a85378f75844e81 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 6 Sep 2023 23:25:13 +0100 Subject: [PATCH 24/62] fixes --- src/main/fc/fc_core.c | 10 +++++----- src/main/navigation/navigation.c | 3 +++ 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 92705ab691..19015989ca 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -512,11 +512,11 @@ bool emergInflightRearmEnabled(void) return false; } - if (isProbablyStillFlying()) { - emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft - return true; - } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { - // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f; + + if (isProbablyStillFlying() || mcDisarmVertVelCheck) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft return true; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 63c5b9884a..5b3f17bb2a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,6 +2809,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { + if (STATE(LANDING_DETECTED)) { + landingDetectorIsActive = false; + } resetLandingDetector(); if (!IS_RC_MODE_ACTIVE(BOXARM)) { From c26a583fa0e08ef6ea8d0609e1a782b700dbadb4 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 12 Sep 2023 22:55:52 +0100 Subject: [PATCH 25/62] Update Unfortunately the large pilot logo can't be available on SD. There seems to be an issue with the storage on the MAX7456 chip. The last 33 characters of page 2 seem to corrupt on power loss. After uploading the font, all looks fine. After a reboot, the logo looks fine. But after a power cycle, the last 33 characters are corrupted. Changes: - cms.h now back to tabs - moved logo and stats drawing to own functions, to save repetition. - Made large pilot logo HD only - Disabled large pilot logo support with BFCOMPAT mode. - Stats and logos will self centre, no matter the column width. - All arm screen times will be increased by 3 seconds with a safehome failure message. --- src/main/cms/cms.h | 22 ++-- src/main/io/osd.c | 250 +++++++++++++++++++++++++-------------------- 2 files changed, 152 insertions(+), 120 deletions(-) diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index f1f8d8017c..69620f354c 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -7,14 +7,14 @@ #include "cms/cms_types.h" typedef enum { - CMS_KEY_NONE, - CMS_KEY_UP, - CMS_KEY_DOWN, - CMS_KEY_LEFT, - CMS_KEY_RIGHT, - CMS_KEY_ESC, - CMS_KEY_MENU, - CMS_KEY_SAVEMENU + CMS_KEY_NONE, + CMS_KEY_UP, + CMS_KEY_DOWN, + CMS_KEY_LEFT, + CMS_KEY_RIGHT, + CMS_KEY_ESC, + CMS_KEY_MENU, + CMS_KEY_SAVEMENU } cms_key_e; @@ -37,9 +37,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration); void cmsUpdate(uint32_t currentTimeUs); void cmsSetExternKey(cms_key_e extKey); -#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" -#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" -#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" // cmsMenuExit special ptr values #define CMS_EXIT (0) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8b7a57260f..e02949936f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3873,6 +3873,133 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) } } +uint8_t drawLogos(bool singular, uint8_t row) { + uint8_t logoRow = row; + uint8_t logoColOffset = 0; + bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); + +#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues. + if (isBfCompatibleVideoSystem(osdConfig())) + usePilotLogo = false; +#endif + + // Draw Logo(s) + if (usePilotLogo && !singular) { + logoColOffset = floor((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) / 3.0f); + } else { + logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); + } + + // Draw INAV logo + if ((singular && !usePilotLogo) || !singular) { + unsigned logo_c = SYM_LOGO_START; + uint8_t logo_x = logoColOffset; + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + } + + // Draw the pilot logo + if (usePilotLogo) { + unsigned logo_c = SYM_PILOT_LOGO_LRG_START; + uint8_t logo_x = 0; + logoRow = row; + if (singular) { + logo_x = logoColOffset; + } else { + logo_x = (logoColOffset * 2) + SYM_LOGO_WIDTH + ((osdDisplayPort->cols % 2 == 0) ? 0 : 1); // Add extra 1 px space between logos, if the OSD has an odd number of columns + } + + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + } + + return logoRow; +} + +uint8_t drawStats(uint8_t row) { +#ifdef USE_STATS + char string_buffer[30]; + uint8_t statNameX = (osdDisplayPort->cols - 22) / 2; + uint8_t statValueX = statNameX + 21; + + if (statsConfig()->stats_enabled) { + displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + string_buffer[5] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + string_buffer[5] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + string_buffer[5] = SYM_KM; + break; + } + string_buffer[6] = '\0'; + displayWrite(osdDisplayPort, statValueX-5, row, string_buffer); + + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); + uint32_t tot_mins = statsConfig()->stats_total_time / 60; + tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); + displayWrite(osdDisplayPort, statValueX-7, row, string_buffer); + +#ifdef USE_ADC + if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); + osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); + displayWrite(osdDisplayPort, statValueX-4, row, string_buffer); + displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH); + + displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:"); + if (statsConfig()->stats_total_dist) { + uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_NM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + break; + } + } else { + string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; + } + string_buffer[4] = '\0'; + displayWrite(osdDisplayPort, statValueX-3, row++, string_buffer); + } +#endif // USE_ADC + } +#endif // USE_STATS + return row; +} + static void osdSetNextRefreshIn(uint32_t timeMs) { resumeRefreshAt = micros() + timeMs * 1000; refreshWaitForResumeCmdRelease = true; @@ -3909,14 +4036,8 @@ static void osdCompleteAsyncInitialization(void) if (fontHasMetadata && metadata.charCount > 256) { hasExtendedFont = true; - unsigned logo_c = (osdConfig()->use_pilot_logo) ? SYM_PILOT_LOGO_LRG_START : SYM_LOGO_START; - unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH); - for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) { - for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) { - displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++); - } - y++; - } + + y = drawLogos(true, y); y++; } else if (!fontHasMetadata) { const char *m = "INVALID FONT"; @@ -3930,7 +4051,7 @@ static void osdCompleteAsyncInitialization(void) char string_buffer[30]; tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); - uint8_t xPos = osdDisplayIsHD() ? 15 : 5; + uint8_t xPos = (osdDisplayPort->cols - 19) / 2; // Automatically centre, regardless of resolution. In the case of odd number screens, bias to the left. displayWrite(osdDisplayPort, xPos, y++, string_buffer); #ifdef USE_CMS displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); @@ -3939,80 +4060,12 @@ static void osdCompleteAsyncInitialization(void) #endif #ifdef USE_STATS - uint8_t statNameX = osdDisplayIsHD() ? 14 : 4; - uint8_t statValueX = osdDisplayIsHD() ? 34 : 24; - - if (statsConfig()->stats_enabled) { - displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:"); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); - string_buffer[5] = SYM_MI; - break; - default: - case OSD_UNIT_GA: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); - string_buffer[5] = SYM_NM; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); - string_buffer[5] = SYM_KM; - break; - } - string_buffer[6] = '\0'; - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); - - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:"); - uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); - displayWrite(osdDisplayPort, statValueX-5, y, string_buffer); - -#ifdef USE_ADC - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:"); - osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); - displayWrite(osdDisplayPort, statValueX-4, y, string_buffer); - displayWriteChar(osdDisplayPort, statValueX, y, SYM_WH); - - displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:"); - if (statsConfig()->stats_total_dist) { - uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_NM; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_KM; - break; - } - } else { - string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; - } - string_buffer[4] = '\0'; - displayWrite(osdDisplayPort, statValueX-3, y, string_buffer); - } -#endif // USE_ADC - } + y = drawStats(++y); #endif displayCommitTransaction(osdDisplayPort); displayResync(osdDisplayPort); - osdSetNextRefreshIn(10000);//SPLASH_SCREEN_DISPLAY_TIME); // TEMPORARY - PUT BACK ONCE TESTING IS FINISHED! + osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); } void osdInit(displayPort_t *osdDisplayPortToUse) @@ -4372,35 +4425,10 @@ static void osdShowHDArmScreen(void) char *time; uint8_t armScreenRow = 2; // Start at row 2 - - // Draw Logo(s) - uint8_t logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); - // uint8_t logoColOffset = floor((osdDisplayPort->cols - ((using pilot logo) ? (SYM_LOGO_WIDTH * 2) : SYM_LOGO_WIDTH)) / 2.0f); - - // Draw INAV logo - unsigned logo_c = SYM_LOGO_START; - uint8_t logo_x = logoColOffset; - uint8_t logoRow = armScreenRow; - for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { - for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { - displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); - } - logoRow++; - } - - if (osdConfig()->use_pilot_logo) { - logo_c = SYM_PILOT_LOGO_LRG_START; - logo_x = (logoColOffset * 2) + ((osdDisplayPort->cols % 2 == 0) ? 0 : 1); // Add extra 1 px space between logos, if the OSD has an odd number of columns - logoRow = armScreenRow; - for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { - for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { - displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); - } - logoRow++; - } - } - armScreenRow = logoRow + 2; + + armScreenRow = drawLogos(false, armScreenRow); + armScreenRow++; strcpy(buf, "ARMED"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); @@ -4467,7 +4495,11 @@ static void osdShowHDArmScreen(void) } tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow, versionBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + +#ifdef USE_STATS + armScreenRow = drawStats(++armScreenRow); +#endif // USE_STATS } static void osdShowSDArmScreen(void) @@ -4662,7 +4694,7 @@ static void osdRefresh(timeUs_t currentTimeUs) uint32_t delay = osdConfig()->arm_screen_display_time; #if defined(USE_SAFE_HOME) if (safehome_distance) - delay *= 3; + delay+= 3000; #endif osdSetNextRefreshIn(delay); } else { From 8feb087730727b3999a72d4293e41d410e4e4d67 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 15 Sep 2023 08:06:02 +0100 Subject: [PATCH 26/62] Updates (not tested) - Added the ability to specify the gap between the logos - Matched arm screen and boot screen logo displays --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 7 +++++++ src/main/io/osd.c | 20 +++++++++++++++++--- src/main/io/osd.h | 3 ++- 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1fd236b8d7..51091fd55b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4312,6 +4312,16 @@ Temperature under which the IMU temperature OSD element will start blinking (dec --- +### osd_inav_to_pilot_logo_spacing + +The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen. + +| Default | Min | Max | +| --- | --- | --- | +| 8 | 0 | 20 | + +--- + ### osd_left_sidebar_scroll _// TODO_ diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index cb2c5b75e4..eff96ffdcf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3486,6 +3486,13 @@ groups: type: bool default_value: OFF + - name: osd_inav_to_pilot_logo_spacing + description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen. + field: inav_to_pilot_logo_spacing + min: 0 + max: 20 + default_value: 8 + - name: osd_arm_screen_display_time description: Amount of time to display the arm screen [ms] field: arm_screen_display_time diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e02949936f..52a1a7dcb5 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3679,6 +3679,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, + .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, #ifdef USE_WIND_ESTIMATOR @@ -3873,6 +3874,13 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) } } +/** + * @brief Draws the INAV and/or pilot logos on the display + * + * @param singular If true, only one logo will be drawn. If false, both logos will be drawn, separated by osdConfig()->inav_to_pilot_logo_spacing characters + * @param row The row number to start drawing the logos. If not singular, both logos are drawn on the same rows. + * @return uint8_t The row number after the logo(s). + */ uint8_t drawLogos(bool singular, uint8_t row) { uint8_t logoRow = row; uint8_t logoColOffset = 0; @@ -3883,9 +3891,15 @@ uint8_t drawLogos(bool singular, uint8_t row) { usePilotLogo = false; #endif + uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; + + if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { + logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing + } + // Draw Logo(s) if (usePilotLogo && !singular) { - logoColOffset = floor((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) / 3.0f); + logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2; } else { logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); } @@ -3910,7 +3924,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { if (singular) { logo_x = logoColOffset; } else { - logo_x = (logoColOffset * 2) + SYM_LOGO_WIDTH + ((osdDisplayPort->cols % 2 == 0) ? 0 : 1); // Add extra 1 px space between logos, if the OSD has an odd number of columns + logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; } for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { @@ -4037,7 +4051,7 @@ static void osdCompleteAsyncInitialization(void) if (fontHasMetadata && metadata.charCount > 256) { hasExtendedFont = true; - y = drawLogos(true, y); + y = drawLogos(false, y); y++; } else if (!fontHasMetadata) { const char *m = "INVALID FONT"; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 08b74cfe23..0d3e3ba976 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -446,7 +446,8 @@ typedef struct osdConfig_s { char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. - bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with/instead of the INAV logo + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. + uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; From 11029ba947e5eb2506a3c9efbb94cb04243cbacd Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 16 Sep 2023 09:06:52 +0100 Subject: [PATCH 27/62] Update Changes to HD arming screen - Explicitly select safehome row - temporarily removed stats due to space - combined craft name and armed to a single row. - put lon and lat on single row. - changed time format (need to test), as tenths of seconds is a bit pointless. date would be more useful. --- src/main/io/osd.c | 74 +++++++++++++++++++++++++++++------------------ 1 file changed, 46 insertions(+), 28 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 52a1a7dcb5..0bdf997a31 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3679,7 +3679,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, - .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, + .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, #ifdef USE_WIND_ESTIMATOR @@ -3891,11 +3891,11 @@ uint8_t drawLogos(bool singular, uint8_t row) { usePilotLogo = false; #endif - uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; + uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; - if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { - logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing - } + if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { + logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing + } // Draw Logo(s) if (usePilotLogo && !singular) { @@ -4433,26 +4433,29 @@ static void osdShowHDArmScreen(void) { dateTime_t dt; char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[30]; char *date; char *time; - + uint8_t safehomeRow = 0; uint8_t armScreenRow = 2; // Start at row 2 - - + armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; - strcpy(buf, "ARMED"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); - armScreenRow += 2; - if (strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + strcpy(buf2, "ARMED!"); + tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); + } else { + strcpy(buf, "ARMED!"); } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + safehomeRow = armScreenRow; + armScreenRow++; + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); @@ -4469,26 +4472,37 @@ static void osdShowHDArmScreen(void) if (STATE(GPS_FIX_HOME)) { if (osdConfig()->osd_home_position_arm_screen){ osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + uint8_t gap = 1; + uint8_t col = strlen(buf) + strlen(buf2) + gap; + + if ((osdDisplayPort->cols %2) != (col %2)) { + gap++; + col++; + } + + col = (osdDisplayPort->cols - col) / 2; + + displayWrite(osdDisplayPort, col, armScreenRow, buf); + displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + int digits = osdConfig()->plus_code_digits; olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } armScreenRow++; + #if defined (USE_SAFE_HOME) if (safehome_distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { - char buf2[12]; // format the distance first osdFormatDistanceStr(buf2, safehome_distance); tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); } textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message above the ARMED message to make it obvious (3rd parameter - row 7) - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, 7, buf, elemAttr); + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); } #endif } else { @@ -4499,21 +4513,25 @@ static void osdShowHDArmScreen(void) } #endif - if (rtcGetDateTime(&dt)) { - dateTimeFormatLocal(buf, &dt); - dateTimeSplitFormatted(buf, &date, &time); + if (rtcGetDateTimeLocal(&dt)) { + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, armScreenRow, date); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, armScreenRow++, time); - armScreenRow += 2; + //dateTimeFormatLocal(buf2, &dt); + //dateTimeSplitFormatted(buf2, &date, &time); + + //displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, armScreenRow, date); + //displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, armScreenRow++, time); + //armScreenRow += 2; } tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); -#ifdef USE_STATS - armScreenRow = drawStats(++armScreenRow); -#endif // USE_STATS +// #ifdef USE_STATS +// armScreenRow = drawStats(++armScreenRow); +// #endif // USE_STATS } static void osdShowSDArmScreen(void) From 09c0f302f95aa2bf401838ed656c47b25dc82d23 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 17 Sep 2023 09:06:44 +0100 Subject: [PATCH 28/62] Tidying up - Removed unneeded code after testing new date time format on HD arm screen. I needed to use actual aircraft as HITL doesn't support date/time over GPS. - Updated date time format on SD arm screen - Remove extra row if safehomes is not used - Switched around format of safehome display for readability - Arm screens will show stats if there is enough space to neatly do so - Single row for long lat on SD, as there is plenty of space - Tidied up all the `y + x` instances on SD arm screen, and made the variable name more explicit --- src/main/io/osd.c | 128 ++++++++++++++++++++++++---------------------- 1 file changed, 66 insertions(+), 62 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0bdf997a31..7d2e2ed9a0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4431,15 +4431,13 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) // HD arming screen. based on the minimum HD OSD grid size of 50 x 18 static void osdShowHDArmScreen(void) { - dateTime_t dt; - char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char buf2[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[30]; - char *date; - char *time; - uint8_t safehomeRow = 0; - uint8_t armScreenRow = 2; // Start at row 2 + dateTime_t dt; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; + uint8_t safehomeRow = 0; + uint8_t armScreenRow = 2; // Start at row 2 armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; @@ -4451,10 +4449,16 @@ static void osdShowHDArmScreen(void) } else { strcpy(buf, "ARMED!"); } - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - safehomeRow = armScreenRow; - armScreenRow++; +#if defined(USE_GPS) +#if defined (USE_SAFE_HOME) + if (safehome_distance) { + safehomeRow = armScreenRow; + armScreenRow++; + } +#endif // USE_SAFE_HOME +#endif // USE_GPS + armScreenRow++; if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION @@ -4470,7 +4474,7 @@ static void osdShowHDArmScreen(void) #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ + if (osdConfig()->osd_home_position_arm_screen) { osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); uint8_t gap = 1; @@ -4490,7 +4494,6 @@ static void osdShowHDArmScreen(void) olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } - armScreenRow++; #if defined (USE_SAFE_HOME) if (safehome_distance) { // safehome found during arming @@ -4498,7 +4501,7 @@ static void osdShowHDArmScreen(void) strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); } textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious @@ -4508,8 +4511,8 @@ static void osdShowHDArmScreen(void) } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - armScreenRow++; } + armScreenRow++; } #endif @@ -4517,100 +4520,101 @@ static void osdShowHDArmScreen(void) tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); armScreenRow++; - - //dateTimeFormatLocal(buf2, &dt); - //dateTimeSplitFormatted(buf2, &date, &time); - - //displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, armScreenRow, date); - //displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, armScreenRow++, time); - //armScreenRow += 2; } tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + armScreenRow++; -// #ifdef USE_STATS -// armScreenRow = drawStats(++armScreenRow); -// #endif // USE_STATS +#ifdef USE_STATS + if (armScreenRow < (osdDisplayPort->rows - 4)) + armScreenRow = drawStats(armScreenRow); +#endif // USE_STATS } static void osdShowSDArmScreen(void) { - dateTime_t dt; - char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[30]; - char *date; - char *time; + dateTime_t dt; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + uint8_t safehomeRow = 0; - strcpy(buf, "ARMED"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 2; + strcpy(buf, "ARMED!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + safehomeRow = armScreenRow; + armScreenRow++; if (strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf ); - y += 1; + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); } + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); #else strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); #endif } - y += 1; + armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ + if (osdConfig()->osd_home_position_arm_screen) { osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - osdFormatCoordinate(buf, SYM_LON, GPS_home.lon); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf); - int digits = osdConfig()->plus_code_digits; + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + + uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; + displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); + displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); + + int digits = osdConfig()->plus_code_digits; olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } - y += 4; + #if defined (USE_SAFE_HOME) if (safehome_distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { - char buf2[12]; // format the distance first osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); } textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf); - y += 1; + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); } + armScreenRow++; } #endif - if (rtcGetDateTime(&dt)) { - dateTimeFormatLocal(buf, &dt); - dateTimeSplitFormatted(buf, &date, &time); - - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time); - y += 3; + if (rtcGetDateTimeLocal(&dt)) { + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; } tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + armScreenRow++; + +#ifdef USE_STATS + if (armScreenRow < (osdDisplayPort->rows - 4)) + armScreenRow = drawStats(armScreenRow); +#endif // USE_STATS } // called when motors armed From d92538de21d0fe89744f820f78e8a54141c79455 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 18 Sep 2023 08:35:29 +0100 Subject: [PATCH 29/62] Converted tabs to 4 spaces --- src/main/cms/cms.h | 16 +- src/main/io/osd.c | 7796 ++++++++++++++++++++++---------------------- src/main/io/osd.h | 538 +-- 3 files changed, 4175 insertions(+), 4175 deletions(-) diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 69620f354c..812975e3b4 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -7,14 +7,14 @@ #include "cms/cms_types.h" typedef enum { - CMS_KEY_NONE, - CMS_KEY_UP, - CMS_KEY_DOWN, - CMS_KEY_LEFT, - CMS_KEY_RIGHT, - CMS_KEY_ESC, - CMS_KEY_MENU, - CMS_KEY_SAVEMENU + CMS_KEY_NONE, + CMS_KEY_UP, + CMS_KEY_DOWN, + CMS_KEY_LEFT, + CMS_KEY_RIGHT, + CMS_KEY_ESC, + CMS_KEY_MENU, + CMS_KEY_SAVEMENU } cms_key_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7d2e2ed9a0..4191c670b0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -141,8 +141,8 @@ // Wrap all string constants intenteded for display as messages with // this macro to ensure compile time length validation. #define OSD_MESSAGE_STR(x) ({ \ - STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \ - x; \ + STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \ + x; \ }) #define OSD_CHR_IS_NUM(c) (c >= '0' && c <= '9') @@ -163,17 +163,17 @@ static pt1Filter_t GForceFilter, GForceFilterAxis[XYZ_AXIS_COUNT]; static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { - uint16_t max_speed; - uint16_t max_3D_speed; - uint16_t max_air_speed; - uint16_t min_voltage; // /100 - int16_t max_current; - int32_t max_power; - int16_t min_rssi; - int16_t min_lq; // for CRSF - int16_t min_rssi_dbm; // for CRSF - int32_t max_altitude; - uint32_t max_distance; + uint16_t max_speed; + uint16_t max_3D_speed; + uint16_t max_air_speed; + uint16_t min_voltage; // /100 + int16_t max_current; + int32_t max_power; + int16_t min_rssi; + int16_t min_lq; // for CRSF + int16_t min_rssi_dbm; // for CRSF + int32_t max_altitude; + uint32_t max_distance; } statistic_t; static statistic_t stats; @@ -186,8 +186,8 @@ static bool fullRedraw = false; static uint8_t armState; typedef struct osdMapData_s { - uint32_t scale; - char referenceSymbol; + uint32_t scale; + char referenceSymbol; } osdMapData_t; static osdMapData_t osdMapData; @@ -207,28 +207,28 @@ PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { - savingSettings = true; + savingSettings = true; } void osdShowEEPROMSavedNotification(void) { - savingSettings = false; - notify_settings_saved = millis() + 5000; + savingSettings = false; + notify_settings_saved = millis() + 5000; } bool osdDisplayIsPAL(void) { - return displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL; + return displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL; } bool osdDisplayIsHD(void) { - if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO) - { - return true; - } - return false; + if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO) + { + return true; + } + return false; } @@ -238,11 +238,11 @@ bool osdDisplayIsHD(void) */ static void osdLeftAlignString(char *buff) { - uint8_t sp = 0, ch = 0; - uint8_t len = strlen(buff); - while (buff[sp] == ' ') sp++; - for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; - for (sp = ch; sp < len; sp++) buff[sp] = ' '; + uint8_t sp = 0, ch = 0; + uint8_t len = strlen(buff); + while (buff[sp] == ' ') sp++; + for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; + for (sp = ch; sp < len; sp++) buff[sp] = ' '; } /* @@ -252,29 +252,29 @@ static void osdLeftAlignString(char *buff) */ /* void osdSimpleDistanceSymbol(char *buff, int32_t dist) { - int32_t convertedDistance; - char suffix; + int32_t convertedDistance; + char suffix; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - convertedDistance = CENTIMETERS_TO_FEET(dist); - suffix = SYM_ALT_FT; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - convertedDistance = CENTIMETERS_TO_METERS(dist); - suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + convertedDistance = CENTIMETERS_TO_FEET(dist); + suffix = SYM_ALT_FT; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + convertedDistance = CENTIMETERS_TO_METERS(dist); + suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode + break; + } - tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases - buff[5] = suffix; - buff[6] = '\0'; + tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases + buff[5] = suffix; + buff[6] = '\0'; } */ /** @@ -284,58 +284,58 @@ static void osdLeftAlignString(char *buff) */ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { - uint8_t digits = 3U; // Total number of digits (including decimal point) - uint8_t sym_index = 3U; // Position (index) at buffer of units symbol - uint8_t symbol_m = SYM_DIST_M; - uint8_t symbol_km = SYM_DIST_KM; - uint8_t symbol_ft = SYM_DIST_FT; - uint8_t symbol_mi = SYM_DIST_MI; - uint8_t symbol_nm = SYM_DIST_NM; + uint8_t digits = 3U; // Total number of digits (including decimal point) + uint8_t sym_index = 3U; // Position (index) at buffer of units symbol + uint8_t symbol_m = SYM_DIST_M; + uint8_t symbol_km = SYM_DIST_KM; + uint8_t symbol_ft = SYM_DIST_FT; + uint8_t symbol_mi = SYM_DIST_MI; + uint8_t symbol_nm = SYM_DIST_NM; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so up no switch to scaled decimal occurs above 99 - digits = 4U; - sym_index = 4U; - // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode - symbol_m = SYM_ALT_M; - symbol_km = SYM_ALT_KM; - symbol_ft = SYM_ALT_FT; - symbol_mi = SYM_MI; - symbol_nm = SYM_MI; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so up no switch to scaled decimal occurs above 99 + digits = 4U; + sym_index = 4U; + // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode + symbol_m = SYM_ALT_M; + symbol_km = SYM_ALT_KM; + symbol_ft = SYM_ALT_FT; + symbol_mi = SYM_MI; + symbol_nm = SYM_MI; + } #endif - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { - buff[sym_index] = symbol_mi; - } else { - buff[sym_index] = symbol_ft; - } - buff[sym_index + 1] = '\0'; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { - buff[sym_index] = symbol_km; - } else { - buff[sym_index] = symbol_m; - } - buff[sym_index + 1] = '\0'; - break; - case OSD_UNIT_GA: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { - buff[sym_index] = symbol_nm; - } else { - buff[sym_index] = symbol_ft; - } - buff[sym_index + 1] = '\0'; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { + buff[sym_index] = symbol_mi; + } else { + buff[sym_index] = symbol_ft; + } + buff[sym_index + 1] = '\0'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { + buff[sym_index] = symbol_km; + } else { + buff[sym_index] = symbol_m; + } + buff[sym_index + 1] = '\0'; + break; + case OSD_UNIT_GA: + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { + buff[sym_index] = symbol_nm; + } else { + buff[sym_index] = symbol_ft; + } + buff[sym_index + 1] = '\0'; + break; + } } /** @@ -344,45 +344,45 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) */ static void osdFormatDistanceStr(char *buff, int32_t dist) { - int32_t centifeet; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { - // Show feet when dist < 0.5mi - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show miles when dist >= 0.5mi - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), - (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (abs(dist) < METERS_PER_KILOMETER * 100) { - // Show meters when dist < 1km - tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); - } else { - // Show kilometers when dist >= 1km - tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), - (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); - } - break; - case OSD_UNIT_GA: - centifeet = CENTIMETERS_TO_CENTIFEET(dist); - if (abs(centifeet) < 100000) { - // Show feet when dist < 1000ft - tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); - } else { - // Show nautical miles when dist >= 1000ft - tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), - (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), SYM_NM); - } - break; - } + int32_t centifeet; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { + // Show feet when dist < 0.5mi + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show miles when dist >= 0.5mi + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)), + (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI); + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (abs(dist) < METERS_PER_KILOMETER * 100) { + // Show meters when dist < 1km + tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M); + } else { + // Show kilometers when dist >= 1km + tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)), + (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM); + } + break; + case OSD_UNIT_GA: + centifeet = CENTIMETERS_TO_CENTIFEET(dist); + if (abs(centifeet) < 100000) { + // Show feet when dist < 1000ft + tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT); + } else { + // Show nautical miles when dist >= 1000ft + tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)), + (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), SYM_NM); + } + break; + } } /** @@ -391,20 +391,20 @@ static void osdFormatDistanceStr(char *buff, int32_t dist) */ static int32_t osdConvertVelocityToUnit(int32_t vel) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph - case OSD_UNIT_METRIC: - return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh - case OSD_UNIT_GA: - return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots - } - // Unreachable - return -1; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph + case OSD_UNIT_METRIC: + return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh + case OSD_UNIT_GA: + return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots + } + // Unreachable + return -1; } /** @@ -415,41 +415,41 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) */ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) { - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } - break; - case OSD_UNIT_METRIC: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } - break; - case OSD_UNIT_GA: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); + } + break; + case OSD_UNIT_METRIC: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); + } + break; + case OSD_UNIT_GA: + if (_max) { + tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } else { + tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); + } + break; + } } /** * Returns the average velocity. This always uses stats, so can be called as an OSD element later if wanted, to show a real time average */ static void osdGenerateAverageVelocityStr(char* buff) { - uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); - osdFormatVelocityStr(buff, cmPerSec, false, false); + uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); + osdFormatVelocityStr(buff, cmPerSec, false, false); } /** @@ -461,35 +461,35 @@ static void osdGenerateAverageVelocityStr(char* buff) { #ifdef USE_WIND_ESTIMATOR static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) { - int32_t centivalue; - char suffix; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - centivalue = CMSEC_TO_CENTIMPH(ws); - suffix = SYM_MPH; - break; - case OSD_UNIT_GA: - centivalue = CMSEC_TO_CENTIKNOTS(ws); - suffix = SYM_KT; - break; - default: - case OSD_UNIT_METRIC: - centivalue = CMSEC_TO_CENTIKPH(ws); - suffix = SYM_KMH; - break; - } + int32_t centivalue; + char suffix; + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + centivalue = CMSEC_TO_CENTIMPH(ws); + suffix = SYM_MPH; + break; + case OSD_UNIT_GA: + centivalue = CMSEC_TO_CENTIKNOTS(ws); + suffix = SYM_KT; + break; + default: + case OSD_UNIT_METRIC: + centivalue = CMSEC_TO_CENTIKPH(ws); + suffix = SYM_KMH; + break; + } - osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); + osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); - if (!isValid && ((millis() / 1000) % 4 < 2)) - suffix = '*'; + if (!isValid && ((millis() / 1000) % 4 < 2)) + suffix = '*'; - buff[3] = suffix; - buff[4] = '\0'; + buff[3] = suffix; + buff[4] = '\0'; } #endif @@ -499,29 +499,29 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ /* void osdSimpleAltitudeSymbol(char *buff, int32_t alt) { - int32_t convertedAltutude = 0; - char suffix = '\0'; + int32_t convertedAltutude = 0; + char suffix = '\0'; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - convertedAltutude = CENTIMETERS_TO_FEET(alt); - suffix = SYM_ALT_FT; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - convertedAltutude = CENTIMETERS_TO_METERS(alt); - suffix = SYM_ALT_M; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + convertedAltutude = CENTIMETERS_TO_FEET(alt); + suffix = SYM_ALT_FT; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + convertedAltutude = CENTIMETERS_TO_METERS(alt); + suffix = SYM_ALT_M; + break; + } - tfp_sprintf(buff, "%4d", (int) convertedAltutude); - buff[4] = suffix; - buff[5] = '\0'; + tfp_sprintf(buff, "%4d", (int) convertedAltutude); + buff[4] = suffix; + buff[5] = '\0'; } */ /** @@ -531,54 +531,54 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { - uint8_t totalDigits = 4U; - uint8_t digits = 4U; - uint8_t symbolIndex = 4U; - uint8_t symbolKFt = SYM_ALT_KFT; + uint8_t totalDigits = 4U; + uint8_t digits = 4U; + uint8_t symbolIndex = 4U; + uint8_t symbolKFt = SYM_ALT_KFT; - if (alt >= 0) { - digits = 3U; - buff[0] = ' '; - } + if (alt >= 0) { + digits = 3U; + buff[0] = ' '; + } #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - totalDigits++; - digits++; - symbolIndex++; - symbolKFt = SYM_ALT_FT; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + totalDigits++; + digits++; + symbolIndex++; + symbolKFt = SYM_ALT_FT; + } #endif - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { - // Scaled to kft - buff[symbolIndex++] = symbolKFt; - } else { - // Formatted in feet - buff[symbolIndex++] = SYM_ALT_FT; - } - buff[symbolIndex] = '\0'; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // alt is alredy in cm - if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { - // Scaled to km - buff[symbolIndex++] = SYM_ALT_KM; - } else { - // Formatted in m - buff[symbolIndex++] = SYM_ALT_M; - } - buff[symbolIndex] = '\0'; - break; - } + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { + // Scaled to kft + buff[symbolIndex++] = symbolKFt; + } else { + // Formatted in feet + buff[symbolIndex++] = SYM_ALT_FT; + } + buff[symbolIndex] = '\0'; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // alt is alredy in cm + if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { + // Scaled to km + buff[symbolIndex++] = SYM_ALT_KM; + } else { + // Formatted in m + buff[symbolIndex++] = SYM_ALT_M; + } + buff[symbolIndex] = '\0'; + break; + } } /** @@ -587,52 +587,52 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) */ static void osdFormatAltitudeStr(char *buff, int32_t alt) { - int32_t value; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - value = CENTIMETERS_TO_FEET(alt); - tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - value = CENTIMETERS_TO_METERS(alt); - tfp_sprintf(buff, "%d%c", (int)value, SYM_M); - break; - } + int32_t value; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + value = CENTIMETERS_TO_FEET(alt); + tfp_sprintf(buff, "%d%c", (int)value, SYM_FT); + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + value = CENTIMETERS_TO_METERS(alt); + tfp_sprintf(buff, "%d%c", (int)value, SYM_M); + break; + } } static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h) { - uint32_t value = seconds; - char sym = sym_m; - // Maximum value we can show in minutes is 99 minutes and 59 seconds - if (seconds > (99 * 60) + 59) { - sym = sym_h; - value = seconds / 60; - } - buff[0] = sym; - tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); + uint32_t value = seconds; + char sym = sym_m; + // Maximum value we can show in minutes is 99 minutes and 59 seconds + if (seconds > (99 * 60) + 59) { + sym = sym_h; + value = seconds / 60; + } + buff[0] = sym; + tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60)); } static inline void osdFormatOnTime(char *buff) { - osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H); + osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H); } static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) { - uint32_t seconds = getFlightTime(); - osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H); - if (attr && osdConfig()->time_alarm > 0) { - if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) { - TEXT_ATTRIBUTES_ADD_BLINK(*attr); - } - } + uint32_t seconds = getFlightTime(); + osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H); + if (attr && osdConfig()->time_alarm > 0) { + if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) { + TEXT_ATTRIBUTES_ADD_BLINK(*attr); + } + } } /** @@ -641,23 +641,23 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) */ char *osdFormatTrimWhiteSpace(char *buff) { - char *end; + char *end; - // Trim leading spaces - while(isspace((unsigned char)*buff)) buff++; + // Trim leading spaces + while(isspace((unsigned char)*buff)) buff++; - // All spaces? - if(*buff == 0) - return buff; + // All spaces? + if(*buff == 0) + return buff; - // Trim trailing spaces - end = buff + strlen(buff) - 1; - while(end > buff && isspace((unsigned char)*end)) end--; + // Trim trailing spaces + end = buff + strlen(buff) - 1; + while(end > buff && isspace((unsigned char)*end)) end--; - // Write new null terminator character - end[1] = '\0'; + // Write new null terminator character + end[1] = '\0'; - return buff; + return buff; } /** @@ -665,32 +665,32 @@ char *osdFormatTrimWhiteSpace(char *buff) */ static uint16_t osdConvertRSSI(void) { - // change range to [0, 99] - return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); + // change range to [0, 99] + return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } static uint16_t osdGetCrsfLQ(void) { - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - int16_t displayedLQ = 0; - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - displayedLQ = statsLQ; - break; - case OSD_CRSF_LQ_TYPE2: - displayedLQ = statsLQ; - break; - case OSD_CRSF_LQ_TYPE3: - displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ; - break; - } - return displayedLQ; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + int16_t displayedLQ = 0; + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + displayedLQ = statsLQ; + break; + case OSD_CRSF_LQ_TYPE2: + displayedLQ = statsLQ; + break; + case OSD_CRSF_LQ_TYPE3: + displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ; + break; + } + return displayedLQ; } static int16_t osdGetCrsfdBm(void) { - return rxLinkStatistics.uplinkRSSI; + return rxLinkStatistics.uplinkRSSI; } /** * Displays a temperature postfixed with a symbol depending on the current unit system @@ -700,366 +700,366 @@ static int16_t osdGetCrsfdBm(void) */ static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t symbol, const char *label, bool valid, int16_t temperature, int16_t alarm_min, int16_t alarm_max) { - char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2]; - textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT; - uint8_t valueXOffset = 0; + char buff[TEMPERATURE_LABEL_LEN + 2 < 6 ? 6 : TEMPERATURE_LABEL_LEN + 2]; + textAttributes_t elemAttr = valid ? TEXT_ATTRIBUTES_NONE : _TEXT_ATTRIBUTES_BLINK_BIT; + uint8_t valueXOffset = 0; - if (symbol) { - buff[0] = symbol; - buff[1] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - valueXOffset = 1; - } + if (symbol) { + buff[0] = symbol; + buff[1] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = 1; + } #ifdef USE_TEMPERATURE_SENSOR - else if (label[0] != '\0') { - uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN); - memcpy(buff, label, label_len); - memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len); - buff[5] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1; - } + else if (label[0] != '\0') { + uint8_t label_len = strnlen(label, TEMPERATURE_LABEL_LEN); + memcpy(buff, label, label_len); + memset(buff + label_len, ' ', TEMPERATURE_LABEL_LEN + 1 - label_len); + buff[5] = '\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + valueXOffset = osdConfig()->temp_label_align == OSD_ALIGN_LEFT ? 5 : label_len + 1; + } #else - UNUSED(label); + UNUSED(label); #endif - if (valid) { + if (valid) { - 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); + 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); - } else - strcpy(buff, "---"); + } else + strcpy(buff, "---"); - buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C; - buff[4] = '\0'; + buff[3] = osdConfig()->units == OSD_UNIT_IMPERIAL ? SYM_TEMP_F : SYM_TEMP_C; + buff[4] = '\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + valueXOffset, elemPosY, buff, elemAttr); } #ifdef USE_TEMPERATURE_SENSOR static void osdDisplayTemperatureSensor(uint8_t elemPosX, uint8_t elemPosY, uint8_t sensorIndex) { - int16_t temperature; - const bool valid = getSensorTemperature(sensorIndex, &temperature); - const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex); - uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0; - osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max); + int16_t temperature; + const bool valid = getSensorTemperature(sensorIndex, &temperature); + const tempSensorConfig_t *sensorConfig = tempSensorConfig(sensorIndex); + uint16_t symbol = sensorConfig->osdSymbol ? SYM_TEMP_SENSOR_FIRST + sensorConfig->osdSymbol - 1 : 0; + osdDisplayTemperature(elemPosX, elemPosY, symbol, sensorConfig->label, valid, temperature, sensorConfig->alarm_min, sensorConfig->alarm_max); } #endif static void osdFormatCoordinate(char *buff, char sym, int32_t val) { - // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals - const int coordinateLength = osdConfig()->coordinate_digits + 1; + // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals + const int coordinateLength = osdConfig()->coordinate_digits + 1; - buff[0] = sym; - int32_t integerPart = val / GPS_DEGREES_DIVIDER; - // Latitude maximum integer width is 3 (-90) while - // longitude maximum integer width is 4 (-180). - int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); - // We can show up to 7 digits in decimalPart. - int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); - STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); - int decimalDigits; - bool bfcompat = false; // Assume BFCOMPAT mode is no enabled + buff[0] = sym; + int32_t integerPart = val / GPS_DEGREES_DIVIDER; + // Latitude maximum integer width is 3 (-90) while + // longitude maximum integer width is 4 (-180). + int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", (int)integerPart); + // We can show up to 7 digits in decimalPart. + int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); + STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); + int decimalDigits; + bool bfcompat = false; // Assume BFCOMPAT mode is no enabled #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; - } + if(isBfCompatibleVideoSystem(osdConfig())) { + bfcompat = true; + } #endif - if (!bfcompat) { - decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); - // Embbed the decimal separator - buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; - buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; - } else { - // BFCOMPAT mode enabled - decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart); - } - // Fill up to coordinateLength with zeros - int total = 1 + integerDigits + decimalDigits; - while(total < coordinateLength) { - buff[total] = '0'; - total++; - } - buff[coordinateLength] = '\0'; + if (!bfcompat) { + decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); + // Embbed the decimal separator + buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; + buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; + } else { + // BFCOMPAT mode enabled + decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart); + } + // Fill up to coordinateLength with zeros + int total = 1 + integerDigits + decimalDigits; + while(total < coordinateLength) { + buff[total] = '0'; + total++; + } + buff[coordinateLength] = '\0'; } static void osdFormatCraftName(char *buff) { - if (strlen(systemConfig()->craftName) == 0) - strcpy(buff, "CRAFT_NAME"); - else { - for (int i = 0; i < MAX_NAME_LENGTH; i++) { - buff[i] = sl_toupper((unsigned char)systemConfig()->craftName[i]); - if (systemConfig()->craftName[i] == 0) - break; - } - } + if (strlen(systemConfig()->craftName) == 0) + strcpy(buff, "CRAFT_NAME"); + else { + for (int i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = sl_toupper((unsigned char)systemConfig()->craftName[i]); + if (systemConfig()->craftName[i] == 0) + break; + } + } } void osdFormatPilotName(char *buff) { - if (strlen(systemConfig()->pilotName) == 0) - strcpy(buff, "PILOT_NAME"); - else { - for (int i = 0; i < MAX_NAME_LENGTH; i++) { - buff[i] = sl_toupper((unsigned char)systemConfig()->pilotName[i]); - if (systemConfig()->pilotName[i] == 0) - break; - } - } + if (strlen(systemConfig()->pilotName) == 0) + strcpy(buff, "PILOT_NAME"); + else { + for (int i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = sl_toupper((unsigned char)systemConfig()->pilotName[i]); + if (systemConfig()->pilotName[i] == 0) + break; + } + } } static const char * osdArmingDisabledReasonMessage(void) { - const char *message = NULL; - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - switch (isArmingDisabledReason()) { - case ARMING_DISABLED_FAILSAFE_SYSTEM: - // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c - if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { - if (failsafeIsReceivingRxData()) { - // If we're not using sticks, it means the ARM switch - // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING - // yet - return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); - } - // Not receiving RX data - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); - } - return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); - case ARMING_DISABLED_NOT_LEVEL: - return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL); - case ARMING_DISABLED_SENSORS_CALIBRATING: - return OSD_MESSAGE_STR(OSD_MSG_SENSORS_CAL); - case ARMING_DISABLED_SYSTEM_OVERLOADED: - return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED); - case ARMING_DISABLED_NAVIGATION_UNSAFE: - // Check the exact reason - switch (navigationIsBlockingArming(NULL)) { - char buf[6]; - case NAV_ARMING_BLOCKER_NONE: - break; - case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: - return OSD_MESSAGE_STR(OSD_MSG_WAITING_GPS_FIX); - case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: - return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); - case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); - tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); - return message = messageBuf; - case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: - return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); - } - break; - case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: - return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL); - case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: - return OSD_MESSAGE_STR(OSD_MSG_ACC_NOT_CAL); - case ARMING_DISABLED_ARM_SWITCH: - return OSD_MESSAGE_STR(OSD_MSG_DISARM_1ST); - case ARMING_DISABLED_HARDWARE_FAILURE: - { - if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_GYRO_FAILURE); - } - if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_ACC_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_MAG_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_BARO_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_GPS_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_RANGEFINDER_FAIL); - } - if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) { - return OSD_MESSAGE_STR(OSD_MSG_PITOT_FAIL); - } - } - return OSD_MESSAGE_STR(OSD_MSG_HW_FAIL); - case ARMING_DISABLED_BOXFAILSAFE: - return OSD_MESSAGE_STR(OSD_MSG_FS_EN); - case ARMING_DISABLED_BOXKILLSWITCH: - return OSD_MESSAGE_STR(OSD_MSG_KILL_SW_EN); - case ARMING_DISABLED_RC_LINK: - return OSD_MESSAGE_STR(OSD_MSG_NO_RC_LINK); - case ARMING_DISABLED_THROTTLE: - return OSD_MESSAGE_STR(OSD_MSG_THROTTLE_NOT_LOW); - case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED: - return OSD_MESSAGE_STR(OSD_MSG_ROLLPITCH_OFFCENTER); - case ARMING_DISABLED_SERVO_AUTOTRIM: - return OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM_ACTIVE); - case ARMING_DISABLED_OOM: - return OSD_MESSAGE_STR(OSD_MSG_NOT_ENOUGH_MEMORY); - case ARMING_DISABLED_INVALID_SETTING: - return OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); - case ARMING_DISABLED_CLI: - return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); - case ARMING_DISABLED_PWM_OUTPUT_ERROR: - return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); - case ARMING_DISABLED_NO_PREARM: - return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); - case ARMING_DISABLED_DSHOT_BEEPER: - return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); - // Cases without message - case ARMING_DISABLED_LANDING_DETECTED: - FALLTHROUGH; - case ARMING_DISABLED_CMS_MENU: - FALLTHROUGH; - case ARMING_DISABLED_OSD_MENU: - FALLTHROUGH; - case ARMING_DISABLED_ALL_FLAGS: - FALLTHROUGH; - case ARMED: - FALLTHROUGH; - case SIMULATOR_MODE_HITL: - FALLTHROUGH; - case SIMULATOR_MODE_SITL: - FALLTHROUGH; - case WAS_EVER_ARMED: - break; - } - return NULL; + switch (isArmingDisabledReason()) { + case ARMING_DISABLED_FAILSAFE_SYSTEM: + // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c + if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) { + if (failsafeIsReceivingRxData()) { + // If we're not using sticks, it means the ARM switch + // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING + // yet + return OSD_MESSAGE_STR(OSD_MSG_TURN_ARM_SW_OFF); + } + // Not receiving RX data + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); + } + return OSD_MESSAGE_STR(OSD_MSG_DISABLED_BY_FS); + case ARMING_DISABLED_NOT_LEVEL: + return OSD_MESSAGE_STR(OSD_MSG_AIRCRAFT_UNLEVEL); + case ARMING_DISABLED_SENSORS_CALIBRATING: + return OSD_MESSAGE_STR(OSD_MSG_SENSORS_CAL); + case ARMING_DISABLED_SYSTEM_OVERLOADED: + return OSD_MESSAGE_STR(OSD_MSG_SYS_OVERLOADED); + case ARMING_DISABLED_NAVIGATION_UNSAFE: + // Check the exact reason + switch (navigationIsBlockingArming(NULL)) { + char buf[6]; + case NAV_ARMING_BLOCKER_NONE: + break; + case NAV_ARMING_BLOCKER_MISSING_GPS_FIX: + return OSD_MESSAGE_STR(OSD_MSG_WAITING_GPS_FIX); + case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: + return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); + case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); + return message = messageBuf; + case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: + return OSD_MESSAGE_STR(OSD_MSG_JUMP_WP_MISCONFIG); + } + break; + case ARMING_DISABLED_COMPASS_NOT_CALIBRATED: + return OSD_MESSAGE_STR(OSD_MSG_MAG_NOT_CAL); + case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED: + return OSD_MESSAGE_STR(OSD_MSG_ACC_NOT_CAL); + case ARMING_DISABLED_ARM_SWITCH: + return OSD_MESSAGE_STR(OSD_MSG_DISARM_1ST); + case ARMING_DISABLED_HARDWARE_FAILURE: + { + if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_GYRO_FAILURE); + } + if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_ACC_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_MAG_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_BARO_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_GPS_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_RANGEFINDER_FAIL); + } + if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) { + return OSD_MESSAGE_STR(OSD_MSG_PITOT_FAIL); + } + } + return OSD_MESSAGE_STR(OSD_MSG_HW_FAIL); + case ARMING_DISABLED_BOXFAILSAFE: + return OSD_MESSAGE_STR(OSD_MSG_FS_EN); + case ARMING_DISABLED_BOXKILLSWITCH: + return OSD_MESSAGE_STR(OSD_MSG_KILL_SW_EN); + case ARMING_DISABLED_RC_LINK: + return OSD_MESSAGE_STR(OSD_MSG_NO_RC_LINK); + case ARMING_DISABLED_THROTTLE: + return OSD_MESSAGE_STR(OSD_MSG_THROTTLE_NOT_LOW); + case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED: + return OSD_MESSAGE_STR(OSD_MSG_ROLLPITCH_OFFCENTER); + case ARMING_DISABLED_SERVO_AUTOTRIM: + return OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM_ACTIVE); + case ARMING_DISABLED_OOM: + return OSD_MESSAGE_STR(OSD_MSG_NOT_ENOUGH_MEMORY); + case ARMING_DISABLED_INVALID_SETTING: + return OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING); + case ARMING_DISABLED_CLI: + return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); + case ARMING_DISABLED_PWM_OUTPUT_ERROR: + return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); + case ARMING_DISABLED_NO_PREARM: + return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + // Cases without message + case ARMING_DISABLED_LANDING_DETECTED: + FALLTHROUGH; + case ARMING_DISABLED_CMS_MENU: + FALLTHROUGH; + case ARMING_DISABLED_OSD_MENU: + FALLTHROUGH; + case ARMING_DISABLED_ALL_FLAGS: + FALLTHROUGH; + case ARMED: + FALLTHROUGH; + case SIMULATOR_MODE_HITL: + FALLTHROUGH; + case SIMULATOR_MODE_SITL: + FALLTHROUGH; + case WAS_EVER_ARMED: + break; + } + return NULL; } static const char * osdFailsafePhaseMessage(void) { - // See failsafe.h for each phase explanation - switch (failsafePhase()) { - case FAILSAFE_RETURN_TO_HOME: - // XXX: Keep this in sync with OSD_FLYMODE. - return OSD_MESSAGE_STR(OSD_MSG_RTH_FS); - case FAILSAFE_LANDING: - // This should be considered an emergengy landing - return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); - case FAILSAFE_RX_LOSS_MONITORING: - // Only reachable from FAILSAFE_LANDED, which performs - // a disarm. Since aircraft has been disarmed, we no - // longer show failsafe details. - FALLTHROUGH; - case FAILSAFE_LANDED: - // Very brief, disarms and transitions into - // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents - // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM, - // so we'll show the user how to re-arm in when - // that flag is the reason to prevent arming. - FALLTHROUGH; - case FAILSAFE_RX_LOSS_IDLE: - // This only happens when user has chosen NONE as FS - // procedure. The recovery messages should be enough. - FALLTHROUGH; - case FAILSAFE_IDLE: - // Failsafe not active - FALLTHROUGH; - case FAILSAFE_RX_LOSS_DETECTED: - // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED - // or the FS procedure immediately. - FALLTHROUGH; - case FAILSAFE_RX_LOSS_RECOVERED: - // Exiting failsafe - break; - } - return NULL; + // See failsafe.h for each phase explanation + switch (failsafePhase()) { + case FAILSAFE_RETURN_TO_HOME: + // XXX: Keep this in sync with OSD_FLYMODE. + return OSD_MESSAGE_STR(OSD_MSG_RTH_FS); + case FAILSAFE_LANDING: + // This should be considered an emergengy landing + return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); + case FAILSAFE_RX_LOSS_MONITORING: + // Only reachable from FAILSAFE_LANDED, which performs + // a disarm. Since aircraft has been disarmed, we no + // longer show failsafe details. + FALLTHROUGH; + case FAILSAFE_LANDED: + // Very brief, disarms and transitions into + // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents + // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM, + // so we'll show the user how to re-arm in when + // that flag is the reason to prevent arming. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_IDLE: + // This only happens when user has chosen NONE as FS + // procedure. The recovery messages should be enough. + FALLTHROUGH; + case FAILSAFE_IDLE: + // Failsafe not active + FALLTHROUGH; + case FAILSAFE_RX_LOSS_DETECTED: + // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED + // or the FS procedure immediately. + FALLTHROUGH; + case FAILSAFE_RX_LOSS_RECOVERED: + // Exiting failsafe + break; + } + return NULL; } static const char * osdFailsafeInfoMessage(void) { - if (failsafeIsReceivingRxData()) { - // User must move sticks to exit FS mode - return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); - } - return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); + if (failsafeIsReceivingRxData()) { + // User must move sticks to exit FS mode + return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS); + } + return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { - return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); - } - return NULL; + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; } #endif static const char * navigationStateMessage(void) { - switch (NAV_Status.state) { - case MW_NAV_STATE_NONE: - break; - case MW_NAV_STATE_RTH_START: - return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); - case MW_NAV_STATE_RTH_CLIMB: - return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); - case MW_NAV_STATE_RTH_ENROUTE: - if (posControl.flags.rthTrackbackActive) { - return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK); - } else { - return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); - } - case MW_NAV_STATE_HOLD_INFINIT: - // Used by HOLD flight modes. No information to add. - break; - case MW_NAV_STATE_HOLD_TIMED: - // "HOLDING WP FOR xx S" Countdown added in osdGetSystemMessage - break; - case MW_NAV_STATE_WP_ENROUTE: - // "TO WP" + WP countdown added in osdGetSystemMessage - break; - case MW_NAV_STATE_PROCESS_NEXT: - return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP); - case MW_NAV_STATE_DO_JUMP: - // Not used - break; - case MW_NAV_STATE_LAND_START: - // Not used - break; - case MW_NAV_STATE_EMERGENCY_LANDING: - return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING); - case MW_NAV_STATE_LAND_IN_PROGRESS: - return OSD_MESSAGE_STR(OSD_MSG_LANDING); - case MW_NAV_STATE_HOVER_ABOVE_HOME: - if (STATE(FIXED_WING_LEGACY)) { + switch (NAV_Status.state) { + case MW_NAV_STATE_NONE: + break; + case MW_NAV_STATE_RTH_START: + return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); + case MW_NAV_STATE_RTH_CLIMB: + return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); + case MW_NAV_STATE_RTH_ENROUTE: + if (posControl.flags.rthTrackbackActive) { + return OSD_MESSAGE_STR(OSD_MSG_RTH_TRACKBACK); + } else { + return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); + } + case MW_NAV_STATE_HOLD_INFINIT: + // Used by HOLD flight modes. No information to add. + break; + case MW_NAV_STATE_HOLD_TIMED: + // "HOLDING WP FOR xx S" Countdown added in osdGetSystemMessage + break; + case MW_NAV_STATE_WP_ENROUTE: + // "TO WP" + WP countdown added in osdGetSystemMessage + break; + case MW_NAV_STATE_PROCESS_NEXT: + return OSD_MESSAGE_STR(OSD_MSG_PREPARE_NEXT_WP); + case MW_NAV_STATE_DO_JUMP: + // Not used + break; + case MW_NAV_STATE_LAND_START: + // Not used + break; + case MW_NAV_STATE_EMERGENCY_LANDING: + return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING); + case MW_NAV_STATE_LAND_IN_PROGRESS: + return OSD_MESSAGE_STR(OSD_MSG_LANDING); + case MW_NAV_STATE_HOVER_ABOVE_HOME: + if (STATE(FIXED_WING_LEGACY)) { #if defined(USE_SAFE_HOME) - if (safehome_applied) { - return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); - } + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); + } #endif - return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); - } - return OSD_MESSAGE_STR(OSD_MSG_HOVERING); - case MW_NAV_STATE_LANDED: - return OSD_MESSAGE_STR(OSD_MSG_LANDED); - case MW_NAV_STATE_LAND_SETTLE: - return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); - case MW_NAV_STATE_LAND_START_DESCENT: - // Not used - break; - } - return NULL; + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); + } + return OSD_MESSAGE_STR(OSD_MSG_HOVERING); + case MW_NAV_STATE_LANDED: + return OSD_MESSAGE_STR(OSD_MSG_LANDED); + case MW_NAV_STATE_LAND_SETTLE: + return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND); + case MW_NAV_STATE_LAND_START_DESCENT: + // Not used + break; + } + return NULL; } static void osdFormatMessage(char *buff, size_t size, const char *message, bool isCenteredText) { - // String is always filled with Blanks - memset(buff, SYM_BLANK, size); - if (message) { - size_t messageLength = strlen(message); - int rem = isCenteredText ? MAX(0, (int)size - (int)messageLength) : 0; - strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); - } - // Ensure buff is zero terminated - buff[size] = '\0'; + // String is always filled with Blanks + memset(buff, SYM_BLANK, size); + if (message) { + size_t messageLength = strlen(message); + int rem = isCenteredText ? MAX(0, (int)size - (int)messageLength) : 0; + strncpy(buff + rem / 2, message, MIN((int)size - rem / 2, (int)messageLength)); + } + // Ensure buff is zero terminated + buff[size] = '\0'; } /** @@ -1068,25 +1068,25 @@ static void osdFormatMessage(char *buff, size_t size, const char *message, bool **/ static void osdFormatBatteryChargeSymbol(char *buff) { - uint8_t p = calculateBatteryPercentage(); - p = (100 - p) / 16.6; - buff[0] = SYM_BATT_FULL + p; + uint8_t p = calculateBatteryPercentage(); + p = (100 - p) / 16.6; + buff[0] = SYM_BATT_FULL + p; } static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr) { - const batteryState_e batteryState = getBatteryState(); + const batteryState_e batteryState = getBatteryState(); - if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { - TEXT_ATTRIBUTES_ADD_BLINK(*attr); - } + if (batteryState == BATTERY_WARNING || batteryState == BATTERY_CRITICAL) { + TEXT_ATTRIBUTES_ADD_BLINK(*attr); + } } void osdCrosshairPosition(uint8_t *x, uint8_t *y) { - *x = osdDisplayPort->cols / 2; - *y = osdDisplayPort->rows / 2; - *y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down + *x = osdDisplayPort->cols / 2; + *y = osdDisplayPort->rows / 2; + *y -= osdConfig()->horizon_offset; // positive horizon_offset moves the HUD up, negative moves down } /** @@ -1095,13 +1095,13 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) */ 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]); + 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; + if (!usingScaledThrottle && !usingRCThrottle) + usingScaledThrottle = true; - return usingScaledThrottle; + return usingScaledThrottle; } /** @@ -1110,26 +1110,26 @@ bool osdUsingScaledThrottle(void) **/ 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 (navigationIsControllingThrottle()) { - buff[0] = SYM_AUTO_THR0; - buff[1] = SYM_AUTO_THR1; - if (isFixedWingAutoThrottleManuallyIncreased()) { - TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); - } - useScaled = true; - } + if (useScaled) { + buff[0] = SYM_SCALE; + } else { + buff[0] = SYM_BLANK; + } + buff[1] = SYM_THR; + 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); - } + if (powerLimiterIsLimiting()) { + TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); + } #endif - tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); + tfp_sprintf(buff + 2, "%3d", getThrottlePercent(useScaled)); } /** @@ -1137,110 +1137,110 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes **/ static void osdFormatGVar(char *buff, uint8_t index) { - buff[0] = 'G'; - buff[1] = '0'+index; - buff[2] = ':'; - #ifdef USE_PROGRAMMING_FRAMEWORK - osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); - #endif + buff[0] = 'G'; + buff[1] = '0'+index; + buff[2] = ':'; + #ifdef USE_PROGRAMMING_FRAMEWORK + osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); + #endif } #if defined(USE_ESC_SENSOR) static void osdFormatRpm(char *buff, uint32_t rpm) { - buff[0] = SYM_RPM; - if (rpm) { - if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { - uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); - osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); - buff[osdConfig()->esc_rpm_precision] = 'K'; - buff[osdConfig()->esc_rpm_precision+1] = '\0'; - } - else { - switch(osdConfig()->esc_rpm_precision) { - case 6: - tfp_sprintf(buff + 1, "%6lu", rpm); - break; - case 5: - tfp_sprintf(buff + 1, "%5lu", rpm); - break; - case 4: - tfp_sprintf(buff + 1, "%4lu", rpm); - break; - case 3: - default: - tfp_sprintf(buff + 1, "%3lu", rpm); - break; - } + buff[0] = SYM_RPM; + if (rpm) { + if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { + uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); + osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); + buff[osdConfig()->esc_rpm_precision] = 'K'; + buff[osdConfig()->esc_rpm_precision+1] = '\0'; + } + else { + switch(osdConfig()->esc_rpm_precision) { + case 6: + tfp_sprintf(buff + 1, "%6lu", rpm); + break; + case 5: + tfp_sprintf(buff + 1, "%5lu", rpm); + break; + case 4: + tfp_sprintf(buff + 1, "%4lu", rpm); + break; + case 3: + default: + tfp_sprintf(buff + 1, "%3lu", rpm); + break; + } - } - } - else { - uint8_t buffPos = 1; - while (buffPos <=( osdConfig()->esc_rpm_precision)) { - strcpy(buff + buffPos++, "-"); - } - } + } + } + else { + uint8_t buffPos = 1; + while (buffPos <=( osdConfig()->esc_rpm_precision)) { + strcpy(buff + buffPos++, "-"); + } + } } #endif int32_t osdGetAltitude(void) { - return getEstimatedActualPosition(Z); + return getEstimatedActualPosition(Z); } static inline int32_t osdGetAltitudeMsl(void) { - return getEstimatedActualPosition(Z)+GPS_home.alt; + return getEstimatedActualPosition(Z)+GPS_home.alt; } uint16_t osdGetRemainingGlideTime(void) { - float value = getEstimatedActualVelocity(Z); - static pt1Filter_t glideTimeFilterState; - const timeMs_t curTimeMs = millis(); - static timeMs_t glideTimeUpdatedMs; + float value = getEstimatedActualVelocity(Z); + static pt1Filter_t glideTimeFilterState; + const timeMs_t curTimeMs = millis(); + static timeMs_t glideTimeUpdatedMs; - value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); - glideTimeUpdatedMs = curTimeMs; + value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs)); + glideTimeUpdatedMs = curTimeMs; - if (value < 0) { - value = osdGetAltitude() / abs((int)value); - } else { - value = 0; - } + if (value < 0) { + value = osdGetAltitude() / abs((int)value); + } else { + value = 0; + } - return (uint16_t)roundf(value); + return (uint16_t)roundf(value); } static bool osdIsHeadingValid(void) { - return isImuHeadingValid(); + return isImuHeadingValid(); } int16_t osdGetHeading(void) { - return attitude.values.yaw; + return attitude.values.yaw; } int16_t osdGetPanServoOffset(void) { - int8_t servoIndex = osdConfig()->pan_servo_index; - int16_t servoPosition = servo[servoIndex]; - int16_t servoMiddle = servoParams(servoIndex)->middle; - return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); + int8_t servoIndex = osdConfig()->pan_servo_index; + int16_t servoPosition = servo[servoIndex]; + int16_t servoMiddle = servoParams(servoIndex)->middle; + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); } // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle) { - while (angle < 0) { - angle += 360; - } - while (angle >= 360) { - angle -= 360; - } - return angle; + while (angle < 0) { + angle += 360; + } + while (angle >= 360) { + angle -= 360; + } + return angle; } #if defined(USE_GPS) @@ -1254,141 +1254,141 @@ int osdGetHeadingAngle(int angle) * erasing all screen on each redraw. */ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t centerSym, - uint32_t poiDistance, int16_t poiDirection, uint16_t poiSymbol, - uint16_t *drawn, uint32_t *usedScale) + uint32_t poiDistance, int16_t poiDirection, uint16_t poiSymbol, + uint16_t *drawn, uint32_t *usedScale) { - // TODO: These need to be tested with several setups. We might - // need to make them configurable. - const int hMargin = 5; - const int vMargin = 3; + // TODO: These need to be tested with several setups. We might + // need to make them configurable. + const int hMargin = 5; + const int vMargin = 3; - // TODO: Get this from the display driver? - const int charWidth = 12; - const int charHeight = 18; + // TODO: Get this from the display driver? + const int charWidth = 12; + const int charHeight = 18; - uint8_t minX = hMargin; - uint8_t maxX = osdDisplayPort->cols - 1 - hMargin; - uint8_t minY = vMargin; - uint8_t maxY = osdDisplayPort->rows - 1 - vMargin; - uint8_t midX = osdDisplayPort->cols / 2; - uint8_t midY = osdDisplayPort->rows / 2; + uint8_t minX = hMargin; + uint8_t maxX = osdDisplayPort->cols - 1 - hMargin; + uint8_t minY = vMargin; + uint8_t maxY = osdDisplayPort->rows - 1 - vMargin; + uint8_t midX = osdDisplayPort->cols / 2; + uint8_t midY = osdDisplayPort->rows / 2; - // Fixed marks - displayWriteChar(osdDisplayPort, midX, midY, centerSym); + // Fixed marks + displayWriteChar(osdDisplayPort, midX, midY, centerSym); - // First, erase the previous drawing. - if (OSD_VISIBLE(*drawn)) { - displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK); - *drawn = 0; - } + // First, erase the previous drawing. + if (OSD_VISIBLE(*drawn)) { + displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK); + *drawn = 0; + } - uint32_t initialScale; - const unsigned scaleMultiplier = 2; - // We try to reduce the scale when the POI will be around half the distance - // between the center and the closers map edge, to avoid too much jumping - const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; + uint32_t initialScale; + const unsigned scaleMultiplier = 2; + // We try to reduce the scale when the POI will be around half the distance + // between the center and the closers map edge, to avoid too much jumping + const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - initialScale = 16; // 16m ~= 0.01miles - break; - case OSD_UNIT_GA: - initialScale = 18; // 18m ~= 0.01 nautical miles - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - initialScale = 10; // 10m as initial scale - break; - } + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + initialScale = 16; // 16m ~= 0.01miles + break; + case OSD_UNIT_GA: + initialScale = 18; // 18m ~= 0.01 nautical miles + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + initialScale = 10; // 10m as initial scale + break; + } - // Try to keep the same scale when getting closer until we draw over the center point - uint32_t scale = initialScale; - if (*usedScale) { - scale = *usedScale; - if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) { - scale /= scaleMultiplier; - } - } + // Try to keep the same scale when getting closer until we draw over the center point + uint32_t scale = initialScale; + if (*usedScale) { + scale = *usedScale; + if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) { + scale /= scaleMultiplier; + } + } - if (STATE(GPS_FIX)) { + if (STATE(GPS_FIX)) { - int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); - float poiAngle = DEGREES_TO_RADIANS(directionToPoi); - float poiSin = sin_approx(poiAngle); - float poiCos = cos_approx(poiAngle); + int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading); + float poiAngle = DEGREES_TO_RADIANS(directionToPoi); + float poiSin = sin_approx(poiAngle); + float poiCos = cos_approx(poiAngle); - // Now start looking for a valid scale that lets us draw everything - int ii; - for (ii = 0; ii < 50; ii++) { - // Calculate location of the aircraft in map - int points = poiDistance / ((float)scale / charHeight); + // Now start looking for a valid scale that lets us draw everything + int ii; + for (ii = 0; ii < 50; ii++) { + // Calculate location of the aircraft in map + int points = poiDistance / ((float)scale / charHeight); - float pointsX = points * poiSin; - int poiX = midX - roundf(pointsX / charWidth); - if (poiX < minX || poiX > maxX) { - scale *= scaleMultiplier; - continue; - } + float pointsX = points * poiSin; + int poiX = midX - roundf(pointsX / charWidth); + if (poiX < minX || poiX > maxX) { + scale *= scaleMultiplier; + continue; + } - float pointsY = points * poiCos; - int poiY = midY + roundf(pointsY / charHeight); - if (poiY < minY || poiY > maxY) { - scale *= scaleMultiplier; - continue; - } + float pointsY = points * poiCos; + int poiY = midY + roundf(pointsY / charHeight); + if (poiY < minY || poiY > maxY) { + scale *= scaleMultiplier; + continue; + } - if (poiX == midX && poiY == midY) { - // We're over the map center symbol, so we would be drawing - // over it even if we increased the scale. Alternate between - // drawing the center symbol or drawing the POI. - if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) { - break; - } - } else { + if (poiX == midX && poiY == midY) { + // We're over the map center symbol, so we would be drawing + // over it even if we increased the scale. Alternate between + // drawing the center symbol or drawing the POI. + if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) { + break; + } + } else { - uint16_t c; - if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { - // Something else written here, increase scale. If the display doesn't support reading - // back characters, we assume there's nothing. - // - // If we're close to the center, decrease scale. Otherwise increase it. - uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2); - uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2); - if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX && - poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY && - scale > scaleMultiplier) { + uint16_t c; + if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) { + // Something else written here, increase scale. If the display doesn't support reading + // back characters, we assume there's nothing. + // + // If we're close to the center, decrease scale. Otherwise increase it. + uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2); + uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2); + if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX && + poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY && + scale > scaleMultiplier) { - scale /= scaleMultiplier; - } else { - scale *= scaleMultiplier; - } - continue; - } - } + scale /= scaleMultiplier; + } else { + scale *= scaleMultiplier; + } + continue; + } + } - // Draw the point on the map - if (poiSymbol == SYM_ARROW_UP) { - // Drawing aircraft, rotate - int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading); - poiSymbol += mapHeading * 2 / 45; - } - displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol); + // Draw the point on the map + if (poiSymbol == SYM_ARROW_UP) { + // Drawing aircraft, rotate + int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading); + poiSymbol += mapHeading * 2 / 45; + } + displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol); - // Update saved location - *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG; - break; - } - } + // Update saved location + *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG; + break; + } + } - *usedScale = scale; + *usedScale = scale; - // Update global map data for scale and reference - osdMapData.scale = scale; - osdMapData.referenceSymbol = referenceSym; + // Update global map data for scale and reference + osdMapData.scale = scale; + osdMapData.referenceSymbol = referenceSym; } /* Draws a map with the home in the center and the craft moving around. @@ -1396,7 +1396,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen */ static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale) { - osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale); + osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale); } /* Draws a map with the aircraft in the center and the home moving around. @@ -1404,2474 +1404,2474 @@ static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t */ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) { - int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading()); - int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180); - osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); + int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading()); + int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180); + osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) { - uint8_t tmp; - tmp = data ^ (uint8_t)(crcAccum & 0xff); - tmp ^= (tmp << 4); - crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); - return crcAccum; + uint8_t tmp; + tmp = data ^ (uint8_t)(crcAccum & 0xff); + tmp ^= (tmp << 4); + crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + return crcAccum; } static void osdDisplayTelemetry(void) { - uint32_t trk_data; - uint16_t trk_crc = 0; - char trk_buffer[31]; - static int16_t trk_elevation = 127; - static uint16_t trk_bearing = 0; + uint32_t trk_data; + uint16_t trk_crc = 0; + char trk_buffer[31]; + static int16_t trk_elevation = 127; + static uint16_t trk_bearing = 0; - if (ARMING_FLAG(ARMED)) { - if (STATE(GPS_FIX)){ - if (GPS_distanceToHome > 5) { - trk_bearing = GPS_directionToHome; - trk_bearing += 360 + 180; - trk_bearing %= 360; - int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); - float at = atan2(alt, GPS_distanceToHome); - trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad - trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 - if (trk_elevation < 0) { - trk_elevation = 0; - } - } - } - } - else{ - trk_elevation = 127; - trk_bearing = 0; - } + if (ARMING_FLAG(ARMED)) { + if (STATE(GPS_FIX)){ + if (GPS_distanceToHome > 5) { + trk_bearing = GPS_directionToHome; + trk_bearing += 360 + 180; + trk_bearing %= 360; + int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); + float at = atan2(alt, GPS_distanceToHome); + trk_elevation = at * 57.2957795f; // 57.2957795 = 1 rad + trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 + if (trk_elevation < 0) { + trk_elevation = 0; + } + } + } + } + else{ + trk_elevation = 127; + trk_bearing = 0; + } - trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet - trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. - trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 - trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 - trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 - trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 - trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF + trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet + trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. + trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 + trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 + trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 + trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 + trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF - for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. - if (trk_data & (uint32_t)1 << t_ctr){ - trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; - } - else{ - trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; - } - } - trk_buffer[30] = 0; - displayWrite(osdDisplayPort, 0, 0, trk_buffer); - if (osdConfig()->telemetry>1){ - displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible - } + for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. + if (trk_data & (uint32_t)1 << t_ctr){ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; + } + else{ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; + } + } + trk_buffer[30] = 0; + displayWrite(osdDisplayPort, 0, 0, trk_buffer); + if (osdConfig()->telemetry>1){ + displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible + } } #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { - strcpy(buff, label); - for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; - uint8_t decimals = showDecimal ? 1 : 0; - osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); - buff[9] = ' '; - osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); - buff[14] = ' '; - osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); - buff[19] = ' '; - osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); - buff[24] = '\0'; + strcpy(buff, label); + for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; + uint8_t decimals = showDecimal ? 1 : 0; + osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); + buff[9] = ' '; + osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); + buff[14] = ' '; + osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); + buff[19] = ' '; + osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); + buff[24] = '\0'; } static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals) { - char buff[7]; - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + char buff[7]; + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatBatteryChargeSymbol(buff); - buff[1] = '\0'; - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + osdFormatBatteryChargeSymbol(buff); + buff[1] = '\0'; + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - elemAttr = TEXT_ATTRIBUTES_NONE; - digits = MIN(digits, 5); - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); - buff[digits] = SYM_VOLT; - buff[digits+1] = '\0'; - const batteryState_e batteryVoltageState = checkBatteryVoltageState(); - if (batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + digits = MIN(digits, 5); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + buff[digits] = SYM_VOLT; + buff[digits+1] = '\0'; + const batteryState_e batteryVoltageState = checkBatteryVoltageState(); + if (batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); } static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF) { - textAttributes_t elemAttr; - char buff[4]; + textAttributes_t elemAttr; + char buff[4]; - const pid8_t *pid = &pidBank()->pid[pidIndex]; - pidType_e pidType = pidIndexGetType(pidIndex); + const pid8_t *pid = &pidBank()->pid[pidIndex]; + pidType_e pidType = pidIndexGetType(pidIndex); - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - if (pidType == PID_TYPE_NONE) { - // PID is not used in this configuration. Draw dashes. - // XXX: Keep this in sync with the %3d format and spacing used below - displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -"); - return; - } + if (pidType == PID_TYPE_NONE) { + // PID is not used in this configuration. Draw dashes. + // XXX: Keep this in sync with the %3d format and spacing used below + displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -"); + return; + } - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); - 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->P); + 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); - 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->I); + 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); - 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->D); + 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); - 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); + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", pid->FF); + 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); } static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) { - textAttributes_t elemAttr; - char buff[4]; + textAttributes_t elemAttr; + char buff[4]; - const pid8_t *pid = &pidBank()->pid[pidIndex]; - pidType_e pidType = pidIndexGetType(pidIndex); + const pid8_t *pid = &pidBank()->pid[pidIndex]; + pidType_e pidType = pidIndexGetType(pidIndex); - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - if (pidType == PID_TYPE_NONE) { - // PID is not used in this configuration. Draw dashes. - // XXX: Keep this in sync with the %3d format and spacing used below - displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - -"); - return; - } + if (pidType == PID_TYPE_NONE) { + // PID is not used in this configuration. Draw dashes. + // XXX: Keep this in sync with the %3d format and spacing used below + displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - -"); + return; + } - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", pid->P); - 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->P); + 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); - 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->I); + 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); - 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", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); + 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); } static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, const char *str, const uint8_t valueOffset, const float value, const uint8_t valueLength, const uint8_t maxDecimals, adjustmentFunction_e adjFunc) { - char buff[8]; - textAttributes_t elemAttr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, str); + char buff[8]; + textAttributes_t elemAttr; + displayWrite(osdDisplayPort, elemPosX, elemPosY, str); - elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); - if (isAdjustmentFunctionSelected(adjFunc)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); + elemAttr = TEXT_ATTRIBUTES_NONE; + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + if (isAdjustmentFunctionSelected(adjFunc)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); } int8_t getGeoWaypointNumber(int8_t waypointIndex) { - static int8_t lastWaypointIndex = 1; - static int8_t geoWaypointIndex; + static int8_t lastWaypointIndex = 1; + static int8_t geoWaypointIndex; - if (waypointIndex != lastWaypointIndex) { - lastWaypointIndex = geoWaypointIndex = waypointIndex; - for (uint8_t i = posControl.startWpIndex; i <= waypointIndex; i++) { - if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || - posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || - posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { - geoWaypointIndex -= 1; - } - } - } + if (waypointIndex != lastWaypointIndex) { + lastWaypointIndex = geoWaypointIndex = waypointIndex; + for (uint8_t i = posControl.startWpIndex; i <= waypointIndex; i++) { + if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || + posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || + posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { + geoWaypointIndex -= 1; + } + } + } - return geoWaypointIndex - posControl.startWpIndex + 1; + return geoWaypointIndex - posControl.startWpIndex + 1; } void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { - int8_t ptr = 0; + int8_t ptr = 0; - if (osdConfig()->osd_switch_indicators_align_left) { - for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { - buff[ptr] = swName[ptr]; - } + if (osdConfig()->osd_switch_indicators_align_left) { + for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { + buff[ptr] = swName[ptr]; + } - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } - } else { - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + } else { + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } - for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { - buff[ptr] = swName[ptr-1]; - } + for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { + buff[ptr] = swName[ptr-1]; + } - ptr++; - } + ptr++; + } - buff[ptr] = '\0'; + buff[ptr] = '\0'; } static bool osdDrawSingleElement(uint8_t item) { - uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; - if (!OSD_VISIBLE(pos)) { - return false; - } - uint8_t elemPosX = OSD_X(pos); - uint8_t elemPosY = OSD_Y(pos); - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - char buff[32] = {0}; + uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; + if (!OSD_VISIBLE(pos)) { + return false; + } + uint8_t elemPosX = OSD_X(pos); + uint8_t elemPosY = OSD_Y(pos); + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + char buff[32] = {0}; - switch (item) { - case OSD_RSSI_VALUE: - { - uint16_t osdRssi = osdConvertRSSI(); - buff[0] = SYM_RSSI; - tfp_sprintf(buff + 1, "%2d", osdRssi); - if (osdRssi < osdConfig()->rssi_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + switch (item) { + case OSD_RSSI_VALUE: + { + uint16_t osdRssi = osdConvertRSSI(); + buff[0] = SYM_RSSI; + tfp_sprintf(buff + 1, "%2d", osdRssi); + if (osdRssi < osdConfig()->rssi_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; + case OSD_MAIN_BATT_VOLTAGE: { + uint8_t base_digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + return true; + } - case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { - uint8_t base_digits = 2U; + case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { + uint8_t base_digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), base_digits + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals); + return true; + } - case OSD_CURRENT_DRAW: { - osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); - buff[3] = SYM_AMP; - buff[4] = '\0'; + case OSD_CURRENT_DRAW: { + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; - uint8_t current_alarm = osdConfig()->current_alarm; - if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_MAH_DRAWN: { - uint8_t mah_digits = osdConfig()->mAh_used_precision; // Initialize to config value - bool bfcompat = false; // Assume BFCOMPAT is off + case OSD_MAH_DRAWN: { + uint8_t mah_digits = osdConfig()->mAh_used_precision; // Initialize to config value + bool bfcompat = false; // Assume BFCOMPAT is off #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if (isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + bfcompat = true; + } #endif - if (bfcompat) { - //BFcompat 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 10Ah+ packs - buff[5] = SYM_MAH; - buff[6] = '\0'; - } else { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { - // Shown in Ah - buff[mah_digits] = SYM_AH; - } else { - // Shown in mAh - buff[mah_digits] = SYM_MAH; - } - buff[mah_digits + 1] = '\0'; - } + if (bfcompat) { + //BFcompat 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 10Ah+ packs + buff[5] = SYM_MAH; + buff[6] = '\0'; + } else { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { + // Shown in Ah + buff[mah_digits] = SYM_AH; + } else { + // Shown in mAh + buff[mah_digits] = SYM_MAH; + } + buff[mah_digits + 1] = '\0'; + } - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - break; - } + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + break; + } - case OSD_WH_DRAWN: - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - buff[3] = SYM_WH; - buff[4] = '\0'; - break; + case OSD_WH_DRAWN: + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + buff[3] = SYM_WH; + buff[4] = '\0'; + break; - case OSD_BATTERY_REMAINING_CAPACITY: + case OSD_BATTERY_REMAINING_CAPACITY: - if (currentBatteryProfile->capacity.value == 0) - tfp_sprintf(buff, " NA"); - else if (!batteryWasFullWhenPluggedIn()) - tfp_sprintf(buff, " NF"); - else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); - else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH - osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + if (currentBatteryProfile->capacity.value == 0) + tfp_sprintf(buff, " NA"); + else if (!batteryWasFullWhenPluggedIn()) + tfp_sprintf(buff, " NF"); + else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) + tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); + else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH + osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); - buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; - buff[5] = '\0'; + buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[5] = '\0'; - if (batteryUsesCapacityThresholds()) { - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - } + if (batteryUsesCapacityThresholds()) { + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + } - break; + break; - case OSD_BATTERY_REMAINING_PERCENT: - osdFormatBatteryChargeSymbol(buff); - tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); - osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); - break; + case OSD_BATTERY_REMAINING_PERCENT: + osdFormatBatteryChargeSymbol(buff); + tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); + osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); + break; - case OSD_POWER_SUPPLY_IMPEDANCE: - if (isPowerSupplyImpedanceValid()) - tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); - else - strcpy(buff, "---"); - buff[3] = SYM_MILLIOHM; - buff[4] = '\0'; - break; + case OSD_POWER_SUPPLY_IMPEDANCE: + if (isPowerSupplyImpedanceValid()) + tfp_sprintf(buff, "%3d", getPowerSupplyImpedance()); + else + strcpy(buff, "---"); + buff[3] = SYM_MILLIOHM; + buff[4] = '\0'; + break; #ifdef USE_GPS - case OSD_GPS_SATS: - buff[0] = SYM_SAT_L; - buff[1] = SYM_SAT_R; - tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); - if (!STATE(GPS_FIX)) { - if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { - strcpy(buff + 2, "X!"); - } - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; + case OSD_GPS_SATS: + buff[0] = SYM_SAT_L; + buff[1] = SYM_SAT_R; + tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); + if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; - case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); - break; + case OSD_GPS_SPEED: + osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); + break; - case OSD_GPS_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_speed, false, true); - break; + case OSD_GPS_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_speed, false, true); + break; - case OSD_3D_SPEED: - osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); - break; + case OSD_3D_SPEED: + osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); + break; - case OSD_3D_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); - break; + case OSD_3D_MAX_SPEED: + osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); + break; - case OSD_GLIDESLOPE: - { - float horizontalSpeed = gpsSol.groundSpeed; - float sinkRate = -getEstimatedActualVelocity(Z); - static pt1Filter_t gsFilterState; - const timeMs_t currentTimeMs = millis(); - static timeMs_t gsUpdatedTimeMs; - float glideSlope = horizontalSpeed / sinkRate; - glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); - gsUpdatedTimeMs = currentTimeMs; + case OSD_GLIDESLOPE: + { + float horizontalSpeed = gpsSol.groundSpeed; + float sinkRate = -getEstimatedActualVelocity(Z); + static pt1Filter_t gsFilterState; + const timeMs_t currentTimeMs = millis(); + static timeMs_t gsUpdatedTimeMs; + float glideSlope = horizontalSpeed / sinkRate; + glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs)); + gsUpdatedTimeMs = currentTimeMs; - buff[0] = SYM_GLIDESLOPE; - if (glideSlope > 0.0f && glideSlope < 100.0f) { - osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = '\0'; - break; - } + buff[0] = SYM_GLIDESLOPE; + if (glideSlope > 0.0f && glideSlope < 100.0f) { + osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = '\0'; + break; + } - case OSD_GPS_LAT: - osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); - break; + case OSD_GPS_LAT: + osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat); + break; - case OSD_GPS_LON: - osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon); - break; + case OSD_GPS_LON: + osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon); + break; - case OSD_HOME_DIR: - { - if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { - if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); - } - else - { - int16_t panHomeDirOffset = 0; - if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ - panHomeDirOffset = osdGetPanServoOffset(); - } - int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); - int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset; - osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); - } - } else { - // No home or no fix or unknown heading, blink. - // If we're unarmed, show the arrow pointing up so users can see the arrow - // while configuring the OSD. If we're armed, show a '-' indicating that - // we don't know the direction to home. - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr); - } - return true; - } + case OSD_HOME_DIR: + { + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) { + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR); + } + else + { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); + } + } else { + // No home or no fix or unknown heading, blink. + // If we're unarmed, show the arrow pointing up so users can see the arrow + // while configuring the OSD. If we're armed, show a '-' indicating that + // we don't know the direction to home. + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP, elemAttr); + } + return true; + } - case OSD_HOME_HEADING_ERROR: - { - buff[0] = SYM_HOME; - buff[1] = SYM_HEADING; + case OSD_HOME_HEADING_ERROR: + { + buff[0] = SYM_HOME; + buff[1] = SYM_HEADING; - 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); - } else { - strcpy(buff + 2, "----"); - } + 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); + } else { + strcpy(buff + 2, "----"); + } - buff[6] = SYM_DEGREES; - buff[7] = '\0'; - break; - } + buff[6] = SYM_DEGREES; + buff[7] = '\0'; + break; + } - case OSD_HOME_DIST: - { - buff[0] = SYM_HOME; - uint32_t distance_to_home_cm = GPS_distanceToHome * 100; - osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); + case OSD_HOME_DIST: + { + buff[0] = SYM_HOME; + uint32_t distance_to_home_cm = GPS_distanceToHome * 100; + osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); - uint16_t dist_alarm = osdConfig()->dist_alarm; - if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + uint16_t dist_alarm = osdConfig()->dist_alarm; + if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; - case OSD_TRIP_DIST: - buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); - break; + case OSD_TRIP_DIST: + buff[0] = SYM_TOTAL; + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); + break; - case OSD_GROUND_COURSE: - { - buff[0] = SYM_GROUND_COURSE; - if (osdIsHeadingValid()) { - tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + case OSD_GROUND_COURSE: + { + buff[0] = SYM_GROUND_COURSE; + if (osdIsHeadingValid()) { + tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog)); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_COURSE_HOLD_ERROR: - { - if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } + case OSD_COURSE_HOLD_ERROR: + { + if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } - buff[0] = SYM_HEADING; + buff[0] = SYM_HEADING; - if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) { - buff[1] = buff[2] = buff[3] = '-'; - } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { - int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); - if (ABS(herr) > 99) - strcpy(buff + 1, ">99"); - else - tfp_sprintf(buff + 1, "%3d", herr); - } + if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) { + buff[1] = buff[2] = buff[3] = '-'; + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); + if (ABS(herr) > 99) + strcpy(buff + 1, ">99"); + else + tfp_sprintf(buff + 1, "%3d", herr); + } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_COURSE_HOLD_ADJUSTMENT: - { - int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); + case OSD_COURSE_HOLD_ADJUSTMENT: + { + int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); - if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } + if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } - buff[0] = SYM_HEADING; + buff[0] = SYM_HEADING; - 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); - } + 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); + } - buff[5] = SYM_DEGREES; - buff[6] = '\0'; - break; - } + buff[5] = SYM_DEGREES; + buff[6] = '\0'; + break; + } - case OSD_CROSS_TRACK_ERROR: - { - if (isWaypointNavTrackingActive()) { - buff[0] = SYM_CROSS_TRACK_ERROR; - osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); - } else { - displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); - return true; - } - break; - } + case OSD_CROSS_TRACK_ERROR: + { + if (isWaypointNavTrackingActive()) { + buff[0] = SYM_CROSS_TRACK_ERROR; + osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); + } else { + displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); + return true; + } + break; + } - case OSD_GPS_HDOP: - { - buff[0] = SYM_HDP_L; - buff[1] = SYM_HDP_R; - int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE; - uint8_t digits = 2U; + case OSD_GPS_HDOP: + { + buff[0] = SYM_HDP_L; + buff[1] = SYM_HDP_R; + int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE; + uint8_t digits = 2U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - digits = 3U; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + digits = 3U; + } #endif - osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); - break; - } + osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); + break; + } - case OSD_MAP_NORTH: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawHomeMap(0, 'N', &drawn, &scale); - return true; - } - case OSD_MAP_TAKEOFF: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale); - return true; - } - case OSD_RADAR: - { - static uint16_t drawn = 0; - static uint32_t scale = 0; - osdDrawRadar(&drawn, &scale); - return true; - } + case OSD_MAP_NORTH: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawHomeMap(0, 'N', &drawn, &scale); + return true; + } + case OSD_MAP_TAKEOFF: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale); + return true; + } + case OSD_RADAR: + { + static uint16_t drawn = 0; + static uint32_t scale = 0; + osdDrawRadar(&drawn, &scale); + return true; + } #endif // GPS - case OSD_ALTITUDE: - { - int32_t alt = osdGetAltitude(); - osdFormatAltitudeSymbol(buff, alt); + case OSD_ALTITUDE: + { + int32_t alt = osdGetAltitude(); + osdFormatAltitudeSymbol(buff, alt); - uint16_t alt_alarm = osdConfig()->alt_alarm; - uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm; - if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) || - (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) { + uint16_t alt_alarm = osdConfig()->alt_alarm; + uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm; + if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) || + (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_ALTITUDE_MSL: - { - int32_t alt = osdGetAltitudeMsl(); - osdFormatAltitudeSymbol(buff, alt); - break; - } + case OSD_ALTITUDE_MSL: + { + int32_t alt = osdGetAltitudeMsl(); + osdFormatAltitudeSymbol(buff, alt); + break; + } #ifdef USE_RANGEFINDER - case OSD_RANGEFINDER: - { - int32_t range = rangefinderGetLatestRawAltitude(); - if (range < 0) { - buff[0] = '-'; - buff[1] = '-'; - buff[2] = '-'; - } else { - osdFormatDistanceSymbol(buff, range, 1); - } - } - break; + case OSD_RANGEFINDER: + { + int32_t range = rangefinderGetLatestRawAltitude(); + if (range < 0) { + buff[0] = '-'; + buff[1] = '-'; + buff[2] = '-'; + } else { + osdFormatDistanceSymbol(buff, range, 1); + } + } + break; #endif - case OSD_ONTIME: - { - osdFormatOnTime(buff); - break; - } + case OSD_ONTIME: + { + osdFormatOnTime(buff); + break; + } - case OSD_FLYTIME: - { - osdFormatFlyTime(buff, &elemAttr); - break; - } + case OSD_FLYTIME: + { + osdFormatFlyTime(buff, &elemAttr); + break; + } - case OSD_ONTIME_FLYTIME: - { - if (ARMING_FLAG(ARMED)) { - osdFormatFlyTime(buff, &elemAttr); - } else { - osdFormatOnTime(buff); - } - break; - } + case OSD_ONTIME_FLYTIME: + { + if (ARMING_FLAG(ARMED)) { + osdFormatFlyTime(buff, &elemAttr); + } else { + osdFormatOnTime(buff); + } + break; + } - case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: - { - /*static int32_t updatedTimeSeconds = 0;*/ - static int32_t timeSeconds = -1; + case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH: + { + /*static int32_t updatedTimeSeconds = 0;*/ + static int32_t timeSeconds = -1; #if defined(USE_ADC) && defined(USE_GPS) - static timeUs_t updatedTimestamp = 0; - timeUs_t currentTimeUs = micros(); - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR - timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); + timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); #else - timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); + timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); #endif - updatedTimestamp = currentTimeUs; - } + updatedTimestamp = currentTimeUs; + } #endif - if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { - buff[0] = SYM_FLIGHT_MINS_REMAINING; - strcpy(buff + 1, "--:--"); + if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { + buff[0] = SYM_FLIGHT_MINS_REMAINING; + strcpy(buff + 1, "--:--"); #if defined(USE_ADC) && defined(USE_GPS) - updatedTimestamp = 0; + updatedTimestamp = 0; #endif - } else if (timeSeconds == -2) { - // Wind is too strong to come back with cruise throttle - buff[0] = SYM_FLIGHT_MINS_REMAINING; - buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL; - buff[3] = ':'; - buff[6] = '\0'; - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else { - osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING); - if (timeSeconds == 0) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + } else if (timeSeconds == -2) { + // Wind is too strong to come back with cruise throttle + buff[0] = SYM_FLIGHT_MINS_REMAINING; + buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL; + buff[3] = ':'; + buff[6] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatTime(buff, timeSeconds, SYM_FLIGHT_MINS_REMAINING, SYM_FLIGHT_HOURS_REMAINING); + if (timeSeconds == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; - case OSD_REMAINING_DISTANCE_BEFORE_RTH:; - static int32_t distanceMeters = -1; + case OSD_REMAINING_DISTANCE_BEFORE_RTH:; + static int32_t distanceMeters = -1; #if defined(USE_ADC) && defined(USE_GPS) - static timeUs_t updatedTimestamp = 0; - timeUs_t currentTimeUs = micros(); - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { + static timeUs_t updatedTimestamp = 0; + timeUs_t currentTimeUs = micros(); + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR - distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); + distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); #else - distanceMeters = calculateRemainingDistanceBeforeRTH(false); + distanceMeters = calculateRemainingDistanceBeforeRTH(false); #endif - updatedTimestamp = currentTimeUs; - } + updatedTimestamp = currentTimeUs; + } #endif - //buff[0] = SYM_TRIP_DIST; - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); - if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[4] = SYM_BLANK; - buff[5] = '\0'; - strcpy(buff + 1, "---"); - } else if (distanceMeters == -2) { - // Wind is too strong to come back with cruise throttle - buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; - switch ((osd_unit_e)osdConfig()->units){ - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - buff[4] = SYM_DIST_MI; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - buff[4] = SYM_DIST_KM; - break; - case OSD_UNIT_GA: - buff[4] = SYM_DIST_NM; - break; - } - buff[5] = '\0'; - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else { - osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); - if (distanceMeters == 0) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; + //buff[0] = SYM_TRIP_DIST; + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); + if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { + buff[4] = SYM_BLANK; + buff[5] = '\0'; + strcpy(buff + 1, "---"); + } else if (distanceMeters == -2) { + // Wind is too strong to come back with cruise throttle + buff[1] = buff[2] = buff[3] = SYM_WIND_HORIZONTAL; + switch ((osd_unit_e)osdConfig()->units){ + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + buff[4] = SYM_DIST_MI; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + buff[4] = SYM_DIST_KM; + break; + case OSD_UNIT_GA: + buff[4] = SYM_DIST_NM; + break; + } + buff[5] = '\0'; + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else { + osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); + if (distanceMeters == 0) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; - case OSD_FLYMODE: - { - char *p = "ACRO"; + case OSD_FLYMODE: + { + char *p = "ACRO"; - if (FLIGHT_MODE(FAILSAFE_MODE)) - p = "!FS!"; - else if (FLIGHT_MODE(MANUAL_MODE)) - p = "MANU"; - else if (FLIGHT_MODE(TURTLE_MODE)) - p = "TURT"; - else if (FLIGHT_MODE(NAV_RTH_MODE)) - p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; - else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) - p = "LOTR"; - else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) - p = "HOLD"; - else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) - p = "CRUZ"; - else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) - p = "CRSH"; - else if (FLIGHT_MODE(NAV_WP_MODE)) - 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, ACRO, etc... - // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. - p = " AH "; - } - else if (FLIGHT_MODE(ANGLE_MODE)) - p = "ANGL"; - else if (FLIGHT_MODE(HORIZON_MODE)) - p = "HOR "; + if (FLIGHT_MODE(FAILSAFE_MODE)) + p = "!FS!"; + else if (FLIGHT_MODE(MANUAL_MODE)) + p = "MANU"; + else if (FLIGHT_MODE(TURTLE_MODE)) + p = "TURT"; + else if (FLIGHT_MODE(NAV_RTH_MODE)) + p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) + p = "LOTR"; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + p = "HOLD"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) + p = "CRUZ"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + p = "CRSH"; + else if (FLIGHT_MODE(NAV_WP_MODE)) + 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, ACRO, etc... + // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE. + p = " AH "; + } + else if (FLIGHT_MODE(ANGLE_MODE)) + p = "ANGL"; + else if (FLIGHT_MODE(HORIZON_MODE)) + p = "HOR "; - displayWrite(osdDisplayPort, elemPosX, elemPosY, p); - return true; - } + displayWrite(osdDisplayPort, elemPosX, elemPosY, p); + return true; + } - case OSD_CRAFT_NAME: - osdFormatCraftName(buff); - break; + case OSD_CRAFT_NAME: + osdFormatCraftName(buff); + break; - case OSD_PILOT_NAME: - osdFormatPilotName(buff); - break; + case OSD_PILOT_NAME: + osdFormatPilotName(buff); + break; - case OSD_PILOT_LOGO: - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_PILOT_LOGO_SML_L); - displayWriteChar(osdDisplayPort, elemPosX+1, elemPosY, SYM_PILOT_LOGO_SML_C); - displayWriteChar(osdDisplayPort, elemPosX+2, elemPosY, SYM_PILOT_LOGO_SML_R); - break; + case OSD_PILOT_LOGO: + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_PILOT_LOGO_SML_L); + displayWriteChar(osdDisplayPort, elemPosX+1, elemPosY, SYM_PILOT_LOGO_SML_C); + displayWriteChar(osdDisplayPort, elemPosX+2, elemPosY, SYM_PILOT_LOGO_SML_R); + break; - case OSD_THROTTLE_POS: - { - osdFormatThrottlePosition(buff, false, &elemAttr); - break; - } + case OSD_THROTTLE_POS: + { + osdFormatThrottlePosition(buff, false, &elemAttr); + break; + } - case OSD_VTX_CHANNEL: - { - vtxDeviceOsdInfo_t osdInfo; - vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); + case OSD_VTX_CHANNEL: + { + vtxDeviceOsdInfo_t osdInfo; + vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); - if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); - return true; - } - break; + tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr); + return true; + } + break; - case OSD_VTX_POWER: - { - vtxDeviceOsdInfo_t osdInfo; - vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); + case OSD_VTX_POWER: + { + vtxDeviceOsdInfo_t osdInfo; + vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo); - tfp_sprintf(buff, "%c", SYM_VTX_POWER); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + tfp_sprintf(buff, "%c", SYM_VTX_POWER); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); - if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - return true; - } + tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter); + if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + return true; + } #if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_RSSI_DBM: - { - 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%c", rssi, SYM_DBM, ' '); - } - if (!failsafeIsReceivingRxData()){ - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } - case OSD_CRSF_LQ: - { - buff[0] = SYM_LQ; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); - } - break; - case OSD_CRSF_LQ_TYPE2: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%s:%3d", " ", 0); - } else { - tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); - } - break; - case OSD_CRSF_LQ_TYPE3: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); - } - break; - } - if (!failsafeIsReceivingRxData()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + case OSD_CRSF_RSSI_DBM: + { + 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%c", rssi, SYM_DBM, ' '); + } + if (!failsafeIsReceivingRxData()){ + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_CRSF_LQ: + { + buff[0] = SYM_LQ; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + } + break; + case OSD_CRSF_LQ_TYPE2: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%s:%3d", " ", 0); + } else { + tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); + } + break; + case OSD_CRSF_LQ_TYPE3: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + } + break; + } + if (!failsafeIsReceivingRxData()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (rxLinkStatistics.uplinkLQ < osdConfig()->link_quality_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_CRSF_SNR_DB: - { - static pt1Filter_t snrFilterState; - static timeMs_t snrUpdated = 0; - int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); - snrUpdated = millis(); + case OSD_CRSF_SNR_DB: + { + static pt1Filter_t snrFilterState; + static timeMs_t snrUpdated = 0; + int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); + snrUpdated = millis(); - const char* showsnr = "-20"; - const char* hidesnr = " "; - if (snrFiltered > osdConfig()->snr_alarm) { - if (cmsInMenu) { - buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); - } else { - buff[0] = SYM_BLANK; - tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); - } - } else if (snrFiltered <= osdConfig()->snr_alarm) { - buff[0] = SYM_SNR; - if (snrFiltered <= -10) { - tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); - } else { - tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); - } - } - break; - } + const char* showsnr = "-20"; + const char* hidesnr = " "; + if (snrFiltered > osdConfig()->snr_alarm) { + if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); + } else { + buff[0] = SYM_BLANK; + tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + } + } else if (snrFiltered <= osdConfig()->snr_alarm) { + buff[0] = SYM_SNR; + if (snrFiltered <= -10) { + tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); + } else { + tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); + } + } + break; + } - case OSD_CRSF_TX_POWER: - { - if (!failsafeIsReceivingRxData()) - tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); - else - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); - break; - } + case OSD_CRSF_TX_POWER: + { + if (!failsafeIsReceivingRxData()) + tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); + else + tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + break; + } #endif - case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair + case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair - osdCrosshairPosition(&elemPosX, &elemPosY); - osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); + osdCrosshairPosition(&elemPosX, &elemPosY); + osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY); - if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { - osdHudDrawHoming(elemPosX, elemPosY); - } + if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) { + osdHudDrawHoming(elemPosX, elemPosY); + } - if (STATE(GPS_FIX) && isImuHeadingValid()) { + if (STATE(GPS_FIX) && isImuHeadingValid()) { - if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { - osdHudClear(); - } + if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) { + osdHudClear(); + } - // -------- POI : Home point + // -------- POI : Home point - if (osdConfig()->hud_homepoint) { // Display the home point (H) - osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0); - } + if (osdConfig()->hud_homepoint) { // Display the home point (H) + osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0); + } - // -------- POI : Nearby aircrafts from ESP32 radar + // -------- POI : Nearby aircrafts from ESP32 radar - if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar - for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) { - if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped - fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE); - radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters + if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar + for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) { + if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE); + radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters - if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) { - radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In ° - radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100; - osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq); - } - } - } - } + if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) { + radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In ° + radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100; + osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq); + } + } + } + } - // -------- POI : Next waypoints from navigation + // -------- POI : Next waypoints from navigation - if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints - gpsLocation_t wp2; - int j; + if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints + gpsLocation_t wp2; + int j; - for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top - j = posControl.activeWaypointIndex + i; - if (j > posControl.startWpIndex + posControl.waypointCount - 1) { // limit to max WP index for mission - break; - } - if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { - wp2.lat = posControl.waypointList[j].lat; - wp2.lon = posControl.waypointList[j].lon; - wp2.alt = posControl.waypointList[j].alt; - fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); - int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); - j = getGeoWaypointNumber(j); - while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0) - osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i); - } - } - } - } + for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top + j = posControl.activeWaypointIndex + i; + if (j > posControl.startWpIndex + posControl.waypointCount - 1) { // limit to max WP index for mission + break; + } + if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { + wp2.lat = posControl.waypointList[j].lat; + wp2.lon = posControl.waypointList[j].lon; + wp2.alt = posControl.waypointList[j].alt; + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); + int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); + j = getGeoWaypointNumber(j); + while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more (48 = ascii 0) + osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 48 + j, i); + } + } + } + } - return true; - break; + return true; + break; - case OSD_ATTITUDE_ROLL: - buff[0] = SYM_ROLL_LEVEL; - if (ABS(attitude.values.roll) >= 1) - buff[0] += (attitude.values.roll < 0 ? -1 : 1); - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); - break; + case OSD_ATTITUDE_ROLL: + buff[0] = SYM_ROLL_LEVEL; + if (ABS(attitude.values.roll) >= 1) + buff[0] += (attitude.values.roll < 0 ? -1 : 1); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); + break; - case OSD_ATTITUDE_PITCH: - if (ABS(attitude.values.pitch) < 1) - buff[0] = 'P'; - else if (attitude.values.pitch > 0) - buff[0] = SYM_PITCH_DOWN; - else if (attitude.values.pitch < 0) - buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); - break; + case OSD_ATTITUDE_PITCH: + if (ABS(attitude.values.pitch) < 1) + buff[0] = 'P'; + else if (attitude.values.pitch > 0) + buff[0] = SYM_PITCH_DOWN; + else if (attitude.values.pitch < 0) + buff[0] = SYM_PITCH_UP; + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); + break; - case OSD_ARTIFICIAL_HORIZON: - { - float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + case OSD_ARTIFICIAL_HORIZON: + { + float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); + float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); - pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0; - pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim()); - if (osdConfig()->ahi_reverse_roll) { - rollAngle = -rollAngle; - } - osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(), - OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle); - osdDrawSingleElement(OSD_HORIZON_SIDEBARS); - osdDrawSingleElement(OSD_CROSSHAIRS); + pitchAngle -= osdConfig()->ahi_camera_uptilt_comp ? DEGREES_TO_RADIANS(osdConfig()->camera_uptilt) : 0; + pitchAngle += DEGREES_TO_RADIANS(getFixedWingLevelTrim()); + if (osdConfig()->ahi_reverse_roll) { + rollAngle = -rollAngle; + } + osdDrawArtificialHorizon(osdDisplayPort, osdGetDisplayPortCanvas(), + OSD_DRAW_POINT_GRID(elemPosX, elemPosY), rollAngle, pitchAngle); + osdDrawSingleElement(OSD_HORIZON_SIDEBARS); + osdDrawSingleElement(OSD_CROSSHAIRS); - return true; - } + return true; + } - case OSD_HORIZON_SIDEBARS: - { - osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas()); - return true; - } + case OSD_HORIZON_SIDEBARS: + { + osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas()); + return true; + } #if defined(USE_BARO) || defined(USE_GPS) - case OSD_VARIO: - { - float zvel = getEstimatedActualVelocity(Z); - osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel); - return true; - } + case OSD_VARIO: + { + float zvel = getEstimatedActualVelocity(Z); + osdDrawVario(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), zvel); + return true; + } - case OSD_VARIO_NUM: - { - int16_t value = getEstimatedActualVelocity(Z); - char sym; - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - // Convert to centifeet/s - value = CENTIMETERS_TO_CENTIFEET(value); - sym = SYM_FTS; - break; - case OSD_UNIT_GA: - // Convert to centi-100feet/min - value = CENTIMETERS_TO_FEET(value * 60); - sym = SYM_100FTM; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // Already in cm/s - sym = SYM_MS; - break; - } + case OSD_VARIO_NUM: + { + int16_t value = getEstimatedActualVelocity(Z); + char sym; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // Convert to centifeet/s + value = CENTIMETERS_TO_CENTIFEET(value); + sym = SYM_FTS; + break; + case OSD_UNIT_GA: + // Convert to centi-100feet/min + value = CENTIMETERS_TO_FEET(value * 60); + sym = SYM_100FTM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // Already in cm/s + sym = SYM_MS; + break; + } - osdFormatCentiNumber(buff, value, 0, 1, 0, 3); - buff[3] = sym; - buff[4] = '\0'; - break; - } - case OSD_CLIMB_EFFICIENCY: - { - // amperage is in centi amps (10mA), vertical speed is in cms/s. We want - // Ah/dist only to show when vertical speed > 1m/s. - static pt1Filter_t veFilterState; - static timeUs_t vEfficiencyUpdated = 0; - int32_t value = 0; - timeUs_t currentTimeUs = micros(); - timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); - if (getEstimatedActualVelocity(Z) > 0) { - if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD - value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); + osdFormatCentiNumber(buff, value, 0, 1, 0, 3); + buff[3] = sym; + buff[4] = '\0'; + break; + } + case OSD_CLIMB_EFFICIENCY: + { + // amperage is in centi amps (10mA), vertical speed is in cms/s. We want + // Ah/dist only to show when vertical speed > 1m/s. + static pt1Filter_t veFilterState; + static timeUs_t vEfficiencyUpdated = 0; + int32_t value = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated); + if (getEstimatedActualVelocity(Z) > 0) { + if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD + value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta)); - vEfficiencyUpdated = currentTimeUs; - } else { - value = veFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - // mAh/foot - if (efficiencyValid) { - osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", 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; - buff[4] = SYM_AH_V_FT_1; - buff[5] = '\0'; - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - // mAh/metre - if (efficiencyValid) { - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); - tfp_sprintf(buff, "%s%c%c", 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; - buff[4] = SYM_AH_V_M_1; - buff[5] = '\0'; - } - break; - } - break; - } - case OSD_GLIDE_TIME_REMAINING: - { - uint16_t glideTime = osdGetRemainingGlideTime(); - buff[0] = SYM_GLIDE_MINS; - if (glideTime > 0) { - // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide - // time will be longer than 99 minutes. If it is, it will show 99:^^ - if (glideTime > (99 * 60) + 59) { - tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); - buff[4] = SYM_DIRECTION; - buff[5] = SYM_DIRECTION; - } else { - tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); - } - } else { - tfp_sprintf(buff + 1, "%s", "--:--"); - } - buff[6] = '\0'; - break; - } - case OSD_GLIDE_RANGE: - { - uint16_t glideSeconds = osdGetRemainingGlideTime(); - buff[0] = SYM_GLIDE_DIST; - if (glideSeconds > 0) { - uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); - } else { - tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); - buff[5] = '\0'; - } - break; - } + vEfficiencyUpdated = currentTimeUs; + } else { + value = veFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (getEstimatedActualVelocity(Z) > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // mAh/foot + if (efficiencyValid) { + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", 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; + buff[4] = SYM_AH_V_FT_1; + buff[5] = '\0'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // mAh/metre + if (efficiencyValid) { + osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + tfp_sprintf(buff, "%s%c%c", 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; + buff[4] = SYM_AH_V_M_1; + buff[5] = '\0'; + } + break; + } + break; + } + case OSD_GLIDE_TIME_REMAINING: + { + uint16_t glideTime = osdGetRemainingGlideTime(); + buff[0] = SYM_GLIDE_MINS; + if (glideTime > 0) { + // Maximum value we can show in minutes is 99 minutes and 59 seconds. It is extremely unlikely that glide + // time will be longer than 99 minutes. If it is, it will show 99:^^ + if (glideTime > (99 * 60) + 59) { + tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); + buff[4] = SYM_DIRECTION; + buff[5] = SYM_DIRECTION; + } else { + tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); + } + } else { + tfp_sprintf(buff + 1, "%s", "--:--"); + } + buff[6] = '\0'; + break; + } + case OSD_GLIDE_RANGE: + { + uint16_t glideSeconds = osdGetRemainingGlideTime(); + buff[0] = SYM_GLIDE_DIST; + if (glideSeconds > 0) { + uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + } else { + tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); + buff[5] = '\0'; + } + break; + } #endif - case OSD_SWITCH_INDICATOR_0: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_0: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_1: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_1: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_2: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_2: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); + break; - case OSD_SWITCH_INDICATOR_3: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); - break; + case OSD_SWITCH_INDICATOR_3: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); + break; - case OSD_PAN_SERVO_CENTRED: - { - int16_t panOffset = osdGetPanServoOffset(); - const timeMs_t panServoTimeNow = millis(); - static timeMs_t panServoTimeOffCentre = 0; + case OSD_PAN_SERVO_CENTRED: + { + int16_t panOffset = osdGetPanServoOffset(); + const timeMs_t panServoTimeNow = millis(); + static timeMs_t panServoTimeOffCentre = 0; - if (panOffset < 0) { - if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset >= -osdConfig()->pan_servo_offcentre_warning) { - if (panServoTimeOffCentre == 0) { - panServoTimeOffCentre = panServoTimeNow; - } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } else { - panServoTimeOffCentre = 0; - } + if (panOffset < 0) { + if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset >= -osdConfig()->pan_servo_offcentre_warning) { + if (panServoTimeOffCentre == 0) { + panServoTimeOffCentre = panServoTimeNow; + } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } else { + panServoTimeOffCentre = 0; + } - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); - } else if (panOffset > 0) { - if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset <= osdConfig()->pan_servo_offcentre_warning) { - if (panServoTimeOffCentre == 0) { - panServoTimeOffCentre = panServoTimeNow; - } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } else { - panServoTimeOffCentre = 0; - } + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr); + } else if (panOffset > 0) { + if (osdConfig()->pan_servo_offcentre_warning != 0 && panOffset <= osdConfig()->pan_servo_offcentre_warning) { + if (panServoTimeOffCentre == 0) { + panServoTimeOffCentre = panServoTimeNow; + } else if (panServoTimeNow >= (panServoTimeOffCentre + 10000 )) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } else { + panServoTimeOffCentre = 0; + } - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); - } else { - panServoTimeOffCentre = 0; + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); + } else { + panServoTimeOffCentre = 0; - if (osdConfig()->pan_servo_indicator_show_degrees) { - tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); - displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); - } - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); - } + if (osdConfig()->pan_servo_indicator_show_degrees) { + tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); + displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); + } + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED); + } - return true; - } - break; + return true; + } + break; - case OSD_ACTIVE_PROFILE: - tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - break; + case OSD_ACTIVE_PROFILE: + tfp_sprintf(buff, "%c%u", SYM_PROFILE, (getConfigProfile() + 1)); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; - case OSD_ROLL_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); - return true; + case OSD_ROLL_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF); + return true; - case OSD_PITCH_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF); - return true; + case OSD_PITCH_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF); + return true; - case OSD_YAW_PIDS: - osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF); - return true; + case OSD_YAW_PIDS: + osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF); + return true; - case OSD_LEVEL_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); - return true; + case OSD_LEVEL_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); + return true; - case OSD_POS_XY_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); - return true; + case OSD_POS_XY_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); + return true; - case OSD_POS_Z_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); - return true; + case OSD_POS_Z_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); + return true; - case OSD_VEL_XY_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); - return true; + case OSD_VEL_XY_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); + return true; - case OSD_VEL_Z_PIDS: - osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); - return true; + case OSD_VEL_Z_PIDS: + osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); + return true; - case OSD_HEADING_P: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); - return true; + case OSD_HEADING_P: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P); + return true; - case OSD_BOARD_ALIGN_ROLL: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); - return true; + case OSD_BOARD_ALIGN_ROLL: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT); + return true; - case OSD_BOARD_ALIGN_PITCH: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); - return true; + case OSD_BOARD_ALIGN_PITCH: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT); + return true; - case OSD_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); - return true; + case OSD_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); + return true; - case OSD_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); - return true; + case OSD_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); + return true; - case OSD_THROTTLE_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); - return true; + case OSD_THROTTLE_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); + return true; - case OSD_PITCH_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); + case OSD_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_ROLL_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); + case OSD_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); - return true; + case OSD_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; - case OSD_MANUAL_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); - return true; + case OSD_MANUAL_RC_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); + return true; - case OSD_MANUAL_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); - return true; + case OSD_MANUAL_RC_YAW_EXPO: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); + return true; - case OSD_MANUAL_PITCH_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); + case OSD_MANUAL_PITCH_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_MANUAL_ROLL_RATE: - displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); + case OSD_MANUAL_ROLL_RATE: + displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); - elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); - if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); - return true; + elemAttr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); + if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); + return true; - case OSD_MANUAL_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); - return true; + case OSD_MANUAL_YAW_RATE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + return true; - case OSD_NAV_FW_CRUISE_THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); - return true; + case OSD_NAV_FW_CRUISE_THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, currentBatteryProfile->nav.fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + return true; - case OSD_NAV_FW_PITCH2THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); - return true; + case OSD_NAV_FW_PITCH2THR: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, currentBatteryProfile->nav.fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR); + return true; - case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); - return true; + case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)navConfig()->fw.minThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE); + return true; - case OSD_FW_ALT_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees - break; - } + case OSD_FW_ALT_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees + break; + } - case OSD_FW_POS_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees - osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true); - break; - } + case OSD_FW_POS_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees + osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true); + break; + } - case OSD_MC_VEL_Z_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs - break; - } + case OSD_MC_VEL_Z_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs + break; + } - case OSD_MC_VEL_X_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2 - break; - } + case OSD_MC_VEL_X_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2 + break; + } - case OSD_MC_VEL_Y_PID_OUTPUTS: - { - const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); - osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2 - break; - } + case OSD_MC_VEL_Y_PID_OUTPUTS: + { + const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); + osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2 + break; + } - case OSD_MC_POS_XYZ_P_OUTPUTS: - { - 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)); - buff[9] = ' '; - tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); - buff[14] = ' '; - tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); - buff[19] = '\0'; - break; - } + case OSD_MC_POS_XYZ_P_OUTPUTS: + { + 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)); + buff[9] = ' '; + tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100)); + buff[14] = ' '; + tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100)); + buff[19] = '\0'; + break; + } - case OSD_POWER: - { - bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; + case OSD_POWER: + { + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; - uint8_t current_alarm = osdConfig()->current_alarm; - if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + uint8_t current_alarm = osdConfig()->current_alarm; + if ((current_alarm > 0) && ((getAmperage() / 100.0f) > current_alarm)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_AIR_SPEED: - { - #ifdef USE_PITOT - buff[0] = SYM_AIR; + case OSD_AIR_SPEED: + { + #ifdef USE_PITOT + buff[0] = SYM_AIR; - if (pitotIsHealthy()) - { - const float airspeed_estimate = getAirspeedEstimate(); - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); - if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || - (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - else - { - strcpy(buff + 1, " X!"); - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - #else - return false; - #endif - break; - } + if (pitotIsHealthy()) + { + const float airspeed_estimate = getAirspeedEstimate(); + osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || + (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + else + { + strcpy(buff + 1, " X!"); + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + #else + return false; + #endif + break; + } - case OSD_AIR_MAX_SPEED: - { - #ifdef USE_PITOT - buff[0] = SYM_MAX; - buff[1] = SYM_AIR; - osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); - #else - return false; - #endif - break; - } + case OSD_AIR_MAX_SPEED: + { + #ifdef USE_PITOT + buff[0] = SYM_MAX; + buff[1] = SYM_AIR; + osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); + #else + return false; + #endif + break; + } - case OSD_RTC_TIME: - { - // RTC not configured will show 00:00 - dateTime_t dateTime; - rtcGetDateTimeLocal(&dateTime); - buff[0] = SYM_CLOCK; - tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds); - break; - } + case OSD_RTC_TIME: + { + // RTC not configured will show 00:00 + dateTime_t dateTime; + rtcGetDateTimeLocal(&dateTime); + buff[0] = SYM_CLOCK; + tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds); + break; + } - case OSD_MESSAGES: - { - elemAttr = osdGetSystemMessage(buff, OSD_MESSAGE_LENGTH, true); - break; - } + case OSD_MESSAGES: + { + elemAttr = osdGetSystemMessage(buff, OSD_MESSAGE_LENGTH, true); + break; + } - case OSD_VERSION: - { - tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - break; - } + case OSD_VERSION: + { + tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + break; + } - case OSD_MAIN_BATT_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; + case OSD_MAIN_BATT_CELL_VOLTAGE: + { + uint8_t base_digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), base_digits, 2); + return true; + } - case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: - { - uint8_t base_digits = 3U; + case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: + { + uint8_t base_digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space - } + if(isBfCompatibleVideoSystem(osdConfig())) { + base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space + } #endif - osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), base_digits, 2); - return true; - } + osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), base_digits, 2); + return true; + } - case OSD_SCALED_THROTTLE_POS: - { - osdFormatThrottlePosition(buff, true, &elemAttr); - break; - } + case OSD_SCALED_THROTTLE_POS: + { + osdFormatThrottlePosition(buff, true, &elemAttr); + break; + } - case OSD_HEADING: - { - buff[0] = SYM_HEADING; - if (osdIsHeadingValid()) { - int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); - if (h < 0) { - h += 360; - } - tfp_sprintf(&buff[1], "%3d", h); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + case OSD_HEADING: + { + buff[0] = SYM_HEADING; + if (osdIsHeadingValid()) { + int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading()); + if (h < 0) { + h += 360; + } + tfp_sprintf(&buff[1], "%3d", h); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_HEADING_GRAPH: - { - if (osdIsHeadingValid()) { - osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading()); - return true; - } else { - buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE; - buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE; - buff[OSD_HEADING_GRAPH_WIDTH] = '\0'; - } - break; - } + case OSD_HEADING_GRAPH: + { + if (osdIsHeadingValid()) { + osdDrawHeadingGraph(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), osdGetHeading()); + return true; + } else { + buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE; + buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE; + buff[OSD_HEADING_GRAPH_WIDTH] = '\0'; + } + break; + } - case OSD_EFFICIENCY_MAH_PER_KM: - { - // amperage is in centi amps, speed is in cms/s. We want - // mah/km. Only show when ground speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; - bool moreThanAh = false; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - uint8_t digits = 3U; + case OSD_EFFICIENCY_MAH_PER_KM: + { + // amperage is in centi amps, speed is in cms/s. We want + // mah/km. Only show when ground speed > 1m/s. + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + bool moreThanAh = false; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + uint8_t digits = 3U; #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber - digits = 4U; - } + if (isBfCompatibleVideoSystem(osdConfig())) { + // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber + digits = 4U; + } #endif - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, + 1, US2S(efficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; - buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode - buff[digits + 1] = SYM_MAH_MI_1; - buff[digits + 2] = '\0'; - } - break; - case OSD_UNIT_GA: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - 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'; - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - 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'; - } - break; - } - break; - } + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode + buff[digits + 1] = SYM_MAH_MI_1; + buff[digits + 2] = '\0'; + } + break; + case OSD_UNIT_GA: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + 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'; + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + 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'; + } + break; + } + break; + } - case OSD_EFFICIENCY_WH_PER_KM: - { - // amperage is in centi amps, speed is in cms/s. We want - // mWh/km. Only show when ground speed > 1m/s. - static pt1Filter_t eFilterState; - static timeUs_t efficiencyUpdated = 0; - int32_t value = 0; - timeUs_t currentTimeUs = micros(); - timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); - if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { - if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { - value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, US2S(efficiencyTimeDelta)); + case OSD_EFFICIENCY_WH_PER_KM: + { + // amperage is in centi amps, speed is in cms/s. We want + // mWh/km. Only show when ground speed > 1m/s. + static pt1Filter_t eFilterState; + static timeUs_t efficiencyUpdated = 0; + int32_t value = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); + if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { + if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { + value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, + 1, US2S(efficiencyTimeDelta)); - efficiencyUpdated = currentTimeUs; - } else { - value = eFilterState.state; - } - } - bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); - buff[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); - buff[3] = SYM_WH_NM; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); - buff[3] = SYM_WH_KM; - break; - } - buff[4] = '\0'; - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - break; - } + efficiencyUpdated = currentTimeUs; + } else { + value = eFilterState.state; + } + } + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); + buff[3] = SYM_WH_KM; + break; + } + buff[4] = '\0'; + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + break; + } - case OSD_GFORCE: - { - buff[0] = SYM_GFORCE; - osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); - if (GForce > osdConfig()->gforce_alarm * 100) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } + case OSD_GFORCE: + { + buff[0] = SYM_GFORCE; + osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); + if (GForce > osdConfig()->gforce_alarm * 100) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } - case OSD_GFORCE_X: - case OSD_GFORCE_Y: - case OSD_GFORCE_Z: - { - float GForceValue = GForceAxis[item - OSD_GFORCE_X]; - buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; - osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); - if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - break; - } - case OSD_DEBUG: - { - /* - * Longest representable string is -2147483648 does not fit in the screen. - * Only 7 digits for negative and 8 digits for positive values allowed - */ - for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { - tfp_sprintf( - buff, - "[%u]=%8ld [%u]=%8ld", - bufferIndex, - (long)constrain(debug[bufferIndex], -9999999, 99999999), - bufferIndex+1, - (long)constrain(debug[bufferIndex+1], -9999999, 99999999) - ); - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - } - break; - } + case OSD_GFORCE_X: + case OSD_GFORCE_Y: + case OSD_GFORCE_Z: + { + float GForceValue = GForceAxis[item - OSD_GFORCE_X]; + buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; + osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); + if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + case OSD_DEBUG: + { + /* + * Longest representable string is -2147483648 does not fit in the screen. + * Only 7 digits for negative and 8 digits for positive values allowed + */ + for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { + tfp_sprintf( + buff, + "[%u]=%8ld [%u]=%8ld", + bufferIndex, + (long)constrain(debug[bufferIndex], -9999999, 99999999), + bufferIndex+1, + (long)constrain(debug[bufferIndex+1], -9999999, 99999999) + ); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + } + break; + } - case OSD_IMU_TEMPERATURE: - { - int16_t temperature; - const bool valid = getIMUTemperature(&temperature); - osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); - return true; - } + case OSD_IMU_TEMPERATURE: + { + int16_t temperature; + const bool valid = getIMUTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_IMU_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } - case OSD_BARO_TEMPERATURE: - { - int16_t temperature; - const bool valid = getBaroTemperature(&temperature); - osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); - return true; - } + case OSD_BARO_TEMPERATURE: + { + int16_t temperature; + const bool valid = getBaroTemperature(&temperature); + osdDisplayTemperature(elemPosX, elemPosY, SYM_BARO_TEMP, NULL, valid, temperature, osdConfig()->imu_temp_alarm_min, osdConfig()->imu_temp_alarm_max); + return true; + } #ifdef USE_TEMPERATURE_SENSOR - case OSD_TEMP_SENSOR_0_TEMPERATURE: - case OSD_TEMP_SENSOR_1_TEMPERATURE: - case OSD_TEMP_SENSOR_2_TEMPERATURE: - case OSD_TEMP_SENSOR_3_TEMPERATURE: - case OSD_TEMP_SENSOR_4_TEMPERATURE: - case OSD_TEMP_SENSOR_5_TEMPERATURE: - case OSD_TEMP_SENSOR_6_TEMPERATURE: - case OSD_TEMP_SENSOR_7_TEMPERATURE: - { - osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE); - return true; - } + case OSD_TEMP_SENSOR_0_TEMPERATURE: + case OSD_TEMP_SENSOR_1_TEMPERATURE: + case OSD_TEMP_SENSOR_2_TEMPERATURE: + case OSD_TEMP_SENSOR_3_TEMPERATURE: + case OSD_TEMP_SENSOR_4_TEMPERATURE: + case OSD_TEMP_SENSOR_5_TEMPERATURE: + case OSD_TEMP_SENSOR_6_TEMPERATURE: + case OSD_TEMP_SENSOR_7_TEMPERATURE: + { + osdDisplayTemperatureSensor(elemPosX, elemPosY, item - OSD_TEMP_SENSOR_0_TEMPERATURE); + return true; + } #endif /* ifdef USE_TEMPERATURE_SENSOR */ - case OSD_WIND_SPEED_HORIZONTAL: + case OSD_WIND_SPEED_HORIZONTAL: #ifdef USE_WIND_ESTIMATOR - { - bool valid = isEstimatedWindSpeedValid(); - float horizontalWindSpeed; - uint16_t angle; - horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); - int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); - buff[0] = SYM_WIND_HORIZONTAL; - buff[1] = SYM_DIRECTION + (windDirection*2 / 90); - osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid); - break; - } + { + bool valid = isEstimatedWindSpeedValid(); + float horizontalWindSpeed; + uint16_t angle; + horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); + int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); + buff[0] = SYM_WIND_HORIZONTAL; + buff[1] = SYM_DIRECTION + (windDirection*2 / 90); + osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid); + break; + } #else - return false; + return false; #endif - case OSD_WIND_SPEED_VERTICAL: + case OSD_WIND_SPEED_VERTICAL: #ifdef USE_WIND_ESTIMATOR - { - buff[0] = SYM_WIND_VERTICAL; - buff[1] = SYM_BLANK; - bool valid = isEstimatedWindSpeedValid(); - float verticalWindSpeed; - verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU - if (verticalWindSpeed < 0) { - buff[1] = SYM_AH_DIRECTION_DOWN; - verticalWindSpeed = -verticalWindSpeed; - } else { - buff[1] = SYM_AH_DIRECTION_UP; - } - osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid); - break; - } + { + buff[0] = SYM_WIND_VERTICAL; + buff[1] = SYM_BLANK; + bool valid = isEstimatedWindSpeedValid(); + float verticalWindSpeed; + verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU + if (verticalWindSpeed < 0) { + buff[1] = SYM_AH_DIRECTION_DOWN; + verticalWindSpeed = -verticalWindSpeed; + } else { + buff[1] = SYM_AH_DIRECTION_UP; + } + osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid); + break; + } #else - return false; + return false; #endif - case OSD_PLUS_CODE: - { - 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)) { - olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); - } else { - // +codes with > 8 digits have a + at the 9th digit - // and we only support 10 and up. - memset(buff, '-', digits + 1); - buff[8] = '+'; - buff[digits + 1] = '\0'; - } - // Optionally trim digits from the left - memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved); - buff[digits + 1 - digitsRemoved] = '\0'; - break; - } + case OSD_PLUS_CODE: + { + 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)) { + olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, digits, buff, sizeof(buff)); + } else { + // +codes with > 8 digits have a + at the 9th digit + // and we only support 10 and up. + memset(buff, '-', digits + 1); + buff[8] = '+'; + buff[digits + 1] = '\0'; + } + // Optionally trim digits from the left + memmove(buff, buff+digitsRemoved, strlen(buff) + digitsRemoved); + buff[digits + 1 - digitsRemoved] = '\0'; + break; + } - case OSD_AZIMUTH: - { + case OSD_AZIMUTH: + { - buff[0] = SYM_AZIMUTH; - if (osdIsHeadingValid()) { - int16_t h = GPS_directionToHome; - if (h < 0) { - h += 360; - } - if (h >= 180) - h = h - 180; - else - h = h + 180; + buff[0] = SYM_AZIMUTH; + if (osdIsHeadingValid()) { + int16_t h = GPS_directionToHome; + if (h < 0) { + h += 360; + } + if (h >= 180) + h = h - 180; + else + h = h + 180; - tfp_sprintf(&buff[1], "%3d", h); - } else { - buff[1] = buff[2] = buff[3] = '-'; - } - buff[4] = SYM_DEGREES; - buff[5] = '\0'; - break; - } + tfp_sprintf(&buff[1], "%3d", h); + } else { + buff[1] = buff[2] = buff[3] = '-'; + } + buff[4] = SYM_DEGREES; + buff[5] = '\0'; + break; + } - case OSD_MAP_SCALE: - { - float scaleToUnit; - int scaleUnitDivisor; - char symUnscaled; - char symScaled; - int maxDecimals; + case OSD_MAP_SCALE: + { + float scaleToUnit; + int scaleUnitDivisor; + char symUnscaled; + char symScaled; + int maxDecimals; - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() - scaleUnitDivisor = 0; - symUnscaled = SYM_MI; - symScaled = SYM_MI; - maxDecimals = 2; - break; - case OSD_UNIT_GA: - scaleToUnit = 100 / 1852.0010f; // scale to 0.01mi for osdFormatCentiNumber() - scaleUnitDivisor = 0; - symUnscaled = SYM_NM; - symScaled = SYM_NM; - maxDecimals = 2; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() - scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m - symUnscaled = SYM_M; - symScaled = SYM_KM; - maxDecimals = 0; - break; - } - buff[0] = SYM_SCALE; - if (osdMapData.scale > 0) { - bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); - buff[4] = scaled ? symScaled : symUnscaled; - // Make sure this is cleared if the map stops being drawn - osdMapData.scale = 0; - } else { - memset(&buff[1], '-', 4); - } - buff[5] = '\0'; - break; - } - case OSD_MAP_REFERENCE: - { - char referenceSymbol; - if (osdMapData.referenceSymbol) { - referenceSymbol = osdMapData.referenceSymbol; - // Make sure this is cleared if the map stops being drawn - osdMapData.referenceSymbol = 0; - } else { - referenceSymbol = '-'; - } - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); - displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); - return true; - } + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_MI; + symScaled = SYM_MI; + maxDecimals = 2; + break; + case OSD_UNIT_GA: + scaleToUnit = 100 / 1852.0010f; // scale to 0.01mi for osdFormatCentiNumber() + scaleUnitDivisor = 0; + symUnscaled = SYM_NM; + symScaled = SYM_NM; + maxDecimals = 2; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + scaleToUnit = 100; // scale to cm for osdFormatCentiNumber() + scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m + symUnscaled = SYM_M; + symScaled = SYM_KM; + maxDecimals = 0; + break; + } + buff[0] = SYM_SCALE; + if (osdMapData.scale > 0) { + bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); + buff[4] = scaled ? symScaled : symUnscaled; + // Make sure this is cleared if the map stops being drawn + osdMapData.scale = 0; + } else { + memset(&buff[1], '-', 4); + } + buff[5] = '\0'; + break; + } + case OSD_MAP_REFERENCE: + { + char referenceSymbol; + if (osdMapData.referenceSymbol) { + referenceSymbol = osdMapData.referenceSymbol; + // Make sure this is cleared if the map stops being drawn + osdMapData.referenceSymbol = 0; + } else { + referenceSymbol = '-'; + } + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); + displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); + return true; + } - case OSD_GVAR_0: - { - osdFormatGVar(buff, 0); - break; - } - case OSD_GVAR_1: - { - osdFormatGVar(buff, 1); - break; - } - case OSD_GVAR_2: - { - osdFormatGVar(buff, 2); - break; - } - case OSD_GVAR_3: - { - osdFormatGVar(buff, 3); - break; - } + case OSD_GVAR_0: + { + osdFormatGVar(buff, 0); + break; + } + case OSD_GVAR_1: + { + osdFormatGVar(buff, 1); + break; + } + case OSD_GVAR_2: + { + osdFormatGVar(buff, 2); + break; + } + case OSD_GVAR_3: + { + osdFormatGVar(buff, 3); + break; + } #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - case OSD_RC_SOURCE: - { - const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD"; - if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr); - return true; - } + case OSD_RC_SOURCE: + { + const char *source_text = IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && !mspOverrideIsInFailsafe() ? "MSP" : "STD"; + if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && mspOverrideIsInFailsafe()) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, source_text, elemAttr); + return true; + } #endif #if defined(USE_ESC_SENSOR) - case OSD_ESC_RPM: - { - escSensorData_t * escSensor = escSensorGetData(); - if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { - osdFormatRpm(buff, escSensor->rpm); - } - else { - osdFormatRpm(buff, 0); - } - break; - } - case OSD_ESC_TEMPERATURE: - { - escSensorData_t * escSensor = escSensorGetData(); - bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE; - osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max); - return true; - } + case OSD_ESC_RPM: + { + escSensorData_t * escSensor = escSensorGetData(); + if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) { + osdFormatRpm(buff, escSensor->rpm); + } + else { + osdFormatRpm(buff, 0); + } + break; + } + case OSD_ESC_TEMPERATURE: + { + escSensorData_t * escSensor = escSensorGetData(); + bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE; + osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max); + return true; + } #endif - case OSD_TPA: - { - char buff[4]; - textAttributes_t attr; + case OSD_TPA: + { + char buff[4]; + textAttributes_t attr; - displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); - attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); - if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { - TEXT_ATTRIBUTES_ADD_BLINK(attr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 5, elemPosY, buff, attr); - displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); - attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); - if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { - TEXT_ATTRIBUTES_ADD_BLINK(attr); - } - displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); + attr = TEXT_ATTRIBUTES_NONE; + tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); + if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { + TEXT_ATTRIBUTES_ADD_BLINK(attr); + } + displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY + 1, buff, attr); - return true; - } - case OSD_TPA_TIME_CONSTANT: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); - return true; - } - case OSD_FW_LEVEL_TRIM: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, getFixedWingLevelTrim(), 3, 1, ADJUSTMENT_FW_LEVEL_TRIM); - return true; - } + return true; + } + case OSD_TPA_TIME_CONSTANT: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); + return true; + } + case OSD_FW_LEVEL_TRIM: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "LEVEL", 0, getFixedWingLevelTrim(), 3, 1, ADJUSTMENT_FW_LEVEL_TRIM); + return true; + } - case OSD_NAV_FW_CONTROL_SMOOTHNESS: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); - return true; - } + case OSD_NAV_FW_CONTROL_SMOOTHNESS: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); + return true; + } #ifdef USE_MULTI_MISSION - case OSD_NAV_WP_MULTI_MISSION_INDEX: - { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "WP NO", 0, navConfig()->general.waypoint_multi_mission_index, 1, 0, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX); - return true; - } + case OSD_NAV_WP_MULTI_MISSION_INDEX: + { + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "WP NO", 0, navConfig()->general.waypoint_multi_mission_index, 1, 0, ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX); + return true; + } #endif - case OSD_MISSION: - { - if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) { - char buf[5]; - switch (posControl.wpMissionPlannerStatus) { - case WP_PLAN_WAIT: - strcpy(buf, "WAIT"); - break; - case WP_PLAN_SAVE: - strcpy(buf, "SAVE"); - break; - case WP_PLAN_OK: - strcpy(buf, " OK "); - break; - case WP_PLAN_FULL: - strcpy(buf, "FULL"); - } - tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex); - } else if (posControl.wpPlannerActiveWPIndex){ - tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active - } + case OSD_MISSION: + { + if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) { + char buf[5]; + switch (posControl.wpMissionPlannerStatus) { + case WP_PLAN_WAIT: + strcpy(buf, "WAIT"); + break; + case WP_PLAN_SAVE: + strcpy(buf, "SAVE"); + break; + case WP_PLAN_OK: + strcpy(buf, " OK "); + break; + case WP_PLAN_FULL: + strcpy(buf, "FULL"); + } + tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex); + } else if (posControl.wpPlannerActiveWPIndex){ + tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active + } #ifdef USE_MULTI_MISSION - else { - if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ - // Limit field size when Armed, only show selected mission - tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex); - } else if (posControl.multiMissionCount) { - if (navConfig()->general.waypoint_multi_mission_index != posControl.loadedMultiMissionIndex) { - tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount); - } else { - if (posControl.waypointListValid && posControl.waypointCount > 0) { - tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - } else { - tfp_sprintf(buff, "M0/%u> 0WP", posControl.multiMissionCount); - } - } - } else { // no multi mission loaded - show active WP count from other source - tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount); - } - } + else { + if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ + // Limit field size when Armed, only show selected mission + tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex); + } else if (posControl.multiMissionCount) { + if (navConfig()->general.waypoint_multi_mission_index != posControl.loadedMultiMissionIndex) { + tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount); + } else { + if (posControl.waypointListValid && posControl.waypointCount > 0) { + tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + } else { + tfp_sprintf(buff, "M0/%u> 0WP", posControl.multiMissionCount); + } + } + } else { // no multi mission loaded - show active WP count from other source + tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount); + } + } #endif - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); - return true; - } + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + return true; + } #ifdef USE_POWER_LIMITS - case OSD_PLIMIT_REMAINING_BURST_TIME: - osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); - buff[3] = 'S'; - buff[4] = '\0'; - break; + case OSD_PLIMIT_REMAINING_BURST_TIME: + osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); + buff[3] = 'S'; + buff[4] = '\0'; + break; - case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: - if (currentBatteryProfile->powerLimits.continuousCurrent) { - osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); - buff[3] = SYM_AMP; - buff[4] = '\0'; + case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: + if (currentBatteryProfile->powerLimits.continuousCurrent) { + osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; - if (powerLimiterIsLimitingCurrent()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; + if (powerLimiterIsLimitingCurrent()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; #ifdef USE_ADC - case OSD_PLIMIT_ACTIVE_POWER_LIMIT: - { - if (currentBatteryProfile->powerLimits.continuousPower) { - bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; + case OSD_PLIMIT_ACTIVE_POWER_LIMIT: + { + if (currentBatteryProfile->powerLimits.continuousPower) { + bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; - if (powerLimiterIsLimitingPower()) { - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } - } - break; - } + if (powerLimiterIsLimitingPower()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; + } #endif // USE_ADC #endif // USE_POWER_LIMITS - default: - return false; - } + default: + return false; + } - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - return true; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + return true; } uint8_t osdIncElementIndex(uint8_t elementIndex) { - ++elementIndex; + ++elementIndex; - if (elementIndex == OSD_ARTIFICIAL_HORIZON) { // always drawn last so skip - elementIndex++; - } + if (elementIndex == OSD_ARTIFICIAL_HORIZON) { // always drawn last so skip + elementIndex++; + } #ifndef USE_TEMPERATURE_SENSOR - if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) { - elementIndex = OSD_ALTITUDE_MSL; - } + if (elementIndex == OSD_TEMP_SENSOR_0_TEMPERATURE) { + elementIndex = OSD_ALTITUDE_MSL; + } #endif - if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))) { - if (elementIndex == OSD_POWER) { - elementIndex = OSD_GPS_LON; - } - if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { - elementIndex = OSD_LEVEL_PIDS; - } + if (!(feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER))) { + if (elementIndex == OSD_POWER) { + elementIndex = OSD_GPS_LON; + } + if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) { + elementIndex = OSD_LEVEL_PIDS; + } #ifdef USE_POWER_LIMITS - if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { - elementIndex = OSD_GLIDESLOPE; - } + if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { + elementIndex = OSD_GLIDESLOPE; + } #endif - } + } #ifndef USE_POWER_LIMITS - if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { - elementIndex = OSD_GLIDESLOPE; - } + if (elementIndex == OSD_PLIMIT_REMAINING_BURST_TIME) { + elementIndex = OSD_GLIDESLOPE; + } #endif - if (!feature(FEATURE_CURRENT_METER)) { - if (elementIndex == OSD_CURRENT_DRAW) { - elementIndex = OSD_GPS_SPEED; - } - if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { - elementIndex = OSD_BATTERY_REMAINING_PERCENT; - } - if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { - elementIndex = OSD_TRIP_DIST; - } - if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) { - elementIndex = OSD_HOME_HEADING_ERROR; - } - if (elementIndex == OSD_CLIMB_EFFICIENCY) { - elementIndex = OSD_NAV_WP_MULTI_MISSION_INDEX; - } - } + if (!feature(FEATURE_CURRENT_METER)) { + if (elementIndex == OSD_CURRENT_DRAW) { + elementIndex = OSD_GPS_SPEED; + } + if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { + elementIndex = OSD_BATTERY_REMAINING_PERCENT; + } + if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { + elementIndex = OSD_TRIP_DIST; + } + if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) { + elementIndex = OSD_HOME_HEADING_ERROR; + } + if (elementIndex == OSD_CLIMB_EFFICIENCY) { + elementIndex = OSD_NAV_WP_MULTI_MISSION_INDEX; + } + } - if (!STATE(ESC_SENSOR_ENABLED)) { - if (elementIndex == OSD_ESC_RPM) { - elementIndex = OSD_AZIMUTH; - } - } + if (!STATE(ESC_SENSOR_ENABLED)) { + if (elementIndex == OSD_ESC_RPM) { + elementIndex = OSD_AZIMUTH; + } + } - if (!feature(FEATURE_GPS)) { - if (elementIndex == OSD_GPS_HDOP || elementIndex == OSD_TRIP_DIST || elementIndex == OSD_3D_SPEED || elementIndex == OSD_MISSION || - elementIndex == OSD_AZIMUTH || elementIndex == OSD_BATTERY_REMAINING_CAPACITY || elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { - elementIndex++; - } - if (elementIndex == OSD_HEADING_GRAPH && !sensors(SENSOR_MAG)) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_WH_DRAWN : OSD_BATTERY_REMAINING_PERCENT; - } - if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { - elementIndex = OSD_ATTITUDE_PITCH; - } - if (elementIndex == OSD_GPS_SPEED) { - elementIndex = OSD_ALTITUDE; - } - if (elementIndex == OSD_GPS_LON) { - elementIndex = sensors(SENSOR_MAG) ? OSD_HEADING : OSD_VARIO; - } - if (elementIndex == OSD_MAP_NORTH) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE : OSD_LEVEL_PIDS; - } - if (elementIndex == OSD_PLUS_CODE) { - elementIndex = OSD_GFORCE; - } - if (elementIndex == OSD_GLIDESLOPE) { - elementIndex = OSD_AIR_MAX_SPEED; - } - if (elementIndex == OSD_GLIDE_RANGE) { - elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; - } - if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { - elementIndex = OSD_PILOT_NAME; - } - } + if (!feature(FEATURE_GPS)) { + if (elementIndex == OSD_GPS_HDOP || elementIndex == OSD_TRIP_DIST || elementIndex == OSD_3D_SPEED || elementIndex == OSD_MISSION || + elementIndex == OSD_AZIMUTH || elementIndex == OSD_BATTERY_REMAINING_CAPACITY || elementIndex == OSD_EFFICIENCY_MAH_PER_KM) { + elementIndex++; + } + if (elementIndex == OSD_HEADING_GRAPH && !sensors(SENSOR_MAG)) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_WH_DRAWN : OSD_BATTERY_REMAINING_PERCENT; + } + if (elementIndex == OSD_EFFICIENCY_WH_PER_KM) { + elementIndex = OSD_ATTITUDE_PITCH; + } + if (elementIndex == OSD_GPS_SPEED) { + elementIndex = OSD_ALTITUDE; + } + if (elementIndex == OSD_GPS_LON) { + elementIndex = sensors(SENSOR_MAG) ? OSD_HEADING : OSD_VARIO; + } + if (elementIndex == OSD_MAP_NORTH) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE : OSD_LEVEL_PIDS; + } + if (elementIndex == OSD_PLUS_CODE) { + elementIndex = OSD_GFORCE; + } + if (elementIndex == OSD_GLIDESLOPE) { + elementIndex = OSD_AIR_MAX_SPEED; + } + if (elementIndex == OSD_GLIDE_RANGE) { + elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME; + } + if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) { + elementIndex = OSD_PILOT_NAME; + } + } - if (!sensors(SENSOR_ACC)) { - if (elementIndex == OSD_CROSSHAIRS) { - elementIndex = OSD_ONTIME; - } - if (elementIndex == OSD_GFORCE) { - elementIndex = OSD_RC_SOURCE; - } - } + if (!sensors(SENSOR_ACC)) { + if (elementIndex == OSD_CROSSHAIRS) { + elementIndex = OSD_ONTIME; + } + if (elementIndex == OSD_GFORCE) { + elementIndex = OSD_RC_SOURCE; + } + } - if (elementIndex == OSD_ITEM_COUNT) { - elementIndex = 0; - } - return elementIndex; + if (elementIndex == OSD_ITEM_COUNT) { + elementIndex = 0; + } + return elementIndex; } void osdDrawNextElement(void) { - static uint8_t elementIndex = 0; - // Flag for end of loop, also prevents infinite loop when no elements are enabled - uint8_t index = elementIndex; - do { - elementIndex = osdIncElementIndex(elementIndex); - } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); + static uint8_t elementIndex = 0; + // Flag for end of loop, also prevents infinite loop when no elements are enabled + uint8_t index = elementIndex; + do { + elementIndex = osdIncElementIndex(elementIndex); + } while (!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon + tracking telemtry last - osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); - if (osdConfig()->telemetry>0){ - osdDisplayTelemetry(); - } + // Draw artificial horizon + tracking telemtry last + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, - .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, - .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, - .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, - .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, - .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, - .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, - .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, - .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, - .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, - .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, - .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, - .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, - .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, + .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, + .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, + .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, + .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, + .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, + .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, + .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, + .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, + .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, + .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, + .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, + .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, + .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, #ifdef USE_BARO - .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, - .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, + .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, + .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif #ifdef USE_SERIALRX_CRSF - .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, - .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, - .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, - .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, - .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, - .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, + .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, + .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, + .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, + .rssi_dbm_alarm = SETTING_OSD_RSSI_DBM_ALARM_DEFAULT, + .rssi_dbm_max = SETTING_OSD_RSSI_DBM_MAX_DEFAULT, + .rssi_dbm_min = SETTING_OSD_RSSI_DBM_MIN_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR - .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, + .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, #endif #ifdef USE_PITOT - .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT, - .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, + .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT, + .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, #endif - .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, - .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, - .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT, + .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, + .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, + .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT, - .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, - .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, - .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, - .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, - .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, - .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT, - .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, - .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, - .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, - .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, - .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, - .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, - .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, - .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, - .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, - .hud_radar_alt_difference_display_time = SETTING_OSD_HUD_RADAR_ALT_DIFFERENCE_DISPLAY_TIME_DEFAULT, - .hud_radar_distance_display_time = SETTING_OSD_HUD_RADAR_DISTANCE_DISPLAY_TIME_DEFAULT, - .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, - .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, - .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, - .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, - .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT, - .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT, - .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT, - .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT, - .ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT, - .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, - .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, - .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, - .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, - .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, - .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, - .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, - .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, - .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, - .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, - .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, - .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, - .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, - .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, - .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, - .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, - .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, - .units = SETTING_OSD_UNITS_DEFAULT, - .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, - .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, - .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, - .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, + .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, + .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, + .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, + .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, + .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, + .ahi_camera_uptilt_comp = SETTING_OSD_AHI_CAMERA_UPTILT_COMP_DEFAULT, + .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, + .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, + .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, + .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, + .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, + .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, + .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, + .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, + .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, + .hud_radar_alt_difference_display_time = SETTING_OSD_HUD_RADAR_ALT_DIFFERENCE_DISPLAY_TIME_DEFAULT, + .hud_radar_distance_display_time = SETTING_OSD_HUD_RADAR_DISTANCE_DISPLAY_TIME_DEFAULT, + .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, + .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, + .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, + .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, + .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT, + .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT, + .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT, + .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT, + .ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT, + .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, + .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, + .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, + .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, + .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, + .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, + .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, + .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, + .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, + .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, + .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, + .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, + .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, + .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, + .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, + .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, + .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, + .units = SETTING_OSD_UNITS_DEFAULT, + .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, + .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, + .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, #ifdef USE_WIND_ESTIMATOR - .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, + .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, #endif - .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT, + .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT, - .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, + .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, - .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, - .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, + .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, + .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, - .ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT, - .ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT, - .ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT, - .ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT, - .ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT, + .ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT, + .ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT, + .ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT, + .ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT, + .ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT, - .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, + .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, - .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, - .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT, - .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT + .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, + .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT, + .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) { - osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG; - //line 2 - osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); - osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); - osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); - osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); - osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2); + osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG; + //line 2 + osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); + osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); + osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); + osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1); + 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_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); - osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2); - osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3); - osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); - osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); - osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7); - osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8); + osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; + 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); + osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2); + osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3); + osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); + osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6); + osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7); + osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8); - osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5); + osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5); - osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7); - osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8); + osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7); + osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8); - // avoid OSD_VARIO under OSD_CROSSHAIRS - osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5); - // OSD_VARIO_NUM at the right of OSD_VARIO - osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); - osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); - osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); - osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); + // avoid OSD_VARIO under OSD_CROSSHAIRS + osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5); + // OSD_VARIO_NUM at the right of OSD_VARIO + osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); + osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); + osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); + osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); - osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); - osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); - osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); - osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2); + osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3); + osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); + osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); #ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); + osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); + osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); + osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); #endif - osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); - osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9); - osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7); - osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6); + osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); + osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9); + osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7); + osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6); - osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10); - osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); + osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10); + osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10); - osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); - // Put this on top of the latitude, since it's very unlikely - // that users will want to use both at the same time. - osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12); - osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG; - osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12); + osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12); + // Put this on top of the latitude, since it's very unlikely + // that users will want to use both at the same time. + osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12); + osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG; + osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12); - osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); - osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11); - osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); - osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); - osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11); + osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11); + osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12); + osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12); - osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); + osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1); - osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2); - osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10); - osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11); + osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2); + osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10); + osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11); - osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); - osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); + osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6); + osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7); - osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); - osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); + osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6); + osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7); - osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_GVAR_0] = OSD_POS(1, 1); - osdLayoutsConfig->item_pos[0][OSD_GVAR_1] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); - osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_GVAR_0] = OSD_POS(1, 1); + osdLayoutsConfig->item_pos[0][OSD_GVAR_1] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3); + osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); #if defined(USE_ESC_SENSOR) - osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); - osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); + osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3); #endif #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); + osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); #endif #ifdef USE_POWER_LIMITS - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5); - osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6); #endif - // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? - osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? + osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; - for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) { - for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) { - osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG; - } - } + for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) { + for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) { + osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG; + } + } } /** @@ -3882,648 +3882,648 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) * @return uint8_t The row number after the logo(s). */ uint8_t drawLogos(bool singular, uint8_t row) { - uint8_t logoRow = row; - uint8_t logoColOffset = 0; - bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); + uint8_t logoRow = row; + uint8_t logoColOffset = 0; + bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues. - if (isBfCompatibleVideoSystem(osdConfig())) - usePilotLogo = false; + if (isBfCompatibleVideoSystem(osdConfig())) + usePilotLogo = false; #endif - uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; + uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; - if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { - logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing - } + if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) { + logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing + } - // Draw Logo(s) - if (usePilotLogo && !singular) { - logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2; - } else { - logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); - } + // Draw Logo(s) + if (usePilotLogo && !singular) { + logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2; + } else { + logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); + } - // Draw INAV logo - if ((singular && !usePilotLogo) || !singular) { - unsigned logo_c = SYM_LOGO_START; - uint8_t logo_x = logoColOffset; - for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { - for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { - displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); - } - logoRow++; - } - } + // Draw INAV logo + if ((singular && !usePilotLogo) || !singular) { + unsigned logo_c = SYM_LOGO_START; + uint8_t logo_x = logoColOffset; + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + } - // Draw the pilot logo - if (usePilotLogo) { - unsigned logo_c = SYM_PILOT_LOGO_LRG_START; - uint8_t logo_x = 0; - logoRow = row; - if (singular) { - logo_x = logoColOffset; - } else { - logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; - } - - for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { - for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { - displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); - } - logoRow++; - } - } + // Draw the pilot logo + if (usePilotLogo) { + unsigned logo_c = SYM_PILOT_LOGO_LRG_START; + uint8_t logo_x = 0; + logoRow = row; + if (singular) { + logo_x = logoColOffset; + } else { + logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; + } + + for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { + for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { + displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++); + } + logoRow++; + } + } - return logoRow; + return logoRow; } uint8_t drawStats(uint8_t row) { #ifdef USE_STATS - char string_buffer[30]; - uint8_t statNameX = (osdDisplayPort->cols - 22) / 2; - uint8_t statValueX = statNameX + 21; + char string_buffer[30]; + uint8_t statNameX = (osdDisplayPort->cols - 22) / 2; + uint8_t statValueX = statNameX + 21; - if (statsConfig()->stats_enabled) { - displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); - string_buffer[5] = SYM_MI; - break; - default: - case OSD_UNIT_GA: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); - string_buffer[5] = SYM_NM; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); - string_buffer[5] = SYM_KM; - break; - } - string_buffer[6] = '\0'; - displayWrite(osdDisplayPort, statValueX-5, row, string_buffer); + if (statsConfig()->stats_enabled) { + displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:"); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE)); + string_buffer[5] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE)); + string_buffer[5] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER)); + string_buffer[5] = SYM_KM; + break; + } + string_buffer[6] = '\0'; + displayWrite(osdDisplayPort, statValueX-5, row, string_buffer); - displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); - uint32_t tot_mins = statsConfig()->stats_total_time / 60; - tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); - displayWrite(osdDisplayPort, statValueX-7, row, string_buffer); + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:"); + uint32_t tot_mins = statsConfig()->stats_total_time / 60; + tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60)); + displayWrite(osdDisplayPort, statValueX-7, row, string_buffer); #ifdef USE_ADC - if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); - osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); - displayWrite(osdDisplayPort, statValueX-4, row, string_buffer); - displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH); + if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); + osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); + displayWrite(osdDisplayPort, statValueX-4, row, string_buffer); + displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH); - displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:"); - if (statsConfig()->stats_total_dist) { - uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_NM; - break; - default: - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); - string_buffer[3] = SYM_WH_KM; - break; - } - } else { - string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; - } - string_buffer[4] = '\0'; - displayWrite(osdDisplayPort, statValueX-3, row++, string_buffer); - } + displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:"); + if (statsConfig()->stats_total_dist) { + uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_NM; + break; + default: + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + break; + } + } else { + string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; + } + string_buffer[4] = '\0'; + displayWrite(osdDisplayPort, statValueX-3, row++, string_buffer); + } #endif // USE_ADC - } + } #endif // USE_STATS - return row; + return row; } static void osdSetNextRefreshIn(uint32_t timeMs) { - resumeRefreshAt = micros() + timeMs * 1000; - refreshWaitForResumeCmdRelease = true; + resumeRefreshAt = micros() + timeMs * 1000; + refreshWaitForResumeCmdRelease = true; } static void osdCompleteAsyncInitialization(void) { - if (!displayIsReady(osdDisplayPort)) { - // Update the display. - // XXX: Rename displayDrawScreen() and associated functions - // to displayUpdate() - displayDrawScreen(osdDisplayPort); - return; - } + if (!displayIsReady(osdDisplayPort)) { + // Update the display. + // XXX: Rename displayDrawScreen() and associated functions + // to displayUpdate() + displayDrawScreen(osdDisplayPort); + return; + } - osdDisplayIsReady = true; + osdDisplayIsReady = true; #if defined(USE_CANVAS) - if (osdConfig()->force_grid) { - osdDisplayHasCanvas = false; - } else { - osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); - } + if (osdConfig()->force_grid) { + osdDisplayHasCanvas = false; + } else { + osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort); + } #endif - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - displayClearScreen(osdDisplayPort); + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); - uint8_t y = 1; - displayFontMetadata_t metadata; - bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); - LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", - fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); + uint8_t y = 1; + displayFontMetadata_t metadata; + bool fontHasMetadata = displayGetFontMetadata(&metadata, osdDisplayPort); + LOG_DEBUG(OSD, "Font metadata version %s: %u (%u chars)", + fontHasMetadata ? "Y" : "N", metadata.version, metadata.charCount); - if (fontHasMetadata && metadata.charCount > 256) { - hasExtendedFont = true; - - y = drawLogos(false, y); - y++; - } else if (!fontHasMetadata) { - const char *m = "INVALID FONT"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); - } + if (fontHasMetadata && metadata.charCount > 256) { + hasExtendedFont = true; + + y = drawLogos(false, y); + y++; + } else if (!fontHasMetadata) { + const char *m = "INVALID FONT"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } - if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { - const char *m = "INVALID FONT VERSION"; - displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); - } + if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) { + const char *m = "INVALID FONT VERSION"; + displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m); + } - char string_buffer[30]; - tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); - uint8_t xPos = (osdDisplayPort->cols - 19) / 2; // Automatically centre, regardless of resolution. In the case of odd number screens, bias to the left. - displayWrite(osdDisplayPort, xPos, y++, string_buffer); + char string_buffer[30]; + tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING); + uint8_t xPos = (osdDisplayPort->cols - 19) / 2; // Automatically centre, regardless of resolution. In the case of odd number screens, bias to the left. + displayWrite(osdDisplayPort, xPos, y++, string_buffer); #ifdef USE_CMS - displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); - displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); + displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2); + displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3); #endif #ifdef USE_STATS - y = drawStats(++y); + y = drawStats(++y); #endif - displayCommitTransaction(osdDisplayPort); - displayResync(osdDisplayPort); - osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); + displayCommitTransaction(osdDisplayPort); + displayResync(osdDisplayPort); + osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME); } void osdInit(displayPort_t *osdDisplayPortToUse) { - if (!osdDisplayPortToUse) - return; + if (!osdDisplayPortToUse) + return; - BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); + BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63)); - osdDisplayPort = osdDisplayPortToUse; + osdDisplayPort = osdDisplayPortToUse; #ifdef USE_CMS - cmsDisplayPortRegister(osdDisplayPort); + cmsDisplayPortRegister(osdDisplayPort); #endif - armState = ARMING_FLAG(ARMED); - osdCompleteAsyncInitialization(); + armState = ARMING_FLAG(ARMED); + osdCompleteAsyncInitialization(); } static void osdResetStats(void) { - stats.max_current = 0; - stats.max_power = 0; - stats.max_speed = 0; - stats.max_3D_speed = 0; - stats.max_air_speed = 0; - stats.min_voltage = 5000; - stats.min_rssi = 99; - stats.min_lq = 300; - stats.min_rssi_dbm = 0; - stats.max_altitude = 0; + stats.max_current = 0; + stats.max_power = 0; + stats.max_speed = 0; + stats.max_3D_speed = 0; + stats.max_air_speed = 0; + stats.min_voltage = 5000; + stats.min_rssi = 99; + stats.min_lq = 300; + stats.min_rssi_dbm = 0; + stats.max_altitude = 0; } static void osdUpdateStats(void) { - int32_t value; + int32_t value; - if (feature(FEATURE_GPS)) { - value = osdGet3DSpeed(); - const float airspeed_estimate = getAirspeedEstimate(); + if (feature(FEATURE_GPS)) { + value = osdGet3DSpeed(); + const float airspeed_estimate = getAirspeedEstimate(); - if (stats.max_3D_speed < value) - stats.max_3D_speed = value; + if (stats.max_3D_speed < value) + stats.max_3D_speed = value; - if (stats.max_speed < gpsSol.groundSpeed) - stats.max_speed = gpsSol.groundSpeed; + if (stats.max_speed < gpsSol.groundSpeed) + stats.max_speed = gpsSol.groundSpeed; - if (stats.max_air_speed < airspeed_estimate) - stats.max_air_speed = airspeed_estimate; + if (stats.max_air_speed < airspeed_estimate) + stats.max_air_speed = airspeed_estimate; - if (stats.max_distance < GPS_distanceToHome) - stats.max_distance = GPS_distanceToHome; - } + if (stats.max_distance < GPS_distanceToHome) + stats.max_distance = GPS_distanceToHome; + } - value = getBatteryVoltage(); - if (stats.min_voltage > value) - stats.min_voltage = value; + value = getBatteryVoltage(); + if (stats.min_voltage > value) + stats.min_voltage = value; - value = abs(getAmperage()); - if (stats.max_current < value) - stats.max_current = value; + value = abs(getAmperage()); + if (stats.max_current < value) + stats.max_current = value; - value = labs(getPower()); - if (stats.max_power < value) - stats.max_power = value; + value = labs(getPower()); + if (stats.max_power < value) + stats.max_power = value; - value = osdConvertRSSI(); - if (stats.min_rssi > value) - stats.min_rssi = value; + value = osdConvertRSSI(); + if (stats.min_rssi > value) + stats.min_rssi = value; - value = osdGetCrsfLQ(); - if (stats.min_lq > value) - stats.min_lq = value; + value = osdGetCrsfLQ(); + if (stats.min_lq > value) + stats.min_lq = value; - if (!failsafeIsReceivingRxData()) - stats.min_lq = 0; + if (!failsafeIsReceivingRxData()) + stats.min_lq = 0; - value = osdGetCrsfdBm(); - if (stats.min_rssi_dbm > value) - stats.min_rssi_dbm = value; + value = osdGetCrsfdBm(); + if (stats.min_rssi_dbm > value) + stats.min_rssi_dbm = value; - stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); + stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); } static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) { - const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; - uint8_t top = 1; // Start one line down leaving space at the top of the screen. - size_t multiValueLengthOffset = 0; + const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; + uint8_t top = 1; // Start one line down leaving space at the top of the screen. + size_t multiValueLengthOffset = 0; - const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; - const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20; - char buff[10]; + const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; + const uint8_t statValuesX = osdDisplayIsHD() ? 30 : 20; + char buff[10]; - if (page > 1) - page = 0; + if (page > 1) + page = 0; - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - displayClearScreen(osdDisplayPort); + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); - } else if (page == 0) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); - } else if (page == 1) { - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); - } + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS ---"); + } else if (page == 0) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); + } else if (page == 1) { + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); + } - if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); - osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + if (isSinglePageStatsCompatible || page == 0) { + if (feature(FEATURE_GPS)) { + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); + osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); - osdGenerateAverageVelocityStr(buff); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "AVG SPEED :"); + osdGenerateAverageVelocityStr(buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); - osdFormatDistanceStr(buff, stats.max_distance*100); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :"); + osdFormatDistanceStr(buff, stats.max_distance*100); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); - osdFormatDistanceStr(buff, getTotalTravelDistance()); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:"); + osdFormatDistanceStr(buff, getTotalTravelDistance()); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); - osdFormatAltitudeStr(buff, stats.max_altitude); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :"); + osdFormatAltitudeStr(buff, stats.max_altitude); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: - if (isSinglePageStatsCompatible) { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); - itoa(stats.min_rssi, buff, 10); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff), "%/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: + if (isSinglePageStatsCompatible) { + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI %/DBM :"); + itoa(stats.min_rssi, buff, 10); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff), "%/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI % :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI DBM :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); - } - - if (isSinglePageStatsCompatible || page == 1) { - if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - } else { - displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); - } - tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + } + + if (isSinglePageStatsCompatible || page == 1) { + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + } + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_AMP); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + if (feature(FEATURE_CURRENT_METER)) { + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); - buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; - buff[4] = '\0'; - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); - } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - tfp_sprintf(buff, "%s%c", buff, SYM_WH); - } - displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); + } else { + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH); + } + displayWrite(osdDisplayPort, statValuesX, top++, buff); - int32_t totalDistance = getTotalTravelDistance(); - bool moreThanAh = false; - bool efficiencyValid = totalDistance >= 10000; - if (feature(FEATURE_GPS)) { - displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - uint8_t digits = 3U; // Total number of digits (including decimal point) - #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { - // Add one digit so no switch to scaled decimal occurs above 99 - digits = 4U; - } - #endif - switch (osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_MI_0; - buff[4] = SYM_MAH_MI_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_GA: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_NM_0; - buff[4] = SYM_MAH_NM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); - if (!moreThanAh) { - tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); - } else { - tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); - } - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; - } - } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); - tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); - if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = '-'; - } - } - break; - } - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - } - } + int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; + if (feature(FEATURE_GPS)) { + displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); + uint8_t digits = 3U; // Total number of digits (including decimal point) + #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values + if (isBfCompatibleVideoSystem(osdConfig())) { + // Add one digit so no switch to scaled decimal occurs above 99 + digits = 4U; + } + #endif + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_GA: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_NM_0; + buff[4] = SYM_MAH_NM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + } + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } + } - const float max_gforce = accGetMeasuredMaxG(); - displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + const float max_gforce = accGetMeasuredMaxG(); + displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); - const float acc_extremes_min = acc_extremes[Z].min; - const float acc_extremes_max = acc_extremes[Z].max; - displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); - osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); - osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); - osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); - } + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const float acc_extremes_min = acc_extremes[Z].min; + const float acc_extremes_max = acc_extremes[Z].max; + displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); + osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); + osdLeftAlignString(buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); + } - if (savingSettings == true) { - displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); - } else if (notify_settings_saved > 0) { - if (millis() > notify_settings_saved) { - notify_settings_saved = 0; - } else { - displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); - } - } + if (savingSettings == true) { + displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS)); + } else if (notify_settings_saved > 0) { + if (millis() > notify_settings_saved) { + notify_settings_saved = 0; + } else { + displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED)); + } + } - displayCommitTransaction(osdDisplayPort); + displayCommitTransaction(osdDisplayPort); } // HD arming screen. based on the minimum HD OSD grid size of 50 x 18 static void osdShowHDArmScreen(void) { dateTime_t dt; - char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[osdDisplayPort->cols]; - uint8_t safehomeRow = 0; - uint8_t armScreenRow = 2; // Start at row 2 + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; + uint8_t safehomeRow = 0; + uint8_t armScreenRow = 2; // Start at row 2 - armScreenRow = drawLogos(false, armScreenRow); - armScreenRow++; + armScreenRow = drawLogos(false, armScreenRow); + armScreenRow++; - if (strlen(systemConfig()->craftName) > 0) { - osdFormatCraftName(craftNameBuf); - strcpy(buf2, "ARMED!"); - tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); - } else { - strcpy(buf, "ARMED!"); - } - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + if (strlen(systemConfig()->craftName) > 0) { + osdFormatCraftName(craftNameBuf); + strcpy(buf2, "ARMED!"); + tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); + } else { + strcpy(buf, "ARMED!"); + } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #if defined(USE_GPS) #if defined (USE_SAFE_HOME) - if (safehome_distance) { - safehomeRow = armScreenRow; - armScreenRow++; + if (safehome_distance) { + safehomeRow = armScreenRow; + armScreenRow++; } #endif // USE_SAFE_HOME #endif // USE_GPS armScreenRow++; - if (posControl.waypointListValid && posControl.waypointCount > 0) { + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION - tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #else - strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + strcpy(buf, "*MISSION LOADED*"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #endif - } - armScreenRow++; + } + armScreenRow++; #if defined(USE_GPS) - if (feature(FEATURE_GPS)) { - if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); - uint8_t gap = 1; - uint8_t col = strlen(buf) + strlen(buf2) + gap; + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + if (osdConfig()->osd_home_position_arm_screen) { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + uint8_t gap = 1; + uint8_t col = strlen(buf) + strlen(buf2) + gap; - if ((osdDisplayPort->cols %2) != (col %2)) { - gap++; - col++; - } + if ((osdDisplayPort->cols %2) != (col %2)) { + gap++; + col++; + } - col = (osdDisplayPort->cols - col) / 2; + col = (osdDisplayPort->cols - col) / 2; - displayWrite(osdDisplayPort, col, armScreenRow, buf); - displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); - - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - } - + displayWrite(osdDisplayPort, col, armScreenRow, buf); + displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } + #if defined (USE_SAFE_HOME) - if (safehome_distance) { // safehome found during arming - if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { - strcpy(buf, "SAFEHOME FOUND; MODE OFF"); - } else { - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); - } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message below the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); - } + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + osdFormatDistanceStr(buf2, safehome_distance); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + } #endif - } else { - strcpy(buf, "!NO HOME POSITION!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - } + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } armScreenRow++; - } + } #endif - if (rtcGetDateTimeLocal(&dt)) { - tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - armScreenRow++; - } + if (rtcGetDateTimeLocal(&dt)) { + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; + } - tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); armScreenRow++; #ifdef USE_STATS @@ -4535,80 +4535,80 @@ static void osdShowHDArmScreen(void) static void osdShowSDArmScreen(void) { dateTime_t dt; - char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; + char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; - char versionBuf[osdDisplayPort->cols]; - // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + char craftNameBuf[MAX_NAME_LENGTH]; + char versionBuf[osdDisplayPort->cols]; + // We need 12 visible rows, start row never < first fully visible row 1 + uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; uint8_t safehomeRow = 0; - strcpy(buf, "ARMED!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + strcpy(buf, "ARMED!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); safehomeRow = armScreenRow; - armScreenRow++; + armScreenRow++; - if (strlen(systemConfig()->craftName) > 0) { - osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); - } + if (strlen(systemConfig()->craftName) > 0) { + osdFormatCraftName(craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + } - if (posControl.waypointListValid && posControl.waypointCount > 0) { + if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION - tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); #else - strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + strcpy(buf, "*MISSION LOADED*"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); #endif - } - armScreenRow++; + } + armScreenRow++; #if defined(USE_GPS) - if (feature(FEATURE_GPS)) { - if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + if (feature(FEATURE_GPS)) { + if (STATE(GPS_FIX_HOME)) { + if (osdConfig()->osd_home_position_arm_screen) { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; - displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); - displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); - + displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); + displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); + int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - } + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } #if defined (USE_SAFE_HOME) - if (safehome_distance) { // safehome found during arming - if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { - strcpy(buf, "SAFEHOME FOUND; MODE OFF"); - } else { - osdFormatDistanceStr(buf2, safehome_distance); + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + osdFormatDistanceStr(buf2, safehome_distance); tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); - } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - // write this message below the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); - } + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message below the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + } #endif - } else { - strcpy(buf, "!NO HOME POSITION!"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - } + } else { + strcpy(buf, "!NO HOME POSITION!"); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + } armScreenRow++; - } + } #endif if (rtcGetDateTimeLocal(&dt)) { - tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - armScreenRow++; - } + tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + armScreenRow++; + } - tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); + tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf); armScreenRow++; #ifdef USE_STATS @@ -4620,220 +4620,220 @@ static void osdShowSDArmScreen(void) // called when motors armed static void osdShowArmed(void) { - displayClearScreen(osdDisplayPort); + displayClearScreen(osdDisplayPort); - if (osdDisplayIsHD()) { - osdShowHDArmScreen(); - } else { - osdShowSDArmScreen(); - } + if (osdDisplayIsHD()) { + osdShowHDArmScreen(); + } else { + osdShowSDArmScreen(); + } } static void osdFilterData(timeUs_t currentTimeUs) { - static timeUs_t lastRefresh = 0; - float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); + static timeUs_t lastRefresh = 0; + float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); - GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; + GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; - if (lastRefresh) { - GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); - } else { - pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); - pt1FilterReset(&GForceFilter, GForce); + if (lastRefresh) { + GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT); + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT); + } else { + pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0); + pt1FilterReset(&GForceFilter, GForce); - for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { - pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); - pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); - } - } + for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) { + pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0); + pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]); + } + } - lastRefresh = currentTimeUs; + lastRefresh = currentTimeUs; } // Detect when the user is holding the roll stick to the right static bool osdIsPageUpStickCommandHeld(void) { - static int pageUpHoldCount = 1; + static int pageUpHoldCount = 1; - bool keyHeld = false; + bool keyHeld = false; - if (IS_HI(ROLL)) { - keyHeld = true; - } + if (IS_HI(ROLL)) { + keyHeld = true; + } - if (!keyHeld) { - pageUpHoldCount = 1; - } else { - ++pageUpHoldCount; - } + if (!keyHeld) { + pageUpHoldCount = 1; + } else { + ++pageUpHoldCount; + } - if (pageUpHoldCount > 20) { - pageUpHoldCount = 1; - return true; - } + if (pageUpHoldCount > 20) { + pageUpHoldCount = 1; + return true; + } - return false; + return false; } // Detect when the user is holding the roll stick to the left static bool osdIsPageDownStickCommandHeld(void) { - static int pageDownHoldCount = 1; + static int pageDownHoldCount = 1; - bool keyHeld = false; - if (IS_LO(ROLL)) { - keyHeld = true; - } + bool keyHeld = false; + if (IS_LO(ROLL)) { + keyHeld = true; + } - if (!keyHeld) { - pageDownHoldCount = 1; - } else { - ++pageDownHoldCount; - } + if (!keyHeld) { + pageDownHoldCount = 1; + } else { + ++pageDownHoldCount; + } - if (pageDownHoldCount > 20) { - pageDownHoldCount = 1; - return true; - } + if (pageDownHoldCount > 20) { + pageDownHoldCount = 1; + return true; + } - return false; + return false; } static void osdRefresh(timeUs_t currentTimeUs) { - osdFilterData(currentTimeUs); + osdFilterData(currentTimeUs); #ifdef USE_CMS - if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #else - if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { + if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) { #endif - displayClearScreen(osdDisplayPort); - armState = ARMING_FLAG(ARMED); - return; - } + displayClearScreen(osdDisplayPort); + armState = ARMING_FLAG(ARMED); + return; + } - bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); - static uint8_t statsCurrentPage = 0; - static bool statsDisplayed = false; - static bool statsAutoPagingEnabled = true; + bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS); + static uint8_t statsCurrentPage = 0; + static bool statsDisplayed = false; + static bool statsAutoPagingEnabled = true; - // Detect arm/disarm - if (armState != ARMING_FLAG(ARMED)) { - if (ARMING_FLAG(ARMED)) { - // Display the "Arming" screen - statsDisplayed = false; - osdResetStats(); - osdShowArmed(); - uint32_t delay = osdConfig()->arm_screen_display_time; + // Detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) { + // Display the "Arming" screen + statsDisplayed = false; + osdResetStats(); + osdShowArmed(); + uint32_t delay = osdConfig()->arm_screen_display_time; #if defined(USE_SAFE_HOME) - if (safehome_distance) - delay+= 3000; + if (safehome_distance) + delay+= 3000; #endif - osdSetNextRefreshIn(delay); - } else { - // Display the "Stats" screen - statsDisplayed = true; - statsCurrentPage = 0; - statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); - } + osdSetNextRefreshIn(delay); + } else { + // Display the "Stats" screen + statsDisplayed = true; + statsCurrentPage = 0; + statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); + } - armState = ARMING_FLAG(ARMED); - } + armState = ARMING_FLAG(ARMED); + } - // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens - if (resumeRefreshAt) { - - // Handle events only when the "Stats" screen is being displayed. - if (statsDisplayed) { + // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens + if (resumeRefreshAt) { + + // Handle events only when the "Stats" screen is being displayed. + if (statsDisplayed) { - // Manual paging stick commands are only applicable to multi-page stats. - // ****************************** - // 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. - // ****************************** - // 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. - bool manualPageUpRequested = 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()) { - manualPageUpRequested = true; - statsAutoPagingEnabled = false; - } else if (osdIsPageDownStickCommandHeld()) { - manualPageDownRequested = true; - statsAutoPagingEnabled = false; - } - } + // Manual paging stick commands are only applicable to multi-page stats. + // ****************************** + // 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. + // ****************************** + // 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. + bool manualPageUpRequested = 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()) { + manualPageUpRequested = true; + statsAutoPagingEnabled = false; + } else if (osdIsPageDownStickCommandHeld()) { + manualPageDownRequested = true; + statsAutoPagingEnabled = false; + } + } - if (statsAutoPagingEnabled) { - // Alternate screens for multi-page stats. - // Also, refreshes screen at swap interval for single-page stats. - if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { - if (statsCurrentPage == 0) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - statsCurrentPage = 1; - } - } else { - if (statsCurrentPage == 1) { - osdShowStats(statsSinglePageCompatible, statsCurrentPage); - statsCurrentPage = 0; - } - } - } else { - // Process manual page change events for multi-page stats. - if (manualPageUpRequested) { - osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; - } else if (manualPageDownRequested) { - osdShowStats(statsSinglePageCompatible, 0); - statsCurrentPage = 0; - } - } - } + if (statsAutoPagingEnabled) { + // Alternate screens for multi-page stats. + // Also, refreshes screen at swap interval for single-page stats. + if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) { + if (statsCurrentPage == 0) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 1; + } + } else { + if (statsCurrentPage == 1) { + osdShowStats(statsSinglePageCompatible, statsCurrentPage); + statsCurrentPage = 0; + } + } + } else { + // Process manual page change events for multi-page stats. + if (manualPageUpRequested) { + osdShowStats(statsSinglePageCompatible, 1); + statsCurrentPage = 1; + } else if (manualPageDownRequested) { + osdShowStats(statsSinglePageCompatible, 0); + statsCurrentPage = 0; + } + } + } - // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. - if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { - // Time elapsed or canceled by stick commands. - // Exit to normal OSD operation. - displayClearScreen(osdDisplayPort); - resumeRefreshAt = 0; - statsDisplayed = false; - } else { - // Continue "Splash", "Armed" or "Stats" screens. - displayHeartbeat(osdDisplayPort); - } - - return; - } + // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. + if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { + // Time elapsed or canceled by stick commands. + // Exit to normal OSD operation. + displayClearScreen(osdDisplayPort); + resumeRefreshAt = 0; + statsDisplayed = false; + } else { + // Continue "Splash", "Armed" or "Stats" screens. + displayHeartbeat(osdDisplayPort); + } + + return; + } #ifdef USE_CMS - if (!displayIsGrabbed(osdDisplayPort)) { - displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); - if (fullRedraw) { - displayClearScreen(osdDisplayPort); - fullRedraw = false; - } - osdDrawNextElement(); - displayHeartbeat(osdDisplayPort); - displayCommitTransaction(osdDisplayPort); + if (!displayIsGrabbed(osdDisplayPort)) { + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + if (fullRedraw) { + displayClearScreen(osdDisplayPort); + fullRedraw = false; + } + osdDrawNextElement(); + displayHeartbeat(osdDisplayPort); + displayCommitTransaction(osdDisplayPort); #ifdef OSD_CALLS_CMS - } else { - cmsUpdate(currentTimeUs); + } else { + cmsUpdate(currentTimeUs); #endif - } + } #endif } @@ -4842,318 +4842,318 @@ static void osdRefresh(timeUs_t currentTimeUs) */ void osdUpdate(timeUs_t currentTimeUs) { - static uint32_t counter = 0; + static uint32_t counter = 0; - // don't touch buffers if DMA transaction is in progress - if (displayIsTransferInProgress(osdDisplayPort)) { - return; - } + // don't touch buffers if DMA transaction is in progress + if (displayIsTransferInProgress(osdDisplayPort)) { + return; + } - if (!osdDisplayIsReady) { - osdCompleteAsyncInitialization(); - return; - } + if (!osdDisplayIsReady) { + osdCompleteAsyncInitialization(); + return; + } #if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0 - // Check if the layout has changed. Higher numbered - // boxes take priority. - unsigned activeLayout; - if (layoutOverride >= 0) { - activeLayout = layoutOverride; - // Check for timed override, it will go into effect on - // the next OSD iteration - if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) { - layoutOverrideUntil = 0; - layoutOverride = -1; - } - } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) { - activeLayout = 0; - } else { + // Check if the layout has changed. Higher numbered + // boxes take priority. + unsigned activeLayout; + if (layoutOverride >= 0) { + activeLayout = layoutOverride; + // Check for timed override, it will go into effect on + // the next OSD iteration + if (layoutOverrideUntil > 0 && millis() > layoutOverrideUntil) { + layoutOverrideUntil = 0; + layoutOverride = -1; + } + } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) { + activeLayout = 0; + } else { #if OSD_ALTERNATE_LAYOUT_COUNT > 2 - if (IS_RC_MODE_ACTIVE(BOXOSDALT3)) - activeLayout = 3; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT3)) + activeLayout = 3; + else #endif #if OSD_ALTERNATE_LAYOUT_COUNT > 1 - if (IS_RC_MODE_ACTIVE(BOXOSDALT2)) - activeLayout = 2; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT2)) + activeLayout = 2; + else #endif - if (IS_RC_MODE_ACTIVE(BOXOSDALT1)) - activeLayout = 1; - else + if (IS_RC_MODE_ACTIVE(BOXOSDALT1)) + activeLayout = 1; + else #ifdef USE_PROGRAMMING_FRAMEWORK - if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT)) - activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); - else + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT)) + activeLayout = constrain(logicConditionValuesByType[LOGIC_CONDITION_SET_OSD_LAYOUT], 0, OSD_ALTERNATE_LAYOUT_COUNT); + else #endif - activeLayout = 0; - } - if (currentLayout != activeLayout) { - currentLayout = activeLayout; - osdStartFullRedraw(); - } + activeLayout = 0; + } + if (currentLayout != activeLayout) { + currentLayout = activeLayout; + osdStartFullRedraw(); + } #endif #define DRAW_FREQ_DENOM 4 #define STATS_FREQ_DENOM 50 - counter++; + counter++; - if ((counter % STATS_FREQ_DENOM) == 0) { - osdUpdateStats(); - } + if ((counter % STATS_FREQ_DENOM) == 0) { + osdUpdateStats(); + } - if ((counter % DRAW_FREQ_DENOM) == 0) { - // redraw values in buffer - osdRefresh(currentTimeUs); - } else { - // rest of time redraw screen - displayDrawScreen(osdDisplayPort); - } + if ((counter % DRAW_FREQ_DENOM) == 0) { + // redraw values in buffer + osdRefresh(currentTimeUs); + } else { + // rest of time redraw screen + displayDrawScreen(osdDisplayPort); + } #ifdef USE_CMS - // do not allow ARM if we are in menu - if (displayIsGrabbed(osdDisplayPort)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); - } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); - } + // do not allow ARM if we are in menu + if (displayIsGrabbed(osdDisplayPort)) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU); + } #endif } void osdStartFullRedraw(void) { - fullRedraw = true; + fullRedraw = true; } void osdOverrideLayout(int layout, timeMs_t duration) { - layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1); - if (layoutOverride >= 0 && duration > 0) { - layoutOverrideUntil = millis() + duration; - } else { - layoutOverrideUntil = 0; - } + layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1); + if (layoutOverride >= 0 && duration > 0) { + layoutOverrideUntil = millis() + duration; + } else { + layoutOverrideUntil = 0; + } } int osdGetActiveLayout(bool *overridden) { - if (overridden) { - *overridden = layoutOverride >= 0; - } - return currentLayout; + if (overridden) { + *overridden = layoutOverride >= 0; + } + return currentLayout; } bool osdItemIsFixed(osd_items_e item) { - return item == OSD_CROSSHAIRS || - item == OSD_ARTIFICIAL_HORIZON || - item == OSD_HORIZON_SIDEBARS; + return item == OSD_CROSSHAIRS || + item == OSD_ARTIFICIAL_HORIZON || + item == OSD_HORIZON_SIDEBARS; } displayPort_t *osdGetDisplayPort(void) { - return osdDisplayPort; + return osdDisplayPort; } displayCanvas_t *osdGetDisplayPortCanvas(void) { #if defined(USE_CANVAS) - if (osdDisplayHasCanvas) { - return &osdCanvas; - } + if (osdDisplayHasCanvas) { + return &osdCanvas; + } #endif - return NULL; + return NULL; } timeMs_t systemMessageCycleTime(unsigned messageCount, const char **messages){ - uint8_t i = 0; - float factor = 1.0f; - while (i < messageCount) { - if ((float)strlen(messages[i]) / 15.0f > factor) { - factor = (float)strlen(messages[i]) / 15.0f; - } - i++; - } - return osdConfig()->system_msg_display_time * factor; + uint8_t i = 0; + float factor = 1.0f; + while (i < messageCount) { + if ((float)strlen(messages[i]) / 15.0f > factor) { + factor = (float)strlen(messages[i]) / 15.0f; + } + i++; + } + return osdConfig()->system_msg_display_time * factor; } textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenteredText) { - textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; + textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE; - if (buff != NULL) { - const char *message = NULL; - char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; - // We might have up to 5 messages to show. - const char *messages[5]; - unsigned messageCount = 0; - const char *failsafeInfoMessage = NULL; - const char *invertedInfoMessage = NULL; + if (buff != NULL) { + const char *message = NULL; + char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; + // We might have up to 5 messages to show. + const char *messages[5]; + unsigned messageCount = 0; + const char *failsafeInfoMessage = NULL; + const char *invertedInfoMessage = NULL; - if (ARMING_FLAG(ARMED)) { - if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - 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); - } - 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) { - // Countdown display for remaining Waypoints - char buf[6]; - osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); - tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); - messages[messageCount++] = messageBuf; - } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { - if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); - } else { - // WP hold time countdown in seconds - timeMs_t currentTime = millis(); - int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); - holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; + if (ARMING_FLAG(ARMED)) { + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + 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); + } + 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) { + // Countdown display for remaining Waypoints + char buf[6]; + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); + messages[messageCount++] = messageBuf; + } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { + if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT); + } else { + // WP hold time countdown in seconds + timeMs_t currentTime = millis(); + int holdTimeRemaining = posControl.waypointList[posControl.activeWaypointIndex].p1 - (int)(MS2S(currentTime - posControl.wpReachedTime)); + holdTimeRemaining = holdTimeRemaining >= 0 ? holdTimeRemaining : 0; - tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); + tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); - messages[messageCount++] = messageBuf; - } - } else { - const char *navStateMessage = navigationStateMessage(); - if (navStateMessage) { - messages[messageCount++] = navStateMessage; - } - } + messages[messageCount++] = messageBuf; + } + } else { + const char *navStateMessage = navigationStateMessage(); + if (navStateMessage) { + messages[messageCount++] = navStateMessage; + } + } #if defined(USE_SAFE_HOME) - const char *safehomeMessage = divertingToSafehomeMessage(); - if (safehomeMessage) { - messages[messageCount++] = safehomeMessage; - } + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } #endif - if (FLIGHT_MODE(FAILSAFE_MODE)) { - // In FS mode while being armed too - const char *failsafePhaseMessage = osdFailsafePhaseMessage(); - failsafeInfoMessage = osdFailsafeInfoMessage(); + if (FLIGHT_MODE(FAILSAFE_MODE)) { + // In FS mode while being armed too + const char *failsafePhaseMessage = osdFailsafePhaseMessage(); + failsafeInfoMessage = osdFailsafeInfoMessage(); - if (failsafePhaseMessage) { - messages[messageCount++] = failsafePhaseMessage; - } - if (failsafeInfoMessage) { - messages[messageCount++] = failsafeInfoMessage; - } - } - } 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/ACRO - // when it doesn't require ANGLE mode (required only in FW - // right now). If if requires ANGLE, its display is handled - // by OSD_FLYMODE. - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); - } - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); - if (FLIGHT_MODE(MANUAL_MODE)) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); - } - } - if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) { - 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); - } - } - } - } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { - unsigned invalidIndex; + if (failsafePhaseMessage) { + messages[messageCount++] = failsafePhaseMessage; + } + if (failsafeInfoMessage) { + messages[messageCount++] = failsafeInfoMessage; + } + } + } 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/ACRO + // when it doesn't require ANGLE mode (required only in FW + // right now). If if requires ANGLE, its display is handled + // by OSD_FLYMODE. + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); + } + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE); + if (FLIGHT_MODE(MANUAL_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); + } + } + if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) { + 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); + } + } + } + } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) { + unsigned invalidIndex; - // Check if we're unable to arm for some reason - if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&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 { + } else { - invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); - messages[messageCount++] = invertedInfoMessage; + invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM); + messages[messageCount++] = invertedInfoMessage; - // Show the reason for not arming - messages[messageCount++] = osdArmingDisabledReasonMessage(); + // Show the reason for not arming + messages[messageCount++] = osdArmingDisabledReasonMessage(); - } - } else if (!ARMING_FLAG(ARMED)) { - if (isWaypointListValid()) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); - } - } + } + } else if (!ARMING_FLAG(ARMED)) { + if (isWaypointListValid()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED); + } + } - /* Messages that are shown regardless of Arming state */ + /* Messages that are shown regardless of Arming state */ - if (savingSettings == true) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); - } else if (notify_settings_saved > 0) { - if (millis() > notify_settings_saved) { - notify_settings_saved = 0; - } else { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED); - } - } + if (savingSettings == true) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS); + } else if (notify_settings_saved > 0) { + if (millis() > notify_settings_saved) { + notify_settings_saved = 0; + } else { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED); + } + } #ifdef USE_DEV_TOOLS - if (systemConfig()->groundTestMode) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE); - } + if (systemConfig()->groundTestMode) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE); + } #endif - if (messageCount > 0) { - message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; - if (message == failsafeInfoMessage) { - // failsafeInfoMessage is not useful for recovering - // a lost model, but might help avoiding a crash. - // Blink to grab user attention. - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } else if (message == invertedInfoMessage) { - TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); - } - // We're shoing either failsafePhaseMessage or - // navStateMessage. Don't BLINK here since - // having this text available might be crucial - // during a lost aircraft recovery and blinking - // will cause it to be missing from some frames. - } + if (messageCount > 0) { + message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)]; + if (message == failsafeInfoMessage) { + // failsafeInfoMessage is not useful for recovering + // a lost model, but might help avoiding a crash. + // Blink to grab user attention. + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (message == invertedInfoMessage) { + TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr); + } + // We're shoing either failsafePhaseMessage or + // navStateMessage. Don't BLINK here since + // having this text available might be crucial + // during a lost aircraft recovery and blinking + // will cause it to be missing from some frames. + } - osdFormatMessage(buff, buff_size, message, isCenteredText); - } - return elemAttr; + osdFormatMessage(buff, buff_size, message, isCenteredText); + } + return elemAttr; } #endif // OSD \ No newline at end of file diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0d3e3ba976..14c0cf3f0c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -129,212 +129,212 @@ #endif typedef enum { - OSD_RSSI_VALUE, - OSD_MAIN_BATT_VOLTAGE, - OSD_CROSSHAIRS, - OSD_ARTIFICIAL_HORIZON, - OSD_HORIZON_SIDEBARS, - OSD_ONTIME, - OSD_FLYTIME, - OSD_FLYMODE, - OSD_CRAFT_NAME, - OSD_THROTTLE_POS, - OSD_VTX_CHANNEL, - OSD_CURRENT_DRAW, - OSD_MAH_DRAWN, - OSD_GPS_SPEED, - OSD_GPS_SATS, - OSD_ALTITUDE, - OSD_ROLL_PIDS, - OSD_PITCH_PIDS, - OSD_YAW_PIDS, - OSD_POWER, - OSD_GPS_LON, - OSD_GPS_LAT, - OSD_HOME_DIR, - OSD_HOME_DIST, - OSD_HEADING, - OSD_VARIO, - OSD_VARIO_NUM, - OSD_AIR_SPEED, - OSD_ONTIME_FLYTIME, - OSD_RTC_TIME, - OSD_MESSAGES, - OSD_GPS_HDOP, - OSD_MAIN_BATT_CELL_VOLTAGE, - OSD_SCALED_THROTTLE_POS, - OSD_HEADING_GRAPH, - OSD_EFFICIENCY_MAH_PER_KM, - OSD_WH_DRAWN, - OSD_BATTERY_REMAINING_CAPACITY, - OSD_BATTERY_REMAINING_PERCENT, - OSD_EFFICIENCY_WH_PER_KM, - OSD_TRIP_DIST, - OSD_ATTITUDE_PITCH, - OSD_ATTITUDE_ROLL, - OSD_MAP_NORTH, - OSD_MAP_TAKEOFF, - OSD_RADAR, - OSD_WIND_SPEED_HORIZONTAL, - OSD_WIND_SPEED_VERTICAL, - OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, - OSD_REMAINING_DISTANCE_BEFORE_RTH, - OSD_HOME_HEADING_ERROR, - OSD_COURSE_HOLD_ERROR, - OSD_COURSE_HOLD_ADJUSTMENT, - OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, - OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, - OSD_POWER_SUPPLY_IMPEDANCE, - OSD_LEVEL_PIDS, - OSD_POS_XY_PIDS, - OSD_POS_Z_PIDS, - OSD_VEL_XY_PIDS, - OSD_VEL_Z_PIDS, - OSD_HEADING_P, - OSD_BOARD_ALIGN_ROLL, - OSD_BOARD_ALIGN_PITCH, - OSD_RC_EXPO, - OSD_RC_YAW_EXPO, - OSD_THROTTLE_EXPO, - OSD_PITCH_RATE, - OSD_ROLL_RATE, - OSD_YAW_RATE, - OSD_MANUAL_RC_EXPO, - OSD_MANUAL_RC_YAW_EXPO, - OSD_MANUAL_PITCH_RATE, - OSD_MANUAL_ROLL_RATE, - OSD_MANUAL_YAW_RATE, - OSD_NAV_FW_CRUISE_THR, - OSD_NAV_FW_PITCH2THR, - OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, - OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI. - OSD_FW_ALT_PID_OUTPUTS, - OSD_FW_POS_PID_OUTPUTS, - OSD_MC_VEL_X_PID_OUTPUTS, - OSD_MC_VEL_Y_PID_OUTPUTS, - OSD_MC_VEL_Z_PID_OUTPUTS, - OSD_MC_POS_XYZ_P_OUTPUTS, - OSD_3D_SPEED, - OSD_IMU_TEMPERATURE, - OSD_BARO_TEMPERATURE, - OSD_TEMP_SENSOR_0_TEMPERATURE, - OSD_TEMP_SENSOR_1_TEMPERATURE, - OSD_TEMP_SENSOR_2_TEMPERATURE, - OSD_TEMP_SENSOR_3_TEMPERATURE, - OSD_TEMP_SENSOR_4_TEMPERATURE, - OSD_TEMP_SENSOR_5_TEMPERATURE, - OSD_TEMP_SENSOR_6_TEMPERATURE, - OSD_TEMP_SENSOR_7_TEMPERATURE, - OSD_ALTITUDE_MSL, - OSD_PLUS_CODE, - OSD_MAP_SCALE, - OSD_MAP_REFERENCE, - OSD_GFORCE, - OSD_GFORCE_X, - OSD_GFORCE_Y, - OSD_GFORCE_Z, - OSD_RC_SOURCE, - OSD_VTX_POWER, - OSD_ESC_RPM, - OSD_ESC_TEMPERATURE, - OSD_AZIMUTH, - OSD_CRSF_RSSI_DBM, - OSD_CRSF_LQ, - OSD_CRSF_SNR_DB, - OSD_CRSF_TX_POWER, - OSD_GVAR_0, - OSD_GVAR_1, - OSD_GVAR_2, - OSD_GVAR_3, - OSD_TPA, - OSD_NAV_FW_CONTROL_SMOOTHNESS, - OSD_VERSION, - OSD_RANGEFINDER, - OSD_PLIMIT_REMAINING_BURST_TIME, - OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, - OSD_PLIMIT_ACTIVE_POWER_LIMIT, - OSD_GLIDESLOPE, - OSD_GPS_MAX_SPEED, - OSD_3D_MAX_SPEED, - OSD_AIR_MAX_SPEED, - OSD_ACTIVE_PROFILE, - OSD_MISSION, - OSD_SWITCH_INDICATOR_0, - OSD_SWITCH_INDICATOR_1, - OSD_SWITCH_INDICATOR_2, - OSD_SWITCH_INDICATOR_3, - OSD_TPA_TIME_CONSTANT, - OSD_FW_LEVEL_TRIM, - OSD_GLIDE_TIME_REMAINING, - OSD_GLIDE_RANGE, - OSD_CLIMB_EFFICIENCY, - OSD_NAV_WP_MULTI_MISSION_INDEX, - OSD_GROUND_COURSE, // 140 - OSD_CROSS_TRACK_ERROR, - OSD_PILOT_NAME, - OSD_PAN_SERVO_CENTRED, - OSD_PILOT_LOGO, - OSD_ITEM_COUNT // MUST BE LAST + OSD_RSSI_VALUE, + OSD_MAIN_BATT_VOLTAGE, + OSD_CROSSHAIRS, + OSD_ARTIFICIAL_HORIZON, + OSD_HORIZON_SIDEBARS, + OSD_ONTIME, + OSD_FLYTIME, + OSD_FLYMODE, + OSD_CRAFT_NAME, + OSD_THROTTLE_POS, + OSD_VTX_CHANNEL, + OSD_CURRENT_DRAW, + OSD_MAH_DRAWN, + OSD_GPS_SPEED, + OSD_GPS_SATS, + OSD_ALTITUDE, + OSD_ROLL_PIDS, + OSD_PITCH_PIDS, + OSD_YAW_PIDS, + OSD_POWER, + OSD_GPS_LON, + OSD_GPS_LAT, + OSD_HOME_DIR, + OSD_HOME_DIST, + OSD_HEADING, + OSD_VARIO, + OSD_VARIO_NUM, + OSD_AIR_SPEED, + OSD_ONTIME_FLYTIME, + OSD_RTC_TIME, + OSD_MESSAGES, + OSD_GPS_HDOP, + OSD_MAIN_BATT_CELL_VOLTAGE, + OSD_SCALED_THROTTLE_POS, + OSD_HEADING_GRAPH, + OSD_EFFICIENCY_MAH_PER_KM, + OSD_WH_DRAWN, + OSD_BATTERY_REMAINING_CAPACITY, + OSD_BATTERY_REMAINING_PERCENT, + OSD_EFFICIENCY_WH_PER_KM, + OSD_TRIP_DIST, + OSD_ATTITUDE_PITCH, + OSD_ATTITUDE_ROLL, + OSD_MAP_NORTH, + OSD_MAP_TAKEOFF, + OSD_RADAR, + OSD_WIND_SPEED_HORIZONTAL, + OSD_WIND_SPEED_VERTICAL, + OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, + OSD_REMAINING_DISTANCE_BEFORE_RTH, + OSD_HOME_HEADING_ERROR, + OSD_COURSE_HOLD_ERROR, + OSD_COURSE_HOLD_ADJUSTMENT, + OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, + OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, + OSD_POWER_SUPPLY_IMPEDANCE, + OSD_LEVEL_PIDS, + OSD_POS_XY_PIDS, + OSD_POS_Z_PIDS, + OSD_VEL_XY_PIDS, + OSD_VEL_Z_PIDS, + OSD_HEADING_P, + OSD_BOARD_ALIGN_ROLL, + OSD_BOARD_ALIGN_PITCH, + OSD_RC_EXPO, + OSD_RC_YAW_EXPO, + OSD_THROTTLE_EXPO, + OSD_PITCH_RATE, + OSD_ROLL_RATE, + OSD_YAW_RATE, + OSD_MANUAL_RC_EXPO, + OSD_MANUAL_RC_YAW_EXPO, + OSD_MANUAL_PITCH_RATE, + OSD_MANUAL_ROLL_RATE, + OSD_MANUAL_YAW_RATE, + OSD_NAV_FW_CRUISE_THR, + OSD_NAV_FW_PITCH2THR, + OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, + OSD_DEBUG, // Intentionally absent from configurator and CMS. Set it from CLI. + OSD_FW_ALT_PID_OUTPUTS, + OSD_FW_POS_PID_OUTPUTS, + OSD_MC_VEL_X_PID_OUTPUTS, + OSD_MC_VEL_Y_PID_OUTPUTS, + OSD_MC_VEL_Z_PID_OUTPUTS, + OSD_MC_POS_XYZ_P_OUTPUTS, + OSD_3D_SPEED, + OSD_IMU_TEMPERATURE, + OSD_BARO_TEMPERATURE, + OSD_TEMP_SENSOR_0_TEMPERATURE, + OSD_TEMP_SENSOR_1_TEMPERATURE, + OSD_TEMP_SENSOR_2_TEMPERATURE, + OSD_TEMP_SENSOR_3_TEMPERATURE, + OSD_TEMP_SENSOR_4_TEMPERATURE, + OSD_TEMP_SENSOR_5_TEMPERATURE, + OSD_TEMP_SENSOR_6_TEMPERATURE, + OSD_TEMP_SENSOR_7_TEMPERATURE, + OSD_ALTITUDE_MSL, + OSD_PLUS_CODE, + OSD_MAP_SCALE, + OSD_MAP_REFERENCE, + OSD_GFORCE, + OSD_GFORCE_X, + OSD_GFORCE_Y, + OSD_GFORCE_Z, + OSD_RC_SOURCE, + OSD_VTX_POWER, + OSD_ESC_RPM, + OSD_ESC_TEMPERATURE, + OSD_AZIMUTH, + OSD_CRSF_RSSI_DBM, + OSD_CRSF_LQ, + OSD_CRSF_SNR_DB, + OSD_CRSF_TX_POWER, + OSD_GVAR_0, + OSD_GVAR_1, + OSD_GVAR_2, + OSD_GVAR_3, + OSD_TPA, + OSD_NAV_FW_CONTROL_SMOOTHNESS, + OSD_VERSION, + OSD_RANGEFINDER, + OSD_PLIMIT_REMAINING_BURST_TIME, + OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, + OSD_PLIMIT_ACTIVE_POWER_LIMIT, + OSD_GLIDESLOPE, + OSD_GPS_MAX_SPEED, + OSD_3D_MAX_SPEED, + OSD_AIR_MAX_SPEED, + OSD_ACTIVE_PROFILE, + OSD_MISSION, + OSD_SWITCH_INDICATOR_0, + OSD_SWITCH_INDICATOR_1, + OSD_SWITCH_INDICATOR_2, + OSD_SWITCH_INDICATOR_3, + OSD_TPA_TIME_CONSTANT, + OSD_FW_LEVEL_TRIM, + OSD_GLIDE_TIME_REMAINING, + OSD_GLIDE_RANGE, + OSD_CLIMB_EFFICIENCY, + OSD_NAV_WP_MULTI_MISSION_INDEX, + OSD_GROUND_COURSE, // 140 + OSD_CROSS_TRACK_ERROR, + OSD_PILOT_NAME, + OSD_PAN_SERVO_CENTRED, + OSD_PILOT_LOGO, + OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; typedef enum { - OSD_UNIT_IMPERIAL, - OSD_UNIT_METRIC, - OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph - OSD_UNIT_UK, // Show everything in imperial, temperature in C - OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C + OSD_UNIT_IMPERIAL, + OSD_UNIT_METRIC, + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C + OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C - OSD_UNIT_MAX = OSD_UNIT_GA, + OSD_UNIT_MAX = OSD_UNIT_GA, } osd_unit_e; typedef enum { - OSD_STATS_ENERGY_UNIT_MAH, - OSD_STATS_ENERGY_UNIT_WH, + OSD_STATS_ENERGY_UNIT_MAH, + OSD_STATS_ENERGY_UNIT_WH, } osd_stats_energy_unit_e; typedef enum { - OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, - OSD_STATS_MIN_VOLTAGE_UNIT_CELL, + OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, + OSD_STATS_MIN_VOLTAGE_UNIT_CELL, } osd_stats_min_voltage_unit_e; typedef enum { - OSD_CROSSHAIRS_STYLE_DEFAULT, - OSD_CROSSHAIRS_STYLE_AIRCRAFT, - OSD_CROSSHAIRS_STYLE_TYPE3, - OSD_CROSSHAIRS_STYLE_TYPE4, - OSD_CROSSHAIRS_STYLE_TYPE5, - OSD_CROSSHAIRS_STYLE_TYPE6, - OSD_CROSSHAIRS_STYLE_TYPE7, + OSD_CROSSHAIRS_STYLE_DEFAULT, + OSD_CROSSHAIRS_STYLE_AIRCRAFT, + OSD_CROSSHAIRS_STYLE_TYPE3, + OSD_CROSSHAIRS_STYLE_TYPE4, + OSD_CROSSHAIRS_STYLE_TYPE5, + OSD_CROSSHAIRS_STYLE_TYPE6, + OSD_CROSSHAIRS_STYLE_TYPE7, } osd_crosshairs_style_e; typedef enum { - OSD_SIDEBAR_SCROLL_NONE, - OSD_SIDEBAR_SCROLL_ALTITUDE, - OSD_SIDEBAR_SCROLL_SPEED, - OSD_SIDEBAR_SCROLL_HOME_DISTANCE, + OSD_SIDEBAR_SCROLL_NONE, + OSD_SIDEBAR_SCROLL_ALTITUDE, + OSD_SIDEBAR_SCROLL_SPEED, + OSD_SIDEBAR_SCROLL_HOME_DISTANCE, - OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE, + OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE, } osd_sidebar_scroll_e; typedef enum { - OSD_ALIGN_LEFT, - OSD_ALIGN_RIGHT + OSD_ALIGN_LEFT, + OSD_ALIGN_RIGHT } osd_alignment_e; typedef enum { - OSD_AHI_STYLE_DEFAULT, - OSD_AHI_STYLE_LINE, + OSD_AHI_STYLE_DEFAULT, + OSD_AHI_STYLE_LINE, } osd_ahi_style_e; typedef enum { - OSD_CRSF_LQ_TYPE1, - OSD_CRSF_LQ_TYPE2, - OSD_CRSF_LQ_TYPE3 + OSD_CRSF_LQ_TYPE1, + OSD_CRSF_LQ_TYPE2, + OSD_CRSF_LQ_TYPE3 } osd_crsf_lq_format_e; typedef struct osdLayoutsConfig_s { - // Layouts - uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; + // Layouts + uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; } osdLayoutsConfig_t; PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); @@ -342,113 +342,113 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); #define OSD_SWITCH_INDICATOR_NAME_LENGTH 4 typedef struct osdConfig_s { - // Alarms - uint8_t rssi_alarm; // rssi % - uint16_t time_alarm; // fly minutes - uint16_t alt_alarm; // positive altitude in m - uint16_t dist_alarm; // home distance in m - uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t current_alarm; // current consumption in A - int16_t imu_temp_alarm_min; - int16_t imu_temp_alarm_max; - int16_t esc_temp_alarm_min; - int16_t esc_temp_alarm_max; - float gforce_alarm; - float gforce_axis_alarm_min; - float gforce_axis_alarm_max; + // Alarms + uint8_t rssi_alarm; // rssi % + uint16_t time_alarm; // fly minutes + uint16_t alt_alarm; // positive altitude in m + uint16_t dist_alarm; // home distance in m + uint16_t neg_alt_alarm; // abs(negative altitude) in m + uint8_t current_alarm; // current consumption in A + int16_t imu_temp_alarm_min; + int16_t imu_temp_alarm_max; + int16_t esc_temp_alarm_min; + int16_t esc_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int8_t snr_alarm; //CRSF SNR alarm in dB - int8_t link_quality_alarm; - int16_t rssi_dbm_alarm; // in dBm - int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% - int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% + int8_t snr_alarm; //CRSF SNR alarm in dB + int8_t link_quality_alarm; + int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO - int16_t baro_temp_alarm_min; - int16_t baro_temp_alarm_max; + int16_t baro_temp_alarm_min; + int16_t baro_temp_alarm_max; #endif #ifdef USE_TEMPERATURE_SENSOR - osd_alignment_e temp_label_align; + osd_alignment_e temp_label_align; #endif #ifdef USE_PITOT - float airspeed_alarm_min; - float airspeed_alarm_max; + float airspeed_alarm_min; + float airspeed_alarm_max; #endif - videoSystem_e video_system; - uint8_t row_shiftdown; - int16_t msp_displayport_fullframe_interval; + videoSystem_e video_system; + uint8_t row_shiftdown; + int16_t msp_displayport_fullframe_interval; - // Preferences - uint8_t main_voltage_decimals; - uint8_t ahi_reverse_roll; - uint8_t ahi_max_pitch; - uint8_t crosshairs_style; // from osd_crosshairs_style_e - int8_t horizon_offset; - int8_t camera_uptilt; - bool ahi_camera_uptilt_comp; - uint8_t camera_fov_h; - uint8_t camera_fov_v; - uint8_t hud_margin_h; - uint8_t hud_margin_v; - bool hud_homing; - bool hud_homepoint; - uint8_t hud_radar_disp; - uint16_t hud_radar_range_min; - uint16_t hud_radar_range_max; - uint8_t hud_radar_alt_difference_display_time; - uint8_t hud_radar_distance_display_time; - uint8_t hud_wp_disp; + // Preferences + uint8_t main_voltage_decimals; + uint8_t ahi_reverse_roll; + uint8_t ahi_max_pitch; + uint8_t crosshairs_style; // from osd_crosshairs_style_e + int8_t horizon_offset; + int8_t camera_uptilt; + bool ahi_camera_uptilt_comp; + uint8_t camera_fov_h; + uint8_t camera_fov_v; + uint8_t hud_margin_h; + uint8_t hud_margin_v; + bool hud_homing; + bool hud_homepoint; + uint8_t hud_radar_disp; + uint16_t hud_radar_range_min; + uint16_t hud_radar_range_max; + uint8_t hud_radar_alt_difference_display_time; + uint8_t hud_radar_distance_display_time; + uint8_t hud_wp_disp; - uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t sidebar_scroll_arrows; + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t sidebar_scroll_arrows; - uint8_t units; // from osd_unit_e - uint8_t stats_energy_unit; // from osd_stats_energy_unit_e - uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e - uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) + uint8_t units; // from osd_unit_e + uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) #ifdef USE_WIND_ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance #endif - uint8_t coordinate_digits; - bool osd_failsafe_switch_layout; - uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; - uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. - bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm - uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred - bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo - uint8_t crsf_lq_format; - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh - uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) - char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. - char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. - char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. - char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. - bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. - uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. - uint16_t arm_screen_display_time; // Length of time the arm screen is displayed + uint8_t coordinate_digits; + bool osd_failsafe_switch_layout; + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_short; + uint8_t ahi_style; + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred + bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo + uint8_t crsf_lq_format; + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. + uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. + uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From 679ec8faeedf83f1a829bbd415aced7c6b2eb5a5 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 18 Sep 2023 08:44:23 +0100 Subject: [PATCH 30/62] More tabs to spaces Damn, I hate spaces. Tabs for alignment makes so much more sense :( --- src/main/io/osd.h | 186 +++++++++++++++++++++++----------------------- 1 file changed, 93 insertions(+), 93 deletions(-) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 14c0cf3f0c..7df3acc404 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -280,9 +280,9 @@ typedef enum { typedef enum { OSD_UNIT_IMPERIAL, OSD_UNIT_METRIC, - OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph - OSD_UNIT_UK, // Show everything in imperial, temperature in C - OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C + OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph + OSD_UNIT_UK, // Show everything in imperial, temperature in C + OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C OSD_UNIT_MAX = OSD_UNIT_GA, } osd_unit_e; @@ -343,112 +343,112 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); typedef struct osdConfig_s { // Alarms - uint8_t rssi_alarm; // rssi % - uint16_t time_alarm; // fly minutes - uint16_t alt_alarm; // positive altitude in m - uint16_t dist_alarm; // home distance in m - uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t current_alarm; // current consumption in A - int16_t imu_temp_alarm_min; - int16_t imu_temp_alarm_max; - int16_t esc_temp_alarm_min; - int16_t esc_temp_alarm_max; - float gforce_alarm; - float gforce_axis_alarm_min; - float gforce_axis_alarm_max; + uint8_t rssi_alarm; // rssi % + uint16_t time_alarm; // fly minutes + uint16_t alt_alarm; // positive altitude in m + uint16_t dist_alarm; // home distance in m + uint16_t neg_alt_alarm; // abs(negative altitude) in m + uint8_t current_alarm; // current consumption in A + int16_t imu_temp_alarm_min; + int16_t imu_temp_alarm_max; + int16_t esc_temp_alarm_min; + int16_t esc_temp_alarm_max; + float gforce_alarm; + float gforce_axis_alarm_min; + float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int8_t snr_alarm; //CRSF SNR alarm in dB - int8_t link_quality_alarm; - int16_t rssi_dbm_alarm; // in dBm - int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% - int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% + int8_t snr_alarm; //CRSF SNR alarm in dB + int8_t link_quality_alarm; + int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO - int16_t baro_temp_alarm_min; - int16_t baro_temp_alarm_max; + int16_t baro_temp_alarm_min; + int16_t baro_temp_alarm_max; #endif #ifdef USE_TEMPERATURE_SENSOR - osd_alignment_e temp_label_align; + osd_alignment_e temp_label_align; #endif #ifdef USE_PITOT - float airspeed_alarm_min; - float airspeed_alarm_max; + float airspeed_alarm_min; + float airspeed_alarm_max; #endif - videoSystem_e video_system; - uint8_t row_shiftdown; - int16_t msp_displayport_fullframe_interval; + videoSystem_e video_system; + uint8_t row_shiftdown; + int16_t msp_displayport_fullframe_interval; // Preferences - uint8_t main_voltage_decimals; - uint8_t ahi_reverse_roll; - uint8_t ahi_max_pitch; - uint8_t crosshairs_style; // from osd_crosshairs_style_e - int8_t horizon_offset; - int8_t camera_uptilt; - bool ahi_camera_uptilt_comp; - uint8_t camera_fov_h; - uint8_t camera_fov_v; - uint8_t hud_margin_h; - uint8_t hud_margin_v; - bool hud_homing; - bool hud_homepoint; - uint8_t hud_radar_disp; - uint16_t hud_radar_range_min; - uint16_t hud_radar_range_max; - uint8_t hud_radar_alt_difference_display_time; - uint8_t hud_radar_distance_display_time; - uint8_t hud_wp_disp; + uint8_t main_voltage_decimals; + uint8_t ahi_reverse_roll; + uint8_t ahi_max_pitch; + uint8_t crosshairs_style; // from osd_crosshairs_style_e + int8_t horizon_offset; + int8_t camera_uptilt; + bool ahi_camera_uptilt_comp; + uint8_t camera_fov_h; + uint8_t camera_fov_v; + uint8_t hud_margin_h; + uint8_t hud_margin_v; + bool hud_homing; + bool hud_homepoint; + uint8_t hud_radar_disp; + uint16_t hud_radar_range_min; + uint16_t hud_radar_range_max; + uint8_t hud_radar_alt_difference_display_time; + uint8_t hud_radar_distance_display_time; + uint8_t hud_wp_disp; - uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t sidebar_scroll_arrows; + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t sidebar_scroll_arrows; - uint8_t units; // from osd_unit_e - uint8_t stats_energy_unit; // from osd_stats_energy_unit_e - uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e - uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) + uint8_t units; // from osd_unit_e + uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) #ifdef USE_WIND_ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance #endif - uint8_t coordinate_digits; - bool osd_failsafe_switch_layout; - uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; - uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. - bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm - uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred - bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo - uint8_t crsf_lq_format; - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh - uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) - char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. - char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. - char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. - char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. - bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. - uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. - uint16_t arm_screen_display_time; // Length of time the arm screen is displayed + uint8_t coordinate_digits; + bool osd_failsafe_switch_layout; + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_short; + uint8_t ahi_style; + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + bool osd_home_position_arm_screen; + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred + bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo + uint8_t crsf_lq_format; + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. + uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. + uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From 073ea3b3eeaf0742cf901340cb7729b93daa8284 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 18 Sep 2023 13:10:45 +0100 Subject: [PATCH 31/62] More tabs to spaces --- src/main/cms/cms.h | 6 +- src/main/io/osd.h | 240 ++++++++++++++++++++++----------------------- 2 files changed, 123 insertions(+), 123 deletions(-) diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index 812975e3b4..7084df8b50 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -37,9 +37,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration); void cmsUpdate(uint32_t currentTimeUs); void cmsSetExternKey(cms_key_e extKey); -#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" -#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" -#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" +#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID" +#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP" // cmsMenuExit special ptr values #define CMS_EXIT (0) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7df3acc404..f911f75b02 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -32,19 +32,19 @@ // 00vb yyyy yyxx xxxx // (visible)(blink)(yCoord)(xCoord) -#define OSD_VISIBLE_FLAG 0x2000 -#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) +#define OSD_VISIBLE_FLAG 0x2000 +#define OSD_VISIBLE(x) ((x) & OSD_VISIBLE_FLAG) -#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) -#define OSD_X(x) ((x) & 0x3F) -#define OSD_Y(x) (((x) >> 6) & 0x3F) -#define OSD_POS_MAX 0xFFF +#define OSD_POS(x,y) (((x) & 0x3F) | (((y) & 0x3F) << 6)) +#define OSD_X(x) ((x) & 0x3F) +#define OSD_Y(x) (((x) >> 6) & 0x3F) +#define OSD_POS_MAX 0xFFF // For DJI compatibility #define OSD_VISIBLE_FLAG_SD 0x0800 -#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) +#define OSD_POS_SD(x,y) (((x) & 0x1F) | (((y) & 0x1F) << 5)) -#define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) +#define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG) #define OSD_HOMING_LIM_H1 6 #define OSD_HOMING_LIM_H2 16 @@ -54,78 +54,78 @@ #define OSD_HOMING_LIM_V3 15 // Message defines to be use in OSD and/or telemetry exports -#define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" -#define OSD_MSG_TURN_ARM_SW_OFF "TURN ARM SWITCH OFF" -#define OSD_MSG_DISABLED_BY_FS "DISABLED BY FAILSAFE" -#define OSD_MSG_AIRCRAFT_UNLEVEL "AIRCRAFT IS NOT LEVEL" -#define OSD_MSG_SENSORS_CAL "SENSORS CALIBRATING" -#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" -#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" -#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" -#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" -#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" -#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" -#define OSD_MSG_DISARM_1ST "DISABLE ARM SWITCH FIRST" -#define OSD_MSG_GYRO_FAILURE "GYRO FAILURE" -#define OSD_MSG_ACC_FAIL "ACCELEROMETER FAILURE" -#define OSD_MSG_MAG_FAIL "COMPASS FAILURE" -#define OSD_MSG_BARO_FAIL "BAROMETER FAILURE" -#define OSD_MSG_GPS_FAIL "GPS FAILURE" -#define OSD_MSG_RANGEFINDER_FAIL "RANGE FINDER FAILURE" -#define OSD_MSG_PITOT_FAIL "PITOT METER FAILURE" -#define OSD_MSG_HW_FAIL "HARDWARE FAILURE" -#define OSD_MSG_FS_EN "FAILSAFE MODE ENABLED" -#define OSD_MSG_KILL_SW_EN "KILLSWITCH MODE ENABLED" -#define OSD_MSG_NO_RC_LINK "NO RC LINK" -#define OSD_MSG_THROTTLE_NOT_LOW "THROTTLE IS NOT LOW" -#define OSD_MSG_ROLLPITCH_OFFCENTER "ROLLPITCH NOT CENTERED" -#define OSD_MSG_AUTOTRIM_ACTIVE "AUTOTRIM IS ACTIVE" -#define OSD_MSG_NOT_ENOUGH_MEMORY "NOT ENOUGH MEMORY" -#define OSD_MSG_INVALID_SETTING "INVALID SETTING" -#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" -#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" -#define OSD_MSG_NO_PREARM "NO PREARM" -#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" -#define OSD_MSG_RTH_FS "(RTH)" -#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" -#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" -#define OSD_MSG_STARTING_RTH "STARTING RTH" -#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" -#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" -#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" -#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" -#define OSD_MSG_WP_LANDED "WP END>LANDED" -#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" -#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" -#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" -#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" -#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" -#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" -#define OSD_MSG_LANDING "LANDING" -#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" -#define OSD_MSG_HOVERING "HOVERING" -#define OSD_MSG_LANDED "LANDED" -#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" -#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" -#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" -#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" -#define OSD_MSG_AUTOTRIM "(AUTOTRIM)" -#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" -#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" -#define OSD_MSG_AUTOLEVEL "(AUTO LEVEL TRIM)" -#define OSD_MSG_HEADFREE "(HEADFREE)" -#define OSD_MSG_NAV_SOARING "(SOARING)" -#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" -#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **" -#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **" +#define OSD_MSG_RC_RX_LINK_LOST "!RC RX LINK LOST!" +#define OSD_MSG_TURN_ARM_SW_OFF "TURN ARM SWITCH OFF" +#define OSD_MSG_DISABLED_BY_FS "DISABLED BY FAILSAFE" +#define OSD_MSG_AIRCRAFT_UNLEVEL "AIRCRAFT IS NOT LEVEL" +#define OSD_MSG_SENSORS_CAL "SENSORS CALIBRATING" +#define OSD_MSG_SYS_OVERLOADED "SYSTEM OVERLOADED" +#define OSD_MSG_WAITING_GPS_FIX "WAITING FOR GPS FIX" +#define OSD_MSG_DISABLE_NAV_FIRST "DISABLE NAVIGATION FIRST" +#define OSD_MSG_JUMP_WP_MISCONFIG "JUMP WAYPOINT MISCONFIGURED" +#define OSD_MSG_MAG_NOT_CAL "COMPASS NOT CALIBRATED" +#define OSD_MSG_ACC_NOT_CAL "ACCELEROMETER NOT CALIBRATED" +#define OSD_MSG_DISARM_1ST "DISABLE ARM SWITCH FIRST" +#define OSD_MSG_GYRO_FAILURE "GYRO FAILURE" +#define OSD_MSG_ACC_FAIL "ACCELEROMETER FAILURE" +#define OSD_MSG_MAG_FAIL "COMPASS FAILURE" +#define OSD_MSG_BARO_FAIL "BAROMETER FAILURE" +#define OSD_MSG_GPS_FAIL "GPS FAILURE" +#define OSD_MSG_RANGEFINDER_FAIL "RANGE FINDER FAILURE" +#define OSD_MSG_PITOT_FAIL "PITOT METER FAILURE" +#define OSD_MSG_HW_FAIL "HARDWARE FAILURE" +#define OSD_MSG_FS_EN "FAILSAFE MODE ENABLED" +#define OSD_MSG_KILL_SW_EN "KILLSWITCH MODE ENABLED" +#define OSD_MSG_NO_RC_LINK "NO RC LINK" +#define OSD_MSG_THROTTLE_NOT_LOW "THROTTLE IS NOT LOW" +#define OSD_MSG_ROLLPITCH_OFFCENTER "ROLLPITCH NOT CENTERED" +#define OSD_MSG_AUTOTRIM_ACTIVE "AUTOTRIM IS ACTIVE" +#define OSD_MSG_NOT_ENOUGH_MEMORY "NOT ENOUGH MEMORY" +#define OSD_MSG_INVALID_SETTING "INVALID SETTING" +#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" +#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" +#define OSD_MSG_NO_PREARM "NO PREARM" +#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" +#define OSD_MSG_RTH_FS "(RTH)" +#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" +#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" +#define OSD_MSG_STARTING_RTH "STARTING RTH" +#define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" +#define OSD_MSG_RTH_TRACKBACK "RTH BACK TRACKING" +#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME" +#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION" +#define OSD_MSG_WP_LANDED "WP END>LANDED" +#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_ADJUSTING_WP_ALT "ADJUSTING WP ALTITUDE" +#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)" +#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" +#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *" +#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" +#define OSD_MSG_LANDING "LANDING" +#define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" +#define OSD_MSG_HOVERING "HOVERING" +#define OSD_MSG_LANDED "LANDED" +#define OSD_MSG_PREPARING_LAND "PREPARING TO LAND" +#define OSD_MSG_AUTOLAUNCH "AUTOLAUNCH" +#define OSD_MSG_AUTOLAUNCH_MANUAL "AUTOLAUNCH (MANUAL)" +#define OSD_MSG_ALTITUDE_HOLD "(ALTITUDE HOLD)" +#define OSD_MSG_AUTOTRIM "(AUTOTRIM)" +#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" +#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" +#define OSD_MSG_AUTOLEVEL "(AUTO LEVEL TRIM)" +#define OSD_MSG_HEADFREE "(HEADFREE)" +#define OSD_MSG_NAV_SOARING "(SOARING)" +#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#define OSD_MSG_SAVING_SETTNGS "** SAVING SETTINGS **" +#define OSD_MSG_SETTINGS_SAVED "** SETTINGS SAVED **" #ifdef USE_DEV_TOOLS -#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" +#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED" #endif #if defined(USE_SAFE_HOME) -#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" -#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" +#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif typedef enum { @@ -343,12 +343,12 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); typedef struct osdConfig_s { // Alarms - uint8_t rssi_alarm; // rssi % - uint16_t time_alarm; // fly minutes - uint16_t alt_alarm; // positive altitude in m - uint16_t dist_alarm; // home distance in m - uint16_t neg_alt_alarm; // abs(negative altitude) in m - uint8_t current_alarm; // current consumption in A + uint8_t rssi_alarm; // rssi % + uint16_t time_alarm; // fly minutes + uint16_t alt_alarm; // positive altitude in m + uint16_t dist_alarm; // home distance in m + uint16_t neg_alt_alarm; // abs(negative altitude) in m + uint8_t current_alarm; // current consumption in A int16_t imu_temp_alarm_min; int16_t imu_temp_alarm_max; int16_t esc_temp_alarm_min; @@ -357,11 +357,11 @@ typedef struct osdConfig_s { float gforce_axis_alarm_min; float gforce_axis_alarm_max; #ifdef USE_SERIALRX_CRSF - int8_t snr_alarm; //CRSF SNR alarm in dB + int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; - int16_t rssi_dbm_alarm; // in dBm - int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% - int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% + int16_t rssi_dbm_alarm; // in dBm + int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100% + int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0% #endif #ifdef USE_BARO int16_t baro_temp_alarm_min; @@ -383,7 +383,7 @@ typedef struct osdConfig_s { uint8_t main_voltage_decimals; uint8_t ahi_reverse_roll; uint8_t ahi_max_pitch; - uint8_t crosshairs_style; // from osd_crosshairs_style_e + uint8_t crosshairs_style; // from osd_crosshairs_style_e int8_t horizon_offset; int8_t camera_uptilt; bool ahi_camera_uptilt_comp; @@ -400,55 +400,55 @@ typedef struct osdConfig_s { uint8_t hud_radar_distance_display_time; uint8_t hud_wp_disp; - uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e - uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e + uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e uint8_t sidebar_scroll_arrows; - uint8_t units; // from osd_unit_e - uint8_t stats_energy_unit; // from osd_stats_energy_unit_e - uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e - uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) + uint8_t units; // from osd_unit_e + uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e + uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds) #ifdef USE_WIND_ESTIMATOR - bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance + bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance #endif uint8_t coordinate_digits; bool osd_failsafe_switch_layout; - uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE + uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE uint8_t plus_code_short; uint8_t ahi_style; - uint8_t force_grid; // Force a pixel based OSD to use grid mode. - uint8_t ahi_bordered; // Only used by the AHI widget - uint8_t ahi_width; // In pixels, only used by the AHI widget - uint8_t ahi_height; // In pixels, only used by the AHI widget - int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. - int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. - uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. - uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. + uint8_t force_grid; // Force a pixel based OSD to use grid mode. + uint8_t ahi_bordered; // Only used by the AHI widget + uint8_t ahi_width; // In pixels, only used by the AHI widget + uint8_t ahi_height; // In pixels, only used by the AHI widget + int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only. + int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges. + uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. + uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; - uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm - uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred - bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred + bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo uint8_t crsf_lq_format; - uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows - uint8_t telemetry; // use telemetry on displayed pixel line 0 - uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. - uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh - uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 + uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. + uint16_t system_msg_display_time; // system message display time for multiple messages (ms) + uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. - bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. - uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. - uint16_t arm_screen_display_time; // Length of time the arm screen is displayed + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. + bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. + uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. + uint16_t arm_screen_display_time; // Length of time the arm screen is displayed } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); From 0d3216ef07177c925f783ef9a4f886be5df70873 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 19 Sep 2023 20:55:00 +0200 Subject: [PATCH 32/62] Set EZ Tune on profile change --- src/main/fc/config.c | 3 +++ src/main/fc/fc_msp.c | 3 +++ src/main/flight/ez_tune.c | 1 - 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3b884db747..65bca8f9ef 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -423,6 +423,9 @@ bool setConfigProfile(uint8_t profileIndex) systemConfigMutable()->current_profile_index = profileIndex; // set the control rate profile to match setControlRateProfile(profileIndex); +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif return ret; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b2c04e9dc1..a60975b9dc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -696,6 +696,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255)); sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255)); } + #ifdef USE_EZ_TUNE + ezTuneUpdate(); + #endif break; case MSP_PIDNAMES: diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index c4846d3989..2703fe870e 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -65,7 +65,6 @@ static float getYawPidScale(float input) { /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed - * FIXME call on profile change * FIXME call on EZTune settings change */ void ezTuneUpdate(void) { From e772d085cdbdfecd0250f431cf5894b257ec797a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 19 Sep 2023 21:14:38 +0200 Subject: [PATCH 33/62] Add EZ Tune MSP frames --- src/main/fc/config.c | 1 + src/main/fc/fc_msp.c | 43 +++++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 3 files changed, 46 insertions(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 65bca8f9ef..fa7d893655 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -64,6 +64,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" +#include "flight/ez_tune.h" #include "fc/config.h" #include "fc/controlrate_profile.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a60975b9dc..f2c80bfd95 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -77,6 +77,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "config/config_eeprom.h" #include "config/feature.h" @@ -1583,6 +1584,24 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE: + { + sbufWriteU8(dst, ezTune()->enabled); + sbufWriteU16(dst, ezTune()->filterHz); + sbufWriteU8(dst, ezTune()->axisRatio); + sbufWriteU8(dst, ezTune()->response); + sbufWriteU8(dst, ezTune()->damping); + sbufWriteU8(dst, ezTune()->stability); + sbufWriteU8(dst, ezTune()->aggressiveness); + sbufWriteU8(dst, ezTune()->rate); + sbufWriteU8(dst, ezTune()->expo); + } + break; + +#endif + default: return false; } @@ -3040,6 +3059,30 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE_SET: + { + if (dataSize == 10) { + ezTuneMutable()->enabled = sbufReadU8(src); + ezTuneMutable()->filterHz = sbufReadU16(src); + ezTuneMutable()->axisRatio = sbufReadU8(src); + ezTuneMutable()->response = sbufReadU8(src); + ezTuneMutable()->damping = sbufReadU8(src); + ezTuneMutable()->stability = sbufReadU8(src); + ezTuneMutable()->aggressiveness = sbufReadU8(src); + ezTuneMutable()->rate = sbufReadU8(src); + ezTuneMutable()->expo = sbufReadU8(src); + + ezTuneUpdate(); + } else { + return MSP_RESULT_ERROR; + } + } + break; + +#endif + default: return MSP_RESULT_ERROR; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index f746f5c8ff..3c38c8ffd4 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -92,3 +92,5 @@ #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 +#define MSP2_INAV_EZ_TUNE 0x2050 +#define MSP2_INAV_EZ_TUNE_SET 0x2051 \ No newline at end of file From 3ffb125977ff71e027bfa1a20aee75f592a0ddf0 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 23 Sep 2023 13:32:38 +0100 Subject: [PATCH 34/62] Increment PG --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce90250d42..d336dea791 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -208,7 +208,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { From 92ab6b87366f5a7b950a47d605d042b8702c9358 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 23 Sep 2023 14:11:15 +0100 Subject: [PATCH 35/62] Fixed bugs post merge from master --- src/main/io/osd.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d336dea791..b9b9b9c0ba 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4496,7 +4496,7 @@ static void osdShowHDArmScreen(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); #if defined(USE_GPS) #if defined (USE_SAFE_HOME) - if (safehome_distance) { + if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; armScreenRow++; } @@ -4625,12 +4625,12 @@ static void osdShowSDArmScreen(void) } #if defined (USE_SAFE_HOME) - if (safehome_distance) { // safehome found during arming + if (posControl.safehomeState.distance) { // safehome found during arming if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { strcpy(buf, "SAFEHOME FOUND; MODE OFF"); } else { - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, safehome_index, buf2); + osdFormatDistanceStr(buf2, posControl.safehomeState.distance); + tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2); } textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious @@ -4775,7 +4775,7 @@ static void osdRefresh(timeUs_t currentTimeUs) #if defined(USE_SAFE_HOME) if (posControl.safehomeState.distance) delay+= 3000; - +#endif osdSetNextRefreshIn(delay); } else { // Display the "Stats" screen @@ -5391,4 +5391,5 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) return elemAttr; } + #endif // OSD From 52f5cffc43e73283f599054236ff5b460ea146ed Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 26 Sep 2023 23:33:56 +0100 Subject: [PATCH 36/62] prevent unwanted resets on disarm --- src/main/fc/fc_core.c | 12 +++++- src/main/fc/runtime_config.h | 1 + src/main/io/osd.c | 6 ++- src/main/navigation/navigation.c | 34 ++++++++++------ .../navigation/navigation_pos_estimator.c | 40 ++++++++++++++----- 5 files changed, 67 insertions(+), 26 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 76d1a2626c..66c275366b 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -436,6 +436,7 @@ void disarm(disarmReason_t disarmReason) lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); #ifdef USE_BLACKBOX if (feature(FEATURE_BLACKBOX)) { @@ -522,6 +523,7 @@ bool emergInflightRearmEnabled(void) if (isProbablyStillFlying() || mcDisarmVertVelCheck) { emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + ENABLE_STATE(IN_FLIGHT_EMERG_REARM); return true; } @@ -574,11 +576,13 @@ void tryArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); - logicConditionReset(); + if (!STATE(IN_FLIGHT_EMERG_REARM)) { + logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK - programmingPidReset(); + programmingPidReset(); #endif + } headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -902,6 +906,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processDelayedSave(); } + if (armTime > 1 * USECS_PER_SEC) { // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); + } + #if defined(SITL_BUILD) if (lockMainPID()) { #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 16d1b411df..241e5f7e25 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,6 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), + IN_FLIGHT_EMERG_REARM = (1 << 27), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f5daeaf63c..b34751096e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4579,9 +4579,13 @@ static void osdRefresh(timeUs_t currentTimeUs) osdResetStats(); osdShowArmed(); uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + if (STATE(IN_FLIGHT_EMERG_REARM)) { + delay /= 3; + } #if defined(USE_SAFE_HOME) - if (posControl.safehomeState.distance) + else if (posControl.safehomeState.distance) { delay *= 3; + } #endif osdSetNextRefreshIn(delay); } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b6cf9583c7..0465eceab6 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2530,11 +2530,15 @@ bool findNearestSafeHome(void) *-----------------------------------------------------------*/ void updateHomePosition(void) { - // Disarmed and have a valid position, constantly update home + // Disarmed and have a valid position, constantly update home before first arm (depending on setting) + // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm) + static bool setHome = false; + navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING; + if (!ARMING_FLAG(ARMED)) { if (posControl.flags.estPosStatus >= EST_USABLE) { const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; - bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; + setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { case NAV_RESET_NEVER: break; @@ -2545,24 +2549,16 @@ void updateHomePosition(void) setHome = true; break; } - if (setHome) { -#if defined(USE_SAFE_HOME) - findNearestSafeHome(); -#endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - // save the current location in case it is replaced by a safehome or HOME_RESET - posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; - } } } else { static bool isHomeResetAllowed = false; - // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { - const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + homeUpdateFlags = 0; + homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + setHome = true; isHomeResetAllowed = false; } } @@ -2577,6 +2573,18 @@ void updateHomePosition(void) posControl.homeDirection = calculateBearingToDestination(tmpHomePos); updateHomePositionCompatibility(); } + + setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm + } + + if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) { +#if defined(USE_SAFE_HOME) + findNearestSafeHome(); +#endif + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; + setHome = false; } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddd..220ff277c4 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -108,15 +108,35 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(void) +static bool shouldResetReferenceAltitude(uint8_t updateSource) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); - case NAV_RESET_ON_EACH_ARM: - return !ARMING_FLAG(ARMED); + /* Altitude reference reset constantly before first arm. + * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) + * Need to check reset status for both altitude sources immediately after rearming before reset complete */ + + static bool resetAltitudeOnArm = false; + if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { + static uint8_t sourceCheck = 0; + sourceCheck |= updateSource; + bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); + + if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + resetAltitudeOnArm = false; + sourceCheck = 0; + } + return !STATE(IN_FLIGHT_EMERG_REARM); + } + + if (!ARMING_FLAG(ARMED)) { + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + break; + case NAV_RESET_ON_EACH_ARM: + resetAltitudeOnArm = true; + } + return !ARMING_FLAG(WAS_EVER_ARMED); } return false; @@ -226,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude()) { + else if (shouldResetReferenceAltitude(SENSOR_GPS)) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -309,7 +329,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude()) { + if (shouldResetReferenceAltitude(SENSOR_BARO)) { initialBaroAltitudeOffset = newBaroAlt; } From a181fc7f4eca15e8ff5e29fe54d3dd22dbb12eac Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 28 Sep 2023 13:19:10 +0100 Subject: [PATCH 37/62] fixes - rearm check sequence + altitude reset --- src/main/fc/fc_core.c | 7 +++---- src/main/io/osd.c | 2 +- src/main/navigation/navigation_pos_estimator.c | 3 +++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 66c275366b..1272ad3aa1 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -553,10 +553,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -568,8 +568,6 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } - resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight - lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); @@ -578,6 +576,7 @@ void tryArm(void) ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); if (!STATE(IN_FLIGHT_EMERG_REARM)) { + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b34751096e..3e464f2d76 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1202,7 +1202,7 @@ int32_t osdGetAltitude(void) static inline int32_t osdGetAltitudeMsl(void) { - return getEstimatedActualPosition(Z)+GPS_home.alt; + return getEstimatedActualPosition(Z) + posControl.gpsOrigin.alt; } uint16_t osdGetRemainingGlideTime(void) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 220ff277c4..4422d6fe9e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -121,6 +121,9 @@ static bool shouldResetReferenceAltitude(uint8_t updateSource) bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; + } resetAltitudeOnArm = false; sourceCheck = 0; } From 3c6128e8eb0bd55349eaa21b89e9d9f9f5413298 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 1 Oct 2023 15:30:35 +0100 Subject: [PATCH 38/62] simplify alt ref reset + fix 0 home alt --- src/main/navigation/navigation.c | 4 ++ .../navigation/navigation_pos_estimator.c | 54 +++++++++---------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0465eceab6..81c9b4b85c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2582,6 +2582,10 @@ void updateHomePosition(void) findNearestSafeHome(); #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + + if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm + } // save the current location in case it is replaced by a safehome or HOME_RESET posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; setHome = false; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4422d6fe9e..d7ef73c938 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -54,6 +54,7 @@ #include "sensors/opflow.h" navigationPosEstimator_t posEstimator; +static float initialBaroAltitudeOffset = 0.0f; PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); @@ -108,38 +109,34 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(uint8_t updateSource) +static bool shouldResetReferenceAltitude(void) { - /* Altitude reference reset constantly before first arm. - * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) - * Need to check reset status for both altitude sources immediately after rearming before reset complete */ + /* Reference altitudes reset constantly when disarmed. + * On arming ref altitudes saved as backup in case of emerg in flight rearm + * If emerg in flight rearm active ref altitudes reset to backup values to avoid unwanted altitude reset */ - static bool resetAltitudeOnArm = false; - if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { - static uint8_t sourceCheck = 0; - sourceCheck |= updateSource; - bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); + static float backupInitialBaroAltitudeOffset = 0.0f; + static int32_t backupGpsOriginAltitude = 0; + static bool emergRearmResetCheck = false; - if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { - if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { - posControl.rthState.homePosition.pos.z = 0; - } - resetAltitudeOnArm = false; - sourceCheck = 0; + if (ARMING_FLAG(ARMED) && emergRearmResetCheck) { + if (STATE(IN_FLIGHT_EMERG_REARM)) { + initialBaroAltitudeOffset = backupInitialBaroAltitudeOffset; + posControl.gpsOrigin.alt = backupGpsOriginAltitude; + } else { + backupInitialBaroAltitudeOffset = initialBaroAltitudeOffset; + backupGpsOriginAltitude = posControl.gpsOrigin.alt; } - return !STATE(IN_FLIGHT_EMERG_REARM); } + emergRearmResetCheck = !ARMING_FLAG(ARMED); - if (!ARMING_FLAG(ARMED)) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - break; - case NAV_RESET_ON_EACH_ARM: - resetAltitudeOnArm = true; - } - return !ARMING_FLAG(WAS_EVER_ARMED); + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); + case NAV_RESET_ON_EACH_ARM: + return !ARMING_FLAG(ARMED); } return false; @@ -249,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude(SENSOR_GPS)) { + else if (shouldResetReferenceAltitude()) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -328,11 +325,10 @@ void onNewGPSData(void) */ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) { - static float initialBaroAltitudeOffset = 0.0f; float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude(SENSOR_BARO)) { + if (shouldResetReferenceAltitude()) { initialBaroAltitudeOffset = newBaroAlt; } From f3109ad8ccb457cef66408542f972ffa2f14dc30 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 1 Oct 2023 16:55:28 +0100 Subject: [PATCH 39/62] Fix for Mac SITL --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b9b9b9c0ba..f96611da44 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3945,7 +3945,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { if (usePilotLogo && !singular) { logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2; } else { - logoColOffset = floor((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); + logoColOffset = floorf((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f); } // Draw INAV logo From ae6b6aa7c0ea070cc5ebf452cfba007039c0a96c Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:32:13 +0200 Subject: [PATCH 40/62] docker build: do not run user commands --- Dockerfile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index 9c816b0c18..e0a3949517 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,15 +4,15 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build RUN pip install pyyaml # if either of these are already set the same as the user's machine, leave them be and ignore the error -RUN addgroup --gid $GROUP_ID inav; exit 0; -RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; +RUN if [ -n "$USER_ID" ]; then RUN addgroup --gid $GROUP_ID inav; exit 0; fi +RUN if [ -n "$USER_ID" ]; then RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; fi -USER inav +RUN if [ -n "$USER_ID" ]; then USER inav; fi RUN git config --global --add safe.directory /src VOLUME /src From 55c1abea376acb8958c06939b4800b53a012fa09 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:32:35 +0200 Subject: [PATCH 41/62] docked build: build with ninja --- cmake/docker.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/docker.sh b/cmake/docker.sh index 3c10ebc8e1..843e03a48a 100755 --- a/cmake/docker.sh +++ b/cmake/docker.sh @@ -6,7 +6,7 @@ CURR_REV="$(git rev-parse HEAD)" initialize_cmake() { echo -e "*** CMake was not initialized yet, doing it now.\n" - cmake .. + cmake -GNinja .. echo "$CURR_REV" > "$LAST_CMAKE_AT_REV_FILE" } @@ -26,4 +26,4 @@ else fi # Let Make handle the arguments coming from the build script -make "$@" +ninja "$@" From ff947cb7439f945b7fe532d66090ebfed179f743 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:33:30 +0200 Subject: [PATCH 42/62] docked build: added scripts for sitl build and run --- cmake/docker_build_sitl.sh | 6 ++++++ cmake/docker_run_sitl.sh | 8 ++++++++ 2 files changed, 14 insertions(+) create mode 100644 cmake/docker_build_sitl.sh create mode 100644 cmake/docker_run_sitl.sh diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh new file mode 100644 index 0000000000..b1a4ada5ca --- /dev/null +++ b/cmake/docker_build_sitl.sh @@ -0,0 +1,6 @@ +#!/bin/bash +rm -r build_SITL +mkdir -p build_SITL +cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cd build_SITL +ninja \ No newline at end of file diff --git a/cmake/docker_run_sitl.sh b/cmake/docker_run_sitl.sh new file mode 100644 index 0000000000..b2089137cc --- /dev/null +++ b/cmake/docker_run_sitl.sh @@ -0,0 +1,8 @@ +#!/bin/bash +cd build_SITL + +#Lauch SITL - configurator only mode +./inav_7.0.0_SITL + +#Launch SITL - connect to X-Plane. IP address should be host IP address, not 127.0.0.1. Can be found in X-Plane "Network" tab. +#./inav_7.0.0_SITL --sim=xp --simip=192.168.2.105 --simport=49000 \ No newline at end of file From a61a175107b83e0cce0c2e81a491a43cd84da4ea Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 6 Oct 2023 02:11:54 +0300 Subject: [PATCH 43/62] Update Building in Docker.md --- docs/development/Building in Docker.md | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md index 80d661ecf3..39cd887003 100755 --- a/docs/development/Building in Docker.md +++ b/docs/development/Building in Docker.md @@ -37,8 +37,19 @@ You'll have to manually execute the same steps that the build script does: + This step is only needed the first time. 2. `docker run --rm -it -u root -v :/src inav-build ` + Where `` must be replaced with the absolute path of where you cloned this repo (see above), and `` with the name of the target that you want to build. - + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image. + + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above. -3. If you need to update `Settings.md`, run `docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` +3. If you need to update `Settings.md`, run: + +`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` + +4. Building SITL: + +`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v :/src inav-build` + +5. Running SITL: + +`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -u root -v :/src inav-build`. + + SITL command line parameters can be adjusted in `cmake/docker_run_sitl.sh`. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. From ebafe30620ff1acaf597c826b06d0f9325de16c7 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 7 Oct 2023 10:32:10 +0100 Subject: [PATCH 44/62] Bug fix after merge from master --- src/main/io/osd.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 057743d9ca..24a6bc6cb2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4055,7 +4055,7 @@ uint8_t drawStats(uint8_t row) { #ifdef USE_ADC if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:"); - osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4); + osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4, false); displayWrite(osdDisplayPort, statValueX-4, row, string_buffer); displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH); @@ -4066,18 +4066,18 @@ uint8_t drawStats(uint8_t row) { case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_MI; break; case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_NM; break; default: case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3, false); string_buffer[3] = SYM_WH_KM; break; } From bf688bbe5042d4be6202832137d2770e32ecdc6e Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 7 Oct 2023 16:13:27 +0100 Subject: [PATCH 45/62] Removed SBUS from the 24 channel options Changing SBUS to 24 channels will need more work. --- src/main/rx/sbus_channels.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h index e5ec0ff2c9..5e8a6a0268 100644 --- a/src/main/rx/sbus_channels.h +++ b/src/main/rx/sbus_channels.h @@ -20,11 +20,7 @@ #include #include "rx/rx.h" -#ifdef USE_24CHANNELS -#define SBUS_MAX_CHANNEL 26 -#else #define SBUS_MAX_CHANNEL 18 -#endif #define SBUS_FLAG_SIGNAL_LOSS (1 << 2) #define SBUS_FLAG_FAILSAFE_ACTIVE (1 << 3) From 9239b5df06e7eda275b7306d678391cb14976515 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 7 Oct 2023 17:51:58 +0200 Subject: [PATCH 46/62] add gdb to allow debugging in dev containers --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index e0a3949517..71077c8ed9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,7 +4,7 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build gdb RUN pip install pyyaml From 79e14749d5ba369bb4272acc2ddc4decd3a93657 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 7 Oct 2023 17:52:37 +0200 Subject: [PATCH 47/62] add option to build sitl with debug information --- cmake/docker_build_sitl.sh | 3 ++- cmake/sitl.cmake | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh index b1a4ada5ca..a79742ae0f 100644 --- a/cmake/docker_build_sitl.sh +++ b/cmake/docker_build_sitl.sh @@ -1,6 +1,7 @@ #!/bin/bash rm -r build_SITL mkdir -p build_SITL -cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +#cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cmake -DSITL=ON -DDEBUG=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. cd build_SITL ninja \ No newline at end of file diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 24a4ae9b27..78697f98a9 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -53,6 +53,11 @@ set(SITL_COMPILE_OPTIONS -funsigned-char ) +if(DEBUG) + message(STATUS "Debug mode enabled. Adding -g to SITL_COMPILE_OPTIONS.") + list(APPEND SITL_COMPILE_OPTIONS -g) +endif() + if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr From 6453c3b43f65b281f21474e8f37fa0fdca0bf69b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 13:35:46 +0200 Subject: [PATCH 48/62] Remove not needed comment --- src/main/flight/ez_tune.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 2703fe870e..ef56143cf9 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -65,7 +65,6 @@ static float getYawPidScale(float input) { /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed - * FIXME call on EZTune settings change */ void ezTuneUpdate(void) { if (ezTune()->enabled) { From f364d3bb60d53e5bb0bd9cecab91d4e12fec3ea9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 14:27:53 +0200 Subject: [PATCH 49/62] Compute rate from EZ Tune --- src/main/flight/ez_tune.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index ef56143cf9..b08e7cf9ca 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -32,6 +32,7 @@ #include "fc/settings.h" #include "flight/pid.h" #include "sensors/gyro.h" +#include "fc/controlrate_profile.h" PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); @@ -127,5 +128,14 @@ void ezTuneUpdate(void) { pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability); pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping); pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); + + //Setup rates + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10; + + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + } } \ No newline at end of file From 059a46daad1551e39fca49ee8f60567a9972efd0 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 17:39:54 +0200 Subject: [PATCH 50/62] Docs --- docs/Settings.md | 90 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index c919327194..ac5df304b4 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -872,6 +872,96 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas --- +### ez_aggressiveness + +EzTune aggressiveness + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_axis_ratio + +EzTune axis ratio + +| Default | Min | Max | +| --- | --- | --- | +| 110 | 25 | 175 | + +--- + +### ez_damping + +EzTune damping + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_enabled + +Enables EzTune feature + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### ez_expo + +EzTune expo + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_filter_hz + +EzTune filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 110 | 10 | 300 | + +--- + +### ez_rate + +EzTune rate + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_response + +EzTune response + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_stability + +EzTune stability + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + ### failsafe_delay Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). From 37e1faced5a3ce859afa27e961be6905f1c0fa9c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 17:42:46 +0200 Subject: [PATCH 51/62] Make features aware --- src/main/flight/ez_tune.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index b08e7cf9ca..918c00a122 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -88,12 +88,15 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; +#ifdef USE_DYNAMIC_FILTERS //Enable dynamic notch gyroConfigMutable()->dynamicGyroNotchEnabled = 1; gyroConfigMutable()->dynamicGyroNotchQ = 250; gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT); gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D; +#endif +#ifdef USE_GYRO_KALMAN //Make sure Kalman filter is enabled gyroConfigMutable()->kalmanEnabled = 1; if (ezTune()->filterHz < 150) { @@ -101,6 +104,7 @@ void ezTuneUpdate(void) { } else { gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); } +#endif //Disable dynamic LPF gyroConfigMutable()->useDynamicLpf = 0; From e63fa473a0f55ec988932b735187536a6f182ccc Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 9 Oct 2023 17:02:10 +0900 Subject: [PATCH 52/62] minor changes on ahrs --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/imu.c | 15 ++++++--------- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c919327194..b8e47c57f1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -258,7 +258,7 @@ Inertia force compensation method when gps is avaliable, VELNED use the acclerat | Default | Min | Max | | --- | --- | --- | -| VELNED | | | +| ADAPTIVE | | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 261bf14a56..37fb701af3 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1452,7 +1452,7 @@ groups: type: bool - name: ahrs_inertia_comp_method description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop" - default_value: VELNED + default_value: ADAPTIVE field: inertia_comp_method table: imu_inertia_comp_method diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c9d0f0aacb..43673e790d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -719,6 +719,7 @@ static void imuCalculateEstimatedAttitude(float dT) } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { + //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); @@ -726,21 +727,17 @@ static void imuCalculateEstimatedAttitude(float dT) fpVector3_t compansatedGravityBF_turnrate; vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); - if (velned_magnitude > turnrate_magnitude) - { - compansatedGravityBF = compansatedGravityBF_turnrate; - } - else - { - compansatedGravityBF = compansatedGravityBF_velned; - } + + compansatedGravityBF = velned_magnitude > turnrate_magnitude? compansatedGravityBF_turnrate:compansatedGravityBF_velned; } - else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy()) + else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { + //velned centrifugal force compensation, quad will use this method vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); } else if (STATE(AIRPLANE)) { + //turnrate centrifugal force compensation vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); } else From 704ca50f5826d5a4ec9c4410c2a879e1379c94a3 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 9 Oct 2023 17:24:26 +0900 Subject: [PATCH 53/62] value name change --- src/main/flight/imu.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 43673e790d..ed5cbcbfaa 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -722,13 +722,13 @@ static void imuCalculateEstimatedAttitude(float dT) //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); - float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); + float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); fpVector3_t compansatedGravityBF_turnrate; vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); - float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); + float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); - compansatedGravityBF = velned_magnitude > turnrate_magnitude? compansatedGravityBF_turnrate:compansatedGravityBF_velned; + compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned; } else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { From ee2ff11261486d346c7249562e244516c7f82173 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 9 Oct 2023 14:49:45 +0200 Subject: [PATCH 54/62] Lower default hover throttle to 1300 1500 is usually too high and quad tends to shoot up. throttle around 30% seems closer to reality for most small quads --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c919327194..0276dac744 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3438,7 +3438,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx | Default | Min | Max | | --- | --- | --- | -| 1500 | 1000 | 2000 | +| 1300 | 1000 | 2000 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 261bf14a56..2245b5bcec 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1045,7 +1045,7 @@ groups: max: PWM_RANGE_MAX - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." - default_value: 1500 + default_value: 1300 field: nav.mc.hover_throttle min: 1000 max: 2000 From e5d90339f495d482a55902da3554b2d528a7fcde Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 13 Oct 2023 02:43:57 +0900 Subject: [PATCH 55/62] fixes servo output issue --- src/main/flight/servos.c | 64 ++++++++++++++++++++++------------------ 1 file changed, 36 insertions(+), 28 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 354acfd6af..d3c5da8c75 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -143,34 +143,14 @@ void servoComputeScalingFactors(uint8_t servoIndex) { servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f; } -void servosInit(void) +void computeServoCount(void) { - // give all servos a default command - for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = servoParams(i)->middle; + static bool firstRun = true; + if (!firstRun) { + return; } - - /* - * load mixer - */ - loadCustomServoMixer(); - - // If there are servo rules after all, update variables - if (servoRuleCount > 0) { - servoOutputEnabled = true; - mixerUsesServos = true; - } - - for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servoComputeScalingFactors(i); - } -} - -int getServoCount(void) -{ - bool servoRuleDetected = false; - minServoIndex = 0; - maxServoIndex = 255; + minServoIndex = 255; + maxServoIndex = 0; for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { for (int i = 0; i < MAX_SERVO_RULES; i++) { // check if done @@ -184,10 +164,38 @@ int getServoCount(void) if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) { maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel; } - servoRuleDetected = true; + mixerUsesServos = true; } } - if (servoRuleDetected) { + firstRun = false; +} + +void servosInit(void) +{ + // give all servos a default command + for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = servoParams(i)->middle; + } + + /* + * load mixer + */ + computeServoCount(); + loadCustomServoMixer(); + + // If there are servo rules after all, update variables + if (mixerUsesServos) { + servoOutputEnabled = true; + } + + for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servoComputeScalingFactors(i); + } +} + +int getServoCount(void) +{ + if (mixerUsesServos) { return 1 + maxServoIndex - minServoIndex; } else { From afd2848d791baf733ea39446ce62d85857a3cd8a Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Sat, 14 Oct 2023 08:37:50 +0800 Subject: [PATCH 56/62] add icm42605 driver for SKYSTARSF405HD --- src/main/target/SKYSTARSF405HD/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index ac88a72084..b8a696e14d 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -45,6 +45,11 @@ #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + // *************** M25P256 flash ******************** #define USE_FLASHFS #define USE_FLASH_M25P16 From 5e608b7f66af313b5e95aa229010dd89b15c2484 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 19 Oct 2023 17:18:45 +0100 Subject: [PATCH 57/62] Update navigation.c --- src/main/navigation/navigation.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4a871de084..69a9c0479a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -949,7 +949,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - + /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/ [NAV_STATE_MIXERAT_INITIALIZE] = { .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE, @@ -992,7 +992,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - + } }, }; @@ -1514,7 +1514,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - + if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -3919,14 +3919,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // CRUISE has priority over COURSE_HOLD and AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } // PH has priority over COURSE_HOLD // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } From 3f5b320dd6ad708c983ce83c5f5d4fcf1755bb45 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Oct 2023 11:31:54 +0200 Subject: [PATCH 58/62] Fix antialiasing filter freqency calculation --- src/main/flight/ez_tune.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 918c00a122..0f38b39211 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -71,11 +71,10 @@ void ezTuneUpdate(void) { if (ezTune()->enabled) { // Setup filtering - //Enable Smith predictor pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); - + //Set Dterm LPF pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); pidProfileMutable()->dterm_lpf_type = FILTER_PT2; @@ -85,7 +84,8 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); + const int gyroNyquist = 1000000 / TASK_GYRO_LOOPTIME; + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), (int)(gyroNyquist * 0.5f)); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; #ifdef USE_DYNAMIC_FILTERS From 927597c3c903196faca0e65f2fa3916efa668121 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Oct 2023 12:09:42 +0200 Subject: [PATCH 59/62] Imrove processing --- src/main/flight/ez_tune.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 0f38b39211..c6c57afbfe 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -54,7 +54,7 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, #define EZ_TUNE_YAW_SCALE 0.5f static float computePt1FilterDelayMs(uint8_t filterHz) { - return 1.0f / (2.0f * M_PIf * filterHz); + return 1000.0f / (2.0f * M_PIf * filterHz); } static float getYawPidScale(float input) { @@ -71,10 +71,6 @@ void ezTuneUpdate(void) { if (ezTune()->enabled) { // Setup filtering - //Enable Smith predictor - pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) - + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); - //Set Dterm LPF pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); pidProfileMutable()->dterm_lpf_type = FILTER_PT2; @@ -84,10 +80,12 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - const int gyroNyquist = 1000000 / TASK_GYRO_LOOPTIME; - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), (int)(gyroNyquist * 0.5f)); + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT; gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; + //Enable Smith predictor + pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz); + #ifdef USE_DYNAMIC_FILTERS //Enable dynamic notch gyroConfigMutable()->dynamicGyroNotchEnabled = 1; From 3456d60e9a4125859eded11d5a05e5fdcb0002c3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 17:30:18 +0200 Subject: [PATCH 60/62] Add SYM_TOTAL -> BF_SYM_TOTAL_DISTANCE mapping --- src/main/io/displayport_msp_bf_compat.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 7cfa8f1bf6..b22c4affe9 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -187,13 +187,10 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ALT_M: return BF_SYM_M; -/* case SYM_TRIP_DIST: - return BF_SYM_TRIP_DIST; - case SYM_TOTAL: - return BF_SYM_TOTAL; - + return BF_SYM_TOTAL_DISTANCE; +/* case SYM_ALT_KM: return BF_SYM_ALT_KM; From 201398e7517178de03a421c43933787ec171b71a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 17:34:54 +0200 Subject: [PATCH 61/62] SYM_TOTAL and SYM_TRIP_DISTANCE are the same symbol --- src/main/io/displayport_msp_bf_compat.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index b22c4affe9..a9831929a7 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -187,7 +187,6 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ALT_M: return BF_SYM_M; - case SYM_TRIP_DIST: case SYM_TOTAL: return BF_SYM_TOTAL_DISTANCE; /* From ab20702dff3c342b31e9a24daecfb272b0e47fd7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 22:48:22 +0200 Subject: [PATCH 62/62] Add G_FORCE symbol as letter G --- src/main/io/displayport_msp_bf_compat.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index a9831929a7..315ac9a90c 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -330,10 +330,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_PITCH_DOWN: return BF_SYM_PITCH_DOWN; + */ case SYM_GFORCE: - return BF_SYM_GFORCE; + return 'G'; +/* case SYM_GFORCE_X: return BF_SYM_GFORCE_X;