?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Adding altitude / sea (MSL)
[inav.git] / src / main / io / osd.c
blobae3dd35b9401cfa91d7280fab664530ad8c3070a
1 /*
2 * This file is part of Cleanflight.
3 *
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 /*
19 Created by Marcin Baliniak
20 some functions based on MinimOSD
21
22 OSD-CMS separation by jflyper
23 */
24
25 #include <stdbool.h>
26 #include <stdint.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <ctype.h>
30 #include <math.h>
31
32 #include "platform.h"
33
34 #ifdef USE_OSD
35
36 #include "build/debug.h"
37 #include "build/version.h"
38
39 #include "cms/cms.h"
40 #include "cms/cms_types.h"
41 #include "cms/cms_menu_osd.h"
42
43 #include "common/axis.h"
44 #include "common/filter.h"
45 #include "common/printf.h"
46 #include "common/string_light.h"
47 #include "common/time.h"
48 #include "common/typeconversion.h"
49 #include "common/utils.h"
50
51 #include "config/feature.h"
52 #include "config/parameter_group.h"
53 #include "config/parameter_group_ids.h"
54
55 #include "drivers/display.h"
56 #include "drivers/max7456_symbols.h"
57 #include "drivers/time.h"
58 #include "drivers/vtx_common.h"
59
60 #include "io/flashfs.h"
61 #include "io/gps.h"
62 #include "io/osd.h"
63 #include "io/vtx_string.h"
64
65 #include "fc/config.h"
66 #include "fc/controlrate_profile.h"
67 #include "fc/fc_core.h"
68 #include "fc/fc_tasks.h"
69 #include "fc/rc_adjustments.h"
70 #include "fc/rc_controls.h"
71 #include "fc/rc_modes.h"
72 #include "fc/runtime_config.h"
73 #include "fc/settings.h"
74
75 #include "flight/imu.h"
76 #include "flight/mixer.h"
77 #include "flight/pid.h"
78 #include "flight/rth_estimator.h"
79 #include "flight/wind_estimator.h"
80
81 #include "navigation/navigation.h"
82
83 #include "rx/rx.h"
84
85 #include "sensors/battery.h"
86 #include "sensors/boardalignment.h"
87 #include "sensors/diagnostics.h"
88 #include "sensors/sensors.h"
89 #include "sensors/pitotmeter.h"
90 #include "sensors/temperature.h"
91
92 #ifdef USE_HARDWARE_REVISION_DETECTION
93 #include "hardware_revision.h"
94 #endif
95
96 #define VIDEO_BUFFER_CHARS_PAL 480
97 #define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL)
98
99 #define CENTIMETERS_TO_CENTIFEET(cm) (cm * (328 / 100.0))
100 #define CENTIMETERS_TO_FEET(cm) (cm * (328 / 10000.0))
101 #define CENTIMETERS_TO_METERS(cm) (cm / 100)
102 #define FEET_PER_MILE 5280
103 #define FEET_PER_KILOFEET 1000 // Used for altitude
104 #define METERS_PER_KILOMETER 1000
105 #define METERS_PER_MILE 1609
106
107 #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
108
109 #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms
110 #define ARMED_SCREEN_DISPLAY_TIME 1500 // ms
111 #define STATS_SCREEN_DISPLAY_TIME 60000 // ms
112
113 #define EFFICIENCY_UPDATE_INTERVAL (5 * 1000)
114
115 // Adjust OSD_MESSAGE's default position when
116 // changing OSD_MESSAGE_LENGTH
117 #define OSD_MESSAGE_LENGTH 28
118 #define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
119 #define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
120 // Wrap all string constants intenteded for display as messages with
121 // this macro to ensure compile time length validation.
122 #define OSD_MESSAGE_STR(x) ({ \
123 STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \
124 x; \
125 })
126
127 static unsigned currentLayout = 0;
128 static int layoutOverride = -1;
129
130 typedef struct statistic_s {
131 uint16_t max_speed;
132 uint16_t min_voltage; // /100
133 int16_t max_current; // /100
134 int16_t max_power; // /100
135 int16_t min_rssi;
136 int32_t max_altitude;
137 uint16_t max_distance;
138 } statistic_t;
139
140 static statistic_t stats;
141
142 typedef enum {
143 OSD_SIDEBAR_ARROW_NONE,
144 OSD_SIDEBAR_ARROW_UP,
145 OSD_SIDEBAR_ARROW_DOWN,
146 } osd_sidebar_arrow_e;
147
148 typedef struct osd_sidebar_s {
149 int32_t offset;
150 timeMs_t updated;
151 osd_sidebar_arrow_e arrow;
152 uint8_t idle;
153 } osd_sidebar_t;
154
155 static timeUs_t resumeRefreshAt = 0;
156 static bool refreshWaitForResumeCmdRelease;
157
158 static bool fullRedraw = false;
159
160 static uint8_t armState;
161
162 static displayPort_t *osdDisplayPort;
163
164 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
165 #define AH_HEIGHT 9
166 #define AH_WIDTH 11
167 #define AH_PREV_SIZE (AH_WIDTH > AH_HEIGHT ? AH_WIDTH : AH_HEIGHT)
168 #define AH_H_SYM_COUNT 9
169 #define AH_V_SYM_COUNT 6
170 #define AH_SIDEBAR_WIDTH_POS 7
171 #define AH_SIDEBAR_HEIGHT_POS 3
172
173 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5);
174
175 static int digitCount(int32_t value)
176 {
177 int digits = 1;
178 while(1) {
179 value = value / 10;
180 if (value == 0) {
181 break;
182 }
183 digits++;
184 }
185 return digits;
186 }
187
188 /**
189 * Formats a number given in cents, to support non integer values
190 * without using floating point math. Value is always right aligned
191 * and spaces are inserted before the number to always yield a string
192 * of the same length. If the value doesn't fit into the provided length
193 * it will be divided by scale and true will be returned.
194 */
195 static bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length)
196 {
197 char *ptr = buff;
198 char *dec;
199 int decimals = maxDecimals;
200 bool negative = false;
201 bool scaled = false;
202
203 buff[length] = '\0';
204
205 if (centivalue < 0) {
206 negative = true;
207 centivalue = -centivalue;
208 length--;
209 }
210
211 int32_t integerPart = centivalue / 100;
212 // 3 decimal digits
213 int32_t millis = (centivalue % 100) * 10;
214
215 int digits = digitCount(integerPart);
216 int remaining = length - digits;
217
218 if (remaining < 0 && scale > 0) {
219 // Reduce by scale
220 scaled = true;
221 decimals = maxScaledDecimals;
222 integerPart = integerPart / scale;
223 // Multiply by 10 to get 3 decimal digits
224 millis = ((centivalue % (100 * scale)) * 10) / scale;
225 digits = digitCount(integerPart);
226 remaining = length - digits;
227 }
228
229 // 3 decimals at most
230 decimals = MIN(remaining, MIN(decimals, 3));
231 remaining -= decimals;
232
233 // Done counting. Time to write the characters.
234
235 // Write spaces at the start
236 while (remaining > 0) {
237 *ptr = SYM_BLANK;
238 ptr++;
239 remaining--;
240 }
241
242 // Write the minus sign if required
243 if (negative) {
244 *ptr = '-';
245 ptr++;
246 }
247 // Now write the digits.
248 ui2a(integerPart, 10, 0, ptr);
249 ptr += digits;
250 if (decimals > 0) {
251 *(ptr-1) += SYM_ZERO_HALF_TRAILING_DOT - '0';
252 dec = ptr;
253 int factor = 3; // we're getting the decimal part in millis first
254 while (decimals < factor) {
255 factor--;
256 millis /= 10;
257 }
258 int decimalDigits = digitCount(millis);
259 while (decimalDigits < decimals) {
260 decimalDigits++;
261 *ptr = '0';
262 ptr++;
263 }
264 ui2a(millis, 10, 0, ptr);
265 *dec += SYM_ZERO_HALF_LEADING_DOT - '0';
266 }
267 return scaled;
268 }
269
270 /**
271 * Converts distance into a string based on the current unit system
272 * prefixed by a a symbol to indicate the unit used.
273 * @param dist Distance in centimeters
274 */
275 static void osdFormatDistanceSymbol(char *buff, int32_t dist)
276 {
277 switch ((osd_unit_e)osdConfig()->units) {
278 case OSD_UNIT_IMPERIAL:
279 if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) {
280 buff[0] = SYM_DIST_MI;
281 } else {
282 buff[0] = SYM_DIST_FT;
283 }
284 break;
285 case OSD_UNIT_UK:
286 FALLTHROUGH;
287 case OSD_UNIT_METRIC:
288 if (osdFormatCentiNumber(buff + 1, dist, METERS_PER_KILOMETER, 0, 3, 3)) {
289 buff[0] = SYM_DIST_KM;
290 } else {
291 buff[0] = SYM_DIST_M;
292 }
293 break;
294 }
295 }
296
297 /**
298 * Converts distance into a string based on the current unit system.
299 * @param dist Distance in centimeters
300 */
301 static void osdFormatDistanceStr(char *buff, int32_t dist)
302 {
303 int32_t centifeet;
304 switch ((osd_unit_e)osdConfig()->units) {
305 case OSD_UNIT_IMPERIAL:
306 centifeet = CENTIMETERS_TO_CENTIFEET(dist);
307 if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
308 // Show feet when dist < 0.5mi
309 tfp_sprintf(buff, "%d%c", centifeet / 100, SYM_FT);
310 } else {
311 // Show miles when dist >= 0.5mi
312 tfp_sprintf(buff, "%d.%02d%c", centifeet / (100*FEET_PER_MILE),
313 (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI);
314 }
315 break;
316 case OSD_UNIT_UK:
317 FALLTHROUGH;
318 case OSD_UNIT_METRIC:
319 if (abs(dist) < METERS_PER_KILOMETER * 100) {
320 // Show meters when dist < 1km
321 tfp_sprintf(buff, "%d%c", dist / 100, SYM_M);
322 } else {
323 // Show kilometers when dist >= 1km
324 tfp_sprintf(buff, "%d.%02d%c", dist / (100*METERS_PER_KILOMETER),
325 (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
326 }
327 break;
328 }
329 }
330
331 /**
332 * Converts velocity based on the current unit system (kmh or mph).
333 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
334 */
335 static int32_t osdConvertVelocityToUnit(int32_t vel)
336 {
337 switch ((osd_unit_e)osdConfig()->units) {
338 case OSD_UNIT_UK:
339 FALLTHROUGH;
340 case OSD_UNIT_IMPERIAL:
341 return (vel * 224) / 10000; // Convert to mph
342 case OSD_UNIT_METRIC:
343 return (vel * 36) / 1000; // Convert to kmh
344 }
345 // Unreachable
346 return -1;
347 }
348
349 /**
350 * Converts velocity into a string based on the current unit system.
351 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
352 */
353 static void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D)
354 {
355 switch ((osd_unit_e)osdConfig()->units) {
356 case OSD_UNIT_UK:
357 FALLTHROUGH;
358 case OSD_UNIT_IMPERIAL:
359 tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH));
360 break;
361 case OSD_UNIT_METRIC:
362 tfp_sprintf(buff, "%3d%c", osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH));
363 break;
364 }
365 }
366
367 /**
368 * Converts wind speed into a string based on the current unit system, using
369 * always 3 digits and an additional character for the unit at the right. buff
370 * is null terminated.
371 * @param ws Raw wind speed in cm/s
372 */
373 #ifdef USE_WIND_ESTIMATOR
374 static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
375 {
376 int32_t centivalue;
377 char suffix;
378 switch (osdConfig()->units) {
379 case OSD_UNIT_UK:
380 FALLTHROUGH;
381 case OSD_UNIT_IMPERIAL:
382 centivalue = (ws * 224) / 100;
383 suffix = SYM_MPH;
384 break;
385 case OSD_UNIT_METRIC:
386 centivalue = (ws * 36) / 10;
387 suffix = SYM_KMH;
388 break;
389 }
390 if (isValid) {
391 osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
392 } else {
393 buff[0] = buff[1] = buff[2] = '-';
394 }
395 buff[3] = suffix;
396 buff[4] = '\0';
397 }
398 #endif
399
400 /**
401 * Converts altitude into a string based on the current unit system
402 * prefixed by a a symbol to indicate the unit used.
403 * @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters)
404 */
405 static void osdFormatAltitudeSymbol(char *buff, int32_t alt)
406 {
407 switch ((osd_unit_e)osdConfig()->units) {
408 case OSD_UNIT_IMPERIAL:
409 if (osdFormatCentiNumber(buff + 1, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, 3)) {
410 // Scaled to kft
411 buff[0] = SYM_ALT_KFT;
412 } else {
413 // Formatted in feet
414 buff[0] = SYM_ALT_FT;
415 }
416 break;
417 case OSD_UNIT_UK:
418 FALLTHROUGH;
419 case OSD_UNIT_METRIC:
420 // alt is alredy in cm
421 if (osdFormatCentiNumber(buff+1, alt, 1000, 0, 2, 3)) {
422 // Scaled to km
423 buff[0] = SYM_ALT_KM;
424 } else {
425 // Formatted in m
426 buff[0] = SYM_ALT_M;
427 }
428 break;
429 }
430 }
431
432 /**
433 * Converts altitude into a string based on the current unit system.
434 * @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters)
435 */
436 static void osdFormatAltitudeStr(char *buff, int32_t alt)
437 {
438 int32_t value;
439 switch ((osd_unit_e)osdConfig()->units) {
440 case OSD_UNIT_IMPERIAL:
441 value = CENTIMETERS_TO_FEET(alt);
442 tfp_sprintf(buff, "%d%c", value, SYM_FT);
443 break;
444 case OSD_UNIT_UK:
445 FALLTHROUGH;
446 case OSD_UNIT_METRIC:
447 value = CENTIMETERS_TO_METERS(alt);
448 tfp_sprintf(buff, "%d%c", value, SYM_M);
449 break;
450 }
451 }
452
453 static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h)
454 {
455 uint32_t value = seconds;
456 char sym = sym_m;
457 // Maximum value we can show in minutes is 99 minutes and 59 seconds
458 if (seconds > (99 * 60) + 59) {
459 sym = sym_h;
460 value = seconds / 60;
461 }
462 buff[0] = sym;
463 tfp_sprintf(buff + 1, "%02d:%02d", value / 60, value % 60);
464 }
465
466 static inline void osdFormatOnTime(char *buff)
467 {
468 osdFormatTime(buff, micros() / 1000000, SYM_ON_M, SYM_ON_H);
469 }
470
471 static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
472 {
473 uint32_t seconds = getFlightTime();
474 osdFormatTime(buff, seconds, SYM_FLY_M, SYM_FLY_H);
475 if (attr && osdConfig()->time_alarm > 0) {
476 if (seconds / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED)) {
477 TEXT_ATTRIBUTES_ADD_BLINK(*attr);
478 }
479 }
480 }
481
482 /**
483 * Converts RSSI into a % value used by the OSD.
484 */
485 static uint16_t osdConvertRSSI(void)
486 {
487 // change range to [0, 99]
488 return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
489 }
490
491 /**
492 * Converts temperature into a string based on the current unit system
493 * postfixed with a symbol to indicate the unit used.
494 * @param temperature Raw temperature (i.e. as taken from getCurrentTemperature() in degC)
495 */
496 static void osdFormatTemperatureSymbol(char *buff, float temperature)
497 {
498 int units_symbol;
499 switch ((osd_unit_e)osdConfig()->units) {
500 case OSD_UNIT_IMPERIAL:
501 units_symbol = SYM_TEMP_F;
502 temperature = (temperature * (9.0f/5)) + 32;
503 break;
504 case OSD_UNIT_UK:
505 FALLTHROUGH;
506 case OSD_UNIT_METRIC:
507 units_symbol = SYM_TEMP_C;
508 break;
509 }
510 osdFormatCentiNumber(buff, (int32_t) (temperature * 100), 0, 0, 0, 3);
511 buff[3] = units_symbol;
512 buff[4] = '\0';
513 }
514
515 static void osdFormatCoordinate(char *buff, char sym, int32_t val)
516 {
517 // up to 4 for number + 1 for the symbol + null terminator + fill the rest with decimals
518 const int coordinateLength = osdConfig()->coordinate_digits + 1;
519
520 buff[0] = sym;
521 int32_t integerPart = val / GPS_DEGREES_DIVIDER;
522 // Latitude maximum integer width is 3 (-90) while
523 // longitude maximum integer width is 4 (-180).
524 int integerDigits = tfp_sprintf(buff + 1, (integerPart == 0 && val < 0) ? "-%d" : "%d", integerPart);
525 // We can show up to 7 digits in decimalPart.
526 int32_t decimalPart = abs(val % GPS_DEGREES_DIVIDER);
527 STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits);
528 int decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", decimalPart);
529 // Embbed the decimal separator
530 buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
531 buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
532 // Fill up to coordinateLength with zeros
533 int total = 1 + integerDigits + decimalDigits;
534 while(total < coordinateLength) {
535 buff[total] = '0';
536 total++;
537 }
538 buff[coordinateLength] = '\0';
539 }
540
541 // Used twice, make sure it's exactly the same string
542 // to save some memory
543 #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
544
545 static const char * osdArmingDisabledReasonMessage(void)
546 {
547 switch (isArmingDisabledReason()) {
548 case ARMING_DISABLED_FAILSAFE_SYSTEM:
549 // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
550 if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
551 if (failsafeIsReceivingRxData()) {
552 if (isUsingSticksForArming()) {
553 // Need to power-cycle
554 return OSD_MESSAGE_STR("POWER CYCLE TO ARM");
555 }
556 // If we're not using sticks, it means the ARM switch
557 // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
558 // yet
559 return OSD_MESSAGE_STR("TURN ARM SWITCH OFF");
560 }
561 // Not receiving RX data
562 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
563 }
564 return OSD_MESSAGE_STR("DISABLED BY FAILSAFE");
565 case ARMING_DISABLED_NOT_LEVEL:
566 return OSD_MESSAGE_STR("AIRCRAFT IS NOT LEVEL");
567 case ARMING_DISABLED_SENSORS_CALIBRATING:
568 return OSD_MESSAGE_STR("SENSORS CALIBRATING");
569 case ARMING_DISABLED_SYSTEM_OVERLOADED:
570 return OSD_MESSAGE_STR("SYSTEM OVERLOADED");
571 case ARMING_DISABLED_NAVIGATION_UNSAFE:
572 return OSD_MESSAGE_STR("NAVIGATION IS UNSAFE");
573 case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
574 return OSD_MESSAGE_STR("COMPASS NOT CALIBRATED");
575 case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
576 return OSD_MESSAGE_STR("ACCELEROMETER NOT CALIBRATED");
577 case ARMING_DISABLED_ARM_SWITCH:
578 return OSD_MESSAGE_STR("DISABLE ARM SWITCH FIRST");
579 case ARMING_DISABLED_HARDWARE_FAILURE:
580 {
581 if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
582 return OSD_MESSAGE_STR("GYRO FAILURE");
583 }
584 if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
585 return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
586 }
587 if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
588 return OSD_MESSAGE_STR("COMPASS FAILURE");
589 }
590 if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
591 return OSD_MESSAGE_STR("BAROMETER FAILURE");
592 }
593 if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
594 return OSD_MESSAGE_STR("GPS FAILURE");
595 }
596 if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
597 return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
598 }
599 if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
600 return OSD_MESSAGE_STR("PITOT METER FAILURE");
601 }
602 }
603 return OSD_MESSAGE_STR("HARDWARE FAILURE");
604 case ARMING_DISABLED_BOXFAILSAFE:
605 return OSD_MESSAGE_STR("FAILSAFE MODE ENABLED");
606 case ARMING_DISABLED_BOXKILLSWITCH:
607 return OSD_MESSAGE_STR("KILLSWITCH MODE ENABLED");
608 case ARMING_DISABLED_RC_LINK:
609 return OSD_MESSAGE_STR("NO RC LINK");
610 case ARMING_DISABLED_THROTTLE:
611 return OSD_MESSAGE_STR("THROTTLE IS NOT LOW");
612 case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
613 return OSD_MESSAGE_STR("ROLLPITCH NOT CENTERED");
614 case ARMING_DISABLED_SERVO_AUTOTRIM:
615 return OSD_MESSAGE_STR("AUTOTRIM IS ACTIVE");
616 case ARMING_DISABLED_OOM:
617 return OSD_MESSAGE_STR("NOT ENOUGH MEMORY");
618 case ARMING_DISABLED_INVALID_SETTING:
619 return OSD_MESSAGE_STR("INVALID SETTING");
620 case ARMING_DISABLED_CLI:
621 return OSD_MESSAGE_STR("CLI IS ACTIVE");
622 // Cases without message
623 case ARMING_DISABLED_CMS_MENU:
624 FALLTHROUGH;
625 case ARMING_DISABLED_OSD_MENU:
626 FALLTHROUGH;
627 case ARMING_DISABLED_ALL_FLAGS:
628 FALLTHROUGH;
629 case ARMED:
630 FALLTHROUGH;
631 case WAS_EVER_ARMED:
632 break;
633 }
634 return NULL;
635 }
636
637 static const char * osdFailsafePhaseMessage(void)
638 {
639 // See failsafe.h for each phase explanation
640 switch (failsafePhase()) {
641 #ifdef USE_NAV
642 case FAILSAFE_RETURN_TO_HOME:
643 // XXX: Keep this in sync with OSD_FLYMODE.
644 return OSD_MESSAGE_STR("(RTH)");
645 #endif
646 case FAILSAFE_LANDING:
647 // This should be considered an emergengy landing
648 return OSD_MESSAGE_STR("(EMERGENCY LANDING)");
649 case FAILSAFE_RX_LOSS_MONITORING:
650 // Only reachable from FAILSAFE_LANDED, which performs
651 // a disarm. Since aircraft has been disarmed, we no
652 // longer show failsafe details.
653 FALLTHROUGH;
654 case FAILSAFE_LANDED:
655 // Very brief, disarms and transitions into
656 // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
657 // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
658 // so we'll show the user how to re-arm in when
659 // that flag is the reason to prevent arming.
660 FALLTHROUGH;
661 case FAILSAFE_RX_LOSS_IDLE:
662 // This only happens when user has chosen NONE as FS
663 // procedure. The recovery messages should be enough.
664 FALLTHROUGH;
665 case FAILSAFE_IDLE:
666 // Failsafe not active
667 FALLTHROUGH;
668 case FAILSAFE_RX_LOSS_DETECTED:
669 // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
670 // or the FS procedure immediately.
671 FALLTHROUGH;
672 case FAILSAFE_RX_LOSS_RECOVERED:
673 // Exiting failsafe
674 break;
675 }
676 return NULL;
677 }
678
679 static const char * osdFailsafeInfoMessage(void)
680 {
681 if (failsafeIsReceivingRxData()) {
682 // User must move sticks to exit FS mode
683 return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
684 }
685 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
686 }
687
688 static const char * navigationStateMessage(void)
689 {
690 switch (NAV_Status.state) {
691 case MW_NAV_STATE_NONE:
692 break;
693 case MW_NAV_STATE_RTH_START:
694 return OSD_MESSAGE_STR("STARTING RTH");
695 case MW_NAV_STATE_RTH_ENROUTE:
696 // TODO: Break this up between climb and head home
697 return OSD_MESSAGE_STR("EN ROUTE TO HOME");
698 case MW_NAV_STATE_HOLD_INFINIT:
699 // Used by HOLD flight modes. No information to add.
700 break;
701 case MW_NAV_STATE_HOLD_TIMED:
702 // Not used anymore
703 break;
704 case MW_NAV_STATE_WP_ENROUTE:
705 // TODO: Show WP number
706 return OSD_MESSAGE_STR("EN ROUTE TO WAYPOINT");
707 case MW_NAV_STATE_PROCESS_NEXT:
708 return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
709 case MW_NAV_STATE_DO_JUMP:
710 // Not used
711 break;
712 case MW_NAV_STATE_LAND_START:
713 // Not used
714 break;
715 case MW_NAV_STATE_EMERGENCY_LANDING:
716 return OSD_MESSAGE_STR("EMERGENCY LANDING");
717 case MW_NAV_STATE_LAND_IN_PROGRESS:
718 return OSD_MESSAGE_STR("LANDING");
719 case MW_NAV_STATE_HOVER_ABOVE_HOME:
720 if (STATE(FIXED_WING)) {
721 return OSD_MESSAGE_STR("LOITERING AROUND HOME");
722 }
723 return OSD_MESSAGE_STR("HOVERING");
724 case MW_NAV_STATE_LANDED:
725 return OSD_MESSAGE_STR("LANDED");
726 case MW_NAV_STATE_LAND_SETTLE:
727 return OSD_MESSAGE_STR("PREPARING TO LAND");
728 case MW_NAV_STATE_LAND_START_DESCENT:
729 // Not used
730 break;
731 }
732 return NULL;
733 }
734
735 static void osdFormatMessage(char *buff, size_t size, const char *message)
736 {
737 memset(buff, SYM_BLANK, size);
738 if (message) {
739 int messageLength = strlen(message);
740 int rem = MAX(0, OSD_MESSAGE_LENGTH - (int)messageLength);
741 // Don't finish the string at the end of the message,
742 // write the rest of the blanks.
743 strncpy(buff + rem / 2, message, MIN(OSD_MESSAGE_LENGTH - rem / 2, messageLength));
744 }
745 // Ensure buff is zero terminated
746 buff[size - 1] = '\0';
747 }
748
749 /**
750 * Draws the battery symbol filled in accordingly to the
751 * battery voltage to buff[0].
752 **/
753 static void osdFormatBatteryChargeSymbol(char *buff)
754 {
755 uint8_t p = calculateBatteryPercentage();
756 p = (100 - p) / 16.6;
757 buff[0] = SYM_BATT_FULL + p;
758 }
759
760 static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *attr)
761 {
762 if ((getBatteryState() != BATTERY_NOT_PRESENT) && ((batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical)) || ((!batteryUsesCapacityThresholds()) && (getBatteryVoltage() <= getBatteryWarningVoltage()))))
763 TEXT_ATTRIBUTES_ADD_BLINK(*attr);
764 }
765
766 static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length)
767 {
768 *x = 14 - 1; // Offset for 1 char to the left
769 *y = 6;
770 if (IS_DISPLAY_PAL) {
771 ++(*y);
772 }
773 int size = 3;
774 if (osdConfig()->crosshairs_style == OSD_CROSSHAIRS_STYLE_AIRCRAFT) {
775 (*x)--;
776 size = 5;
777 }
778 if (length) {
779 *length = size;
780 }
781 }
782
783 /**
784 * Formats throttle position prefixed by its symbol. If autoThr
785 * is true and the navigation system is controlling THR, it
786 * uses the THR value applied by the system rather than the
787 * input value received by the sticks.
788 **/
789 static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t *elemAttr)
790 {
791 buff[0] = SYM_THR;
792 buff[1] = SYM_THR1;
793 int16_t thr = rcData[THROTTLE];
794 if (autoThr && navigationIsControllingThrottle()) {
795 buff[0] = SYM_AUTO_THR0;
796 buff[1] = SYM_AUTO_THR1;
797 thr = rcCommand[THROTTLE];
798 if (isFixedWingAutoThrottleManuallyIncreased())
799 TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
800 }
801 tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
802 }
803
804 static inline int32_t osdGetAltitude(void)
805 {
806 #if defined(USE_NAV)
807 return getEstimatedActualPosition(Z);
808 #elif defined(USE_BARO)
809 return baro.alt;
810 #else
811 return 0;
812 #endif
813 }
814
815 static inline int32_t osdGetAltitudeMsl(void)
816 {
817 #if defined(USE_NAV)
818 return getEstimatedActualPosition(Z)+GPS_home.alt;
819 #elif defined(USE_BARO)
820 return baro.alt+GPS_home.alt;
821 #else
822 return 0;
823 #endif
824 }
825
826 static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs)
827 {
828 // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
829 // Zero scrolling should draw SYM_AH_DECORATION.
830 uint8_t decoration = SYM_AH_DECORATION;
831 int offset;
832 int steps;
833 switch (scroll) {
834 case OSD_SIDEBAR_SCROLL_NONE:
835 sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
836 sidebar->offset = 0;
837 return decoration;
838 case OSD_SIDEBAR_SCROLL_ALTITUDE:
839 // Move 1 char for every 20cm
840 offset = osdGetAltitude();
841 steps = offset / 20;
842 break;
843 case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
844 #if defined(USE_GPS)
845 offset = gpsSol.groundSpeed;
846 #else
847 offset = 0;
848 #endif
849 // Move 1 char for every 20 cm/s
850 steps = offset / 20;
851 break;
852 case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
853 #if defined(USE_GPS)
854 offset = GPS_distanceToHome;
855 #else
856 offset = 0;
857 #endif
858 // Move 1 char for every 5m
859 steps = offset / 5;
860 break;
861 }
862 if (offset) {
863 decoration -= steps % SYM_AH_DECORATION_COUNT;
864 if (decoration > SYM_AH_DECORATION_MAX) {
865 decoration -= SYM_AH_DECORATION_COUNT;
866 } else if (decoration < SYM_AH_DECORATION_MIN) {
867 decoration += SYM_AH_DECORATION_COUNT;
868 }
869 }
870 if (currentTimeMs - sidebar->updated > 100) {
871 if (offset > sidebar->offset) {
872 sidebar->arrow = OSD_SIDEBAR_ARROW_UP;
873 sidebar->idle = 0;
874 } else if (offset < sidebar->offset) {
875 sidebar->arrow = OSD_SIDEBAR_ARROW_DOWN;
876 sidebar->idle = 0;
877 } else {
878 if (sidebar->idle > 3) {
879 sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
880 } else {
881 sidebar->idle++;
882 }
883 }
884 sidebar->offset = offset;
885 sidebar->updated = currentTimeMs;
886 }
887 return decoration;
888 }
889
890 static bool osdIsHeadingValid(void)
891 {
892 return isImuHeadingValid();
893 }
894
895 static int16_t osdGetHeading(void)
896 {
897 return attitude.values.yaw;
898 }
899
900 // Returns a heading angle in degrees normalized to [0, 360).
901 static int osdGetHeadingAngle(int angle)
902 {
903 while (angle < 0) {
904 angle += 360;
905 }
906 while (angle >= 360) {
907 angle -= 360;
908 }
909 return angle;
910 }
911
912 #if defined(USE_GPS)
913
914 /* Draws a map with the given symbol in the center and given point of interest
915 * defined by its distance in meters and direction in degrees.
916 * referenceHeading indicates the up direction in the map, in degrees, while
917 * referenceSym (if non-zero) is drawn at the upper right corner below a small
918 * arrow to indicate the map reference to the user. The drawn argument is an
919 * in-out used to store the last position where the craft was drawn to avoid
920 * erasing all screen on each redraw.
921 */
922 static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t centerSym,
923 uint32_t poiDistance, int16_t poiDirection, uint8_t poiSymbol,
924 uint16_t *drawn, uint32_t *usedScale)
925 {
926 // TODO: These need to be tested with several setups. We might
927 // need to make them configurable.
928 const int hMargin = 1;
929 const int vMargin = 1;
930
931 // TODO: Get this from the display driver?
932 const int charWidth = 12;
933 const int charHeight = 18;
934
935 char buf[16];
936
937 uint8_t minX = hMargin;
938 uint8_t maxX = osdDisplayPort->cols - 1 - hMargin;
939 uint8_t minY = vMargin;
940 uint8_t maxY = osdDisplayPort->rows - 1 - vMargin;
941 uint8_t midX = osdDisplayPort->cols / 2;
942 uint8_t midY = osdDisplayPort->rows / 2;
943
944 // Fixed marks
945 if (referenceSym) {
946 displayWriteChar(osdDisplayPort, maxX, minY, SYM_DIRECTION);
947 displayWriteChar(osdDisplayPort, maxX, minY + 1, referenceSym);
948 }
949 displayWriteChar(osdDisplayPort, minX, maxY, SYM_SCALE);
950 displayWriteChar(osdDisplayPort, midX, midY, centerSym);
951
952 // First, erase the previous drawing.
953 if (OSD_VISIBLE(*drawn)) {
954 displayWriteChar(osdDisplayPort, OSD_X(*drawn), OSD_Y(*drawn), SYM_BLANK);
955 *drawn = 0;
956 }
957
958 uint32_t initialScale;
959 float scaleToUnit;
960 int scaleUnitDivisor;
961 char symUnscaled;
962 char symScaled;
963 int maxDecimals;
964 const unsigned scaleMultiplier = 2;
965 // We try to reduce the scale when the POI will be around half the distance
966 // between the center and the closers map edge, to avoid too much jumping
967 const int scaleReductionMultiplier = MIN(midX - hMargin, midY - vMargin) / 2;
968
969 switch (osdConfig()->units) {
970 case OSD_UNIT_IMPERIAL:
971 initialScale = 16; // 16m ~= 0.01miles
972 scaleToUnit = 100 / 1609.3440f; // scale to 0.01mi for osdFormatCentiNumber()
973 scaleUnitDivisor = 0;
974 symUnscaled = SYM_MI;
975 symScaled = SYM_MI;
976 maxDecimals = 2;
977 break;
978 case OSD_UNIT_UK:
979 FALLTHROUGH;
980 case OSD_UNIT_METRIC:
981 initialScale = 10; // 10m as initial scale
982 scaleToUnit = 100; // scale to cm for osdFormatCentiNumber()
983 scaleUnitDivisor = 1000; // Convert to km when scale gets bigger than 999m
984 symUnscaled = SYM_M;
985 symScaled = SYM_KM;
986 maxDecimals = 0;
987 break;
988 }
989
990 // Try to keep the same scale when getting closer until we draw over the center point
991 uint32_t scale = initialScale;
992 if (*usedScale) {
993 scale = *usedScale;
994 if (scale > initialScale && poiDistance < *usedScale * scaleReductionMultiplier) {
995 scale /= scaleMultiplier;
996 }
997 }
998
999 if (STATE(GPS_FIX)) {
1000
1001 int directionToPoi = osdGetHeadingAngle(poiDirection - referenceHeading);
1002 float poiAngle = DEGREES_TO_RADIANS(directionToPoi);
1003 float poiSin = sin_approx(poiAngle);
1004 float poiCos = cos_approx(poiAngle);
1005
1006 // Now start looking for a valid scale that lets us draw everything
1007 int ii;
1008 for (ii = 0; ii < 50; ii++) {
1009 // Calculate location of the aircraft in map
1010 int points = poiDistance / ((float)scale / charHeight);
1011
1012 float pointsX = points * poiSin;
1013 int poiX = midX - roundf(pointsX / charWidth);
1014 if (poiX < minX || poiX > maxX) {
1015 scale *= scaleMultiplier;
1016 continue;
1017 }
1018
1019 float pointsY = points * poiCos;
1020 int poiY = midY + roundf(pointsY / charHeight);
1021 if (poiY < minY || poiY > maxY) {
1022 scale *= scaleMultiplier;
1023 continue;
1024 }
1025
1026 if (poiX == midX && poiY == midY) {
1027 // We're over the map center symbol, so we would be drawing
1028 // over it even if we increased the scale. Alternate between
1029 // drawing the center symbol or drawing the POI.
1030 if (centerSym != SYM_BLANK && OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
1031 break;
1032 }
1033 } else {
1034
1035 uint8_t c;
1036 if (displayReadCharWithAttr(osdDisplayPort, poiX, poiY, &c, NULL) && c != SYM_BLANK) {
1037 // Something else written here, increase scale. If the display doesn't support reading
1038 // back characters, we assume there's nothing.
1039 //
1040 // If we're close to the center, decrease scale. Otherwise increase it.
1041 uint8_t centerDeltaX = (maxX - minX) / (scaleMultiplier * 2);
1042 uint8_t centerDeltaY = (maxY - minY) / (scaleMultiplier * 2);
1043 if (poiX >= midX - centerDeltaX && poiX <= midX + centerDeltaX &&
1044 poiY >= midY - centerDeltaY && poiY <= midY + centerDeltaY &&
1045 scale > scaleMultiplier) {
1046
1047 scale /= scaleMultiplier;
1048 } else {
1049 scale *= scaleMultiplier;
1050 }
1051 continue;
1052 }
1053 }
1054
1055 // Draw the point on the map
1056 if (poiSymbol == SYM_ARROW_UP) {
1057 // Drawing aircraft, rotate
1058 int mapHeading = osdGetHeadingAngle(DECIDEGREES_TO_DEGREES(osdGetHeading()) - referenceHeading);
1059 poiSymbol += mapHeading * 2 / 45;
1060 }
1061 displayWriteChar(osdDisplayPort, poiX, poiY, poiSymbol);
1062
1063 // Update saved location
1064 *drawn = OSD_POS(poiX, poiY) | OSD_VISIBLE_FLAG;
1065 break;
1066 }
1067 }
1068
1069 // Draw the used scale
1070 bool scaled = osdFormatCentiNumber(buf, scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3);
1071 buf[3] = scaled ? symScaled : symUnscaled;
1072 buf[4] = '\0';
1073 displayWrite(osdDisplayPort, minX + 1, maxY, buf);
1074 *usedScale = scale;
1075 }
1076
1077 /* Draws a map with the home in the center and the craft moving around.
1078 * See osdDrawMap() for reference.
1079 */
1080 static void osdDrawHomeMap(int referenceHeading, uint8_t referenceSym, uint16_t *drawn, uint32_t *usedScale)
1081 {
1082 osdDrawMap(referenceHeading, referenceSym, SYM_HOME, GPS_distanceToHome, GPS_directionToHome, SYM_ARROW_UP, drawn, usedScale);
1083 }
1084
1085 /* Draws a map with the aircraft in the center and the home moving around.
1086 * See osdDrawMap() for reference.
1087 */
1088 static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
1089 {
1090 int16_t reference = DECIDEGREES_TO_DEGREES(osdGetHeading());
1091 int16_t poiDirection = osdGetHeadingAngle(GPS_directionToHome + 180);
1092 osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
1093 }
1094
1095 static int16_t osdGet3DSpeed(void)
1096 {
1097 int16_t vert_speed = getEstimatedActualVelocity(Z);
1098 int16_t hor_speed = gpsSol.groundSpeed;
1099 return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
1100 }
1101
1102 #endif
1103
1104 static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
1105 strcpy(buff, label);
1106 for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' ';
1107 uint8_t decimals = showDecimal ? 1 : 0;
1108 osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4);
1109 buff[9] = ' ';
1110 osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4);
1111 buff[14] = ' ';
1112 osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4);
1113 buff[19] = ' ';
1114 osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4);
1115 buff[24] = '\0';
1116 }
1117
1118 static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_t voltage, uint8_t digits, uint8_t decimals)
1119 {
1120 char buff[6];
1121 textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
1122
1123 osdFormatBatteryChargeSymbol(buff);
1124 buff[1] = '\0';
1125 osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1126 displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
1127
1128 elemAttr = TEXT_ATTRIBUTES_NONE;
1129 osdFormatCentiNumber(buff, voltage, 0, decimals, 0, MIN(digits, 4));
1130 strcat(buff, "V");
1131 if ((getBatteryState() != BATTERY_NOT_PRESENT) && (getBatteryVoltage() <= getBatteryWarningVoltage()))
1132 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1133 displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
1134 }
1135
1136 static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, const pid8_t *pid, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
1137 {
1138 textAttributes_t elemAttr;
1139 char buff[4];
1140
1141 displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
1142
1143 elemAttr = TEXT_ATTRIBUTES_NONE;
1144 tfp_sprintf(buff, "%3d", pid->P);
1145 if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
1146 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1147 displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1148
1149 elemAttr = TEXT_ATTRIBUTES_NONE;
1150 tfp_sprintf(buff, "%3d", pid->I);
1151 if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
1152 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1153 displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
1154
1155 elemAttr = TEXT_ATTRIBUTES_NONE;
1156 tfp_sprintf(buff, "%3d", pid->D);
1157 if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
1158 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1159 displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
1160 }
1161
1162 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) {
1163 char buff[8];
1164 textAttributes_t elemAttr;
1165 displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
1166
1167 elemAttr = TEXT_ATTRIBUTES_NONE;
1168 osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8));
1169 if (isAdjustmentFunctionSelected(adjFunc))
1170 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1171 displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
1172 }
1173
1174 static bool osdDrawSingleElement(uint8_t item)
1175 {
1176 uint16_t pos = osdConfig()->item_pos[currentLayout][item];
1177 if (!OSD_VISIBLE(pos)) {
1178 return false;
1179 }
1180
1181 uint8_t elemPosX = OSD_X(pos);
1182 uint8_t elemPosY = OSD_Y(pos);
1183 textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
1184 char buff[32];
1185
1186 switch (item) {
1187 case OSD_RSSI_VALUE:
1188 {
1189 uint16_t osdRssi = osdConvertRSSI();
1190 buff[0] = SYM_RSSI;
1191 tfp_sprintf(buff + 1, "%2d", osdRssi);
1192 if (osdRssi < osdConfig()->rssi_alarm) {
1193 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1194 }
1195 break;
1196 }
1197
1198 case OSD_MAIN_BATT_VOLTAGE:
1199 osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
1200 return true;
1201
1202 case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE:
1203 osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedVoltage(), 2 + osdConfig()->main_voltage_decimals, osdConfig()->main_voltage_decimals);
1204 return true;
1205
1206 case OSD_CURRENT_DRAW:
1207 buff[0] = SYM_AMP;
1208 osdFormatCentiNumber(buff + 1, getAmperage(), 0, 2, 0, 3);
1209 break;
1210
1211 case OSD_MAH_DRAWN:
1212 buff[0] = SYM_MAH;
1213 tfp_sprintf(buff + 1, "%-4d", getMAhDrawn());
1214 osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1215 break;
1216
1217 case OSD_WH_DRAWN:
1218 buff[0] = SYM_WH;
1219 osdFormatCentiNumber(buff + 1, getMWhDrawn() / 10, 0, 2, 0, 3);
1220 osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1221 break;
1222
1223 case OSD_BATTERY_REMAINING_CAPACITY:
1224 buff[0] = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH);
1225
1226 if (currentBatteryProfile->capacity.value == 0)
1227 tfp_sprintf(buff + 1, "NA");
1228 else if (!batteryWasFullWhenPluggedIn())
1229 tfp_sprintf(buff + 1, "NF");
1230 else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH)
1231 tfp_sprintf(buff + 1, "%-4lu", getBatteryRemainingCapacity());
1232 else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
1233 osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3);
1234
1235 if ((getBatteryState() != BATTERY_NOT_PRESENT) && batteryUsesCapacityThresholds() && (getBatteryRemainingCapacity() <= currentBatteryProfile->capacity.warning - currentBatteryProfile->capacity.critical))
1236 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1237
1238 break;
1239
1240 case OSD_BATTERY_REMAINING_PERCENT:
1241 tfp_sprintf(buff, "%3d%%", calculateBatteryPercentage());
1242 osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr);
1243 break;
1244
1245 case OSD_POWER_SUPPLY_IMPEDANCE:
1246 if (isPowerSupplyImpedanceValid())
1247 tfp_sprintf(buff, "%3d", getPowerSupplyImpedance());
1248 else
1249 strcpy(buff, "---");
1250 buff[3] = SYM_MILLIOHM;
1251 buff[4] = '\0';
1252 break;
1253
1254 #ifdef USE_GPS
1255 case OSD_GPS_SATS:
1256 buff[0] = SYM_SAT_L;
1257 buff[1] = SYM_SAT_R;
1258 tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
1259 if (!STATE(GPS_FIX)) {
1260 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1261 }
1262 break;
1263
1264 case OSD_GPS_SPEED:
1265 osdFormatVelocityStr(buff, gpsSol.groundSpeed, false);
1266 break;
1267
1268 case OSD_3D_SPEED:
1269 {
1270 osdFormatVelocityStr(buff, osdGet3DSpeed(), true);
1271 break;
1272 }
1273
1274 case OSD_GPS_LAT:
1275 osdFormatCoordinate(buff, SYM_LAT, gpsSol.llh.lat);
1276 break;
1277
1278 case OSD_GPS_LON:
1279 osdFormatCoordinate(buff, SYM_LON, gpsSol.llh.lon);
1280 break;
1281
1282 case OSD_HOME_DIR:
1283 {
1284 if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
1285 // There are 16 orientations for the home direction arrow.
1286 // so we use 22.5deg per image, where the 1st image is used
1287 // for [349, 11], the 2nd for [12, 33], etc...
1288 int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
1289 // Add 11 to the angle, so first character maps to [349, 11]
1290 int homeArrowDir = osdGetHeadingAngle(homeDirection + 11);
1291 unsigned arrowOffset = homeArrowDir * 2 / 45;
1292 buff[0] = SYM_ARROW_UP + arrowOffset;
1293 } else {
1294 // No home or no fix or unknown heading, blink.
1295 // If we're unarmed, show the arrow pointing up so users can see the arrow
1296 // while configuring the OSD. If we're armed, show a '-' indicating that
1297 // we don't know the direction to home.
1298 buff[0] = ARMING_FLAG(ARMED) ? '-' : SYM_ARROW_UP;
1299 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1300 }
1301 buff[1] = 0;
1302 break;
1303 }
1304
1305 case OSD_HOME_HEADING_ERROR:
1306 {
1307 buff[0] = SYM_HOME;
1308 buff[1] = SYM_HEADING;
1309
1310 if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) {
1311 int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading()))));
1312 tfp_sprintf(buff + 2, "%4d", h);
1313 } else {
1314 strcpy(buff + 2, "----");
1315 }
1316
1317 buff[6] = SYM_DEGREES;
1318 buff[7] = '\0';
1319 break;
1320 }
1321
1322 case OSD_HOME_DIST:
1323 {
1324 buff[0] = SYM_HOME;
1325 osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100);
1326 uint16_t dist_alarm = osdConfig()->dist_alarm;
1327 if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) {
1328 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1329 }
1330 }
1331 break;
1332
1333 case OSD_TRIP_DIST:
1334 buff[0] = SYM_TRIP_DIST;
1335 osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance());
1336 break;
1337
1338 case OSD_HEADING:
1339 {
1340 buff[0] = SYM_HEADING;
1341 if (osdIsHeadingValid()) {
1342 int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
1343 if (h < 0) {
1344 h += 360;
1345 }
1346 tfp_sprintf(&buff[1], "%3d", h);
1347 } else {
1348 buff[1] = buff[2] = buff[3] = '-';
1349 }
1350 buff[4] = SYM_DEGREES;
1351 buff[5] = '\0';
1352 break;
1353 }
1354
1355 case OSD_CRUISE_HEADING_ERROR:
1356 {
1357 if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
1358 displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
1359 return true;
1360 }
1361
1362 buff[0] = SYM_HEADING;
1363
1364 if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
1365 buff[1] = buff[2] = buff[3] = '-';
1366 } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
1367 int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
1368 if (ABS(herr) > 99)
1369 strcpy(buff + 1, ">99");
1370 else
1371 tfp_sprintf(buff + 1, "%3d", herr);
1372 }
1373
1374 buff[4] = SYM_DEGREES;
1375 buff[5] = '\0';
1376 break;
1377 }
1378
1379 case OSD_CRUISE_HEADING_ADJUSTMENT:
1380 {
1381 int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
1382
1383 if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
1384 displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
1385 return true;
1386 }
1387
1388 buff[0] = SYM_HEADING;
1389
1390 if (!ARMING_FLAG(ARMED)) {
1391 buff[1] = buff[2] = buff[3] = buff[4] = '-';
1392 } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
1393 tfp_sprintf(buff + 1, "%4d", heading_adjust);
1394 }
1395
1396 buff[5] = SYM_DEGREES;
1397 buff[6] = '\0';
1398 break;
1399 }
1400
1401 case OSD_GPS_HDOP:
1402 {
1403 buff[0] = SYM_HDP_L;
1404 buff[1] = SYM_HDP_R;
1405 int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE;
1406 osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, 2);
1407 break;
1408 }
1409
1410 case OSD_MAP_NORTH:
1411 {
1412 static uint16_t drawn = 0;
1413 static uint32_t scale = 0;
1414 osdDrawHomeMap(0, 'N', &drawn, &scale);
1415 return true;
1416 }
1417 case OSD_MAP_TAKEOFF:
1418 {
1419 static uint16_t drawn = 0;
1420 static uint32_t scale = 0;
1421 osdDrawHomeMap(CENTIDEGREES_TO_DEGREES(navigationGetHomeHeading()), 'T', &drawn, &scale);
1422 return true;
1423 }
1424 case OSD_RADAR:
1425 {
1426 static uint16_t drawn = 0;
1427 static uint32_t scale = 0;
1428 osdDrawRadar(&drawn, &scale);
1429 return true;
1430 }
1431 #endif // GPS
1432
1433 case OSD_ALTITUDE:
1434 {
1435 int32_t alt = osdGetAltitude();
1436 osdFormatAltitudeSymbol(buff, alt);
1437 uint16_t alt_alarm = osdConfig()->alt_alarm;
1438 uint16_t neg_alt_alarm = osdConfig()->neg_alt_alarm;
1439 if ((alt_alarm > 0 && CENTIMETERS_TO_METERS(alt) > alt_alarm) ||
1440 (neg_alt_alarm > 0 && alt < 0 && -CENTIMETERS_TO_METERS(alt) > neg_alt_alarm)) {
1441
1442 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1443 }
1444 break;
1445 }
1446
1447 case OSD_ALTITUDE_MSL:
1448 {
1449 int32_t alt = osdGetAltitudeMsl();
1450 osdFormatAltitudeSymbol(buff, alt);
1451 break;
1452 }
1453
1454 case OSD_ONTIME:
1455 {
1456 osdFormatOnTime(buff);
1457 break;
1458 }
1459
1460 case OSD_FLYTIME:
1461 {
1462 osdFormatFlyTime(buff, &elemAttr);
1463 break;
1464 }
1465
1466 case OSD_ONTIME_FLYTIME:
1467 {
1468 if (ARMING_FLAG(ARMED)) {
1469 osdFormatFlyTime(buff, &elemAttr);
1470 } else {
1471 osdFormatOnTime(buff);
1472 }
1473 break;
1474 }
1475
1476 case OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH:
1477 {
1478 static timeUs_t updatedTimestamp = 0;
1479 /*static int32_t updatedTimeSeconds = 0;*/
1480 timeUs_t currentTimeUs = micros();
1481 static int32_t timeSeconds = -1;
1482 if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
1483 timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
1484 updatedTimestamp = currentTimeUs;
1485 }
1486 if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
1487 buff[0] = SYM_FLY_M;
1488 strcpy(buff + 1, "--:--");
1489 updatedTimestamp = 0;
1490 } else if (timeSeconds == -2) {
1491 // Wind is too strong to come back with cruise throttle
1492 buff[0] = SYM_FLY_M;
1493 buff[1] = buff[2] = buff[4] = buff[5] = SYM_WIND_HORIZONTAL;
1494 buff[3] = ':';
1495 buff[6] = '\0';
1496 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1497 } else {
1498 osdFormatTime(buff, timeSeconds, SYM_FLY_M, SYM_FLY_H);
1499 if (timeSeconds == 0)
1500 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1501 }
1502 }
1503 break;
1504
1505 case OSD_REMAINING_DISTANCE_BEFORE_RTH:;
1506 static timeUs_t updatedTimestamp = 0;
1507 timeUs_t currentTimeUs = micros();
1508 static int32_t distanceMeters = -1;
1509 if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
1510 distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
1511 updatedTimestamp = currentTimeUs;
1512 }
1513 buff[0] = SYM_TRIP_DIST;
1514 if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) {
1515 buff[1] = SYM_DIST_M;
1516 strcpy(buff + 2, "---");
1517 } else if (distanceMeters == -2) {
1518 // Wind is too strong to come back with cruise throttle
1519 buff[1] = SYM_DIST_M;
1520 buff[2] = buff[3] = buff[4] = SYM_WIND_HORIZONTAL;
1521 buff[5] = '\0';
1522 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1523 } else {
1524 osdFormatDistanceSymbol(buff + 1, distanceMeters * 100);
1525 if (distanceMeters == 0)
1526 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1527 }
1528 break;
1529
1530 case OSD_FLYMODE:
1531 {
1532 char *p = "ACRO";
1533
1534 if (FLIGHT_MODE(FAILSAFE_MODE))
1535 p = "!FS!";
1536 else if (FLIGHT_MODE(MANUAL_MODE))
1537 p = "MANU";
1538 else if (FLIGHT_MODE(NAV_RTH_MODE))
1539 p = "RTH ";
1540 else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
1541 p = "HOLD";
1542 else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
1543 p = "3CRS";
1544 else if (FLIGHT_MODE(NAV_CRUISE_MODE))
1545 p = "CRS ";
1546 else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
1547 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
1548 // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
1549 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
1550 p = " AH ";
1551 } else if (FLIGHT_MODE(NAV_WP_MODE))
1552 p = " WP ";
1553 else if (FLIGHT_MODE(ANGLE_MODE))
1554 p = "ANGL";
1555 else if (FLIGHT_MODE(HORIZON_MODE))
1556 p = "HOR ";
1557 else if (isAirmodeActive())
1558 p = "AIR ";
1559
1560 displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
1561 return true;
1562 }
1563
1564 case OSD_CRAFT_NAME:
1565 if (strlen(systemConfig()->name) == 0)
1566 strcpy(buff, "CRAFT_NAME");
1567 else {
1568 for (int i = 0; i < MAX_NAME_LENGTH; i++) {
1569 buff[i] = sl_toupper((unsigned char)systemConfig()->name[i]);
1570 if (systemConfig()->name[i] == 0)
1571 break;
1572 }
1573 }
1574 break;
1575
1576 case OSD_THROTTLE_POS:
1577 {
1578 osdFormatThrottlePosition(buff, false, NULL);
1579 break;
1580 }
1581
1582 #if defined(VTX) || defined(USE_VTX_COMMON)
1583 case OSD_VTX_CHANNEL:
1584 #if defined(VTX)
1585 // FIXME: This doesn't actually work. It's for boards with
1586 // builtin VTX.
1587 tfp_sprintf(buff, "CH:%2d", current_vtx_channel % CHANNELS_PER_BAND + 1);
1588 #else
1589 {
1590 uint8_t band = 0;
1591 uint8_t channel = 0;
1592 uint8_t powerIndex = 0;
1593 char bandChr = '-';
1594 const char *channelStr = "-";
1595 char powerChr = '-';
1596 vtxDevice_t *vtxDevice = vtxCommonDevice();
1597 if (vtxDevice) {
1598 if (vtxCommonGetBandAndChannel(vtxDevice, &band, &channel)) {
1599 bandChr = vtx58BandLetter[band];
1600 channelStr = vtx58ChannelNames[channel];
1601 }
1602 if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex)) {
1603 powerChr = '0' + powerIndex;
1604 }
1605 }
1606 tfp_sprintf(buff, "CH:%c%s:%c", bandChr, channelStr, powerChr);
1607 }
1608 #endif
1609 break;
1610 #endif // VTX || VTX_COMMON
1611
1612 case OSD_CROSSHAIRS:
1613 osdCrosshairsBounds(&elemPosX, &elemPosY, NULL);
1614 switch ((osd_crosshairs_style_e)osdConfig()->crosshairs_style) {
1615 case OSD_CROSSHAIRS_STYLE_DEFAULT:
1616 buff[0] = SYM_AH_CENTER_LINE;
1617 buff[1] = SYM_AH_CENTER;
1618 buff[2] = SYM_AH_CENTER_LINE_RIGHT;
1619 buff[3] = '\0';
1620 break;
1621 case OSD_CROSSHAIRS_STYLE_AIRCRAFT:
1622 buff[0] = SYM_AH_CROSSHAIRS_AIRCRAFT0;
1623 buff[1] = SYM_AH_CROSSHAIRS_AIRCRAFT1;
1624 buff[2] = SYM_AH_CROSSHAIRS_AIRCRAFT2;
1625 buff[3] = SYM_AH_CROSSHAIRS_AIRCRAFT3;
1626 buff[4] = SYM_AH_CROSSHAIRS_AIRCRAFT4;
1627 buff[5] = '\0';
1628 break;
1629 }
1630 break;
1631
1632 case OSD_ATTITUDE_ROLL:
1633 buff[0] = SYM_ROLL_LEVEL;
1634 if (ABS(attitude.values.roll) >= 1)
1635 buff[0] += (attitude.values.roll < 0 ? -1 : 1);
1636 osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3);
1637 break;
1638
1639 case OSD_ATTITUDE_PITCH:
1640 if (ABS(attitude.values.pitch) < 1)
1641 buff[0] = 'P';
1642 else if (attitude.values.pitch > 0)
1643 buff[0] = SYM_PITCH_DOWN;
1644 else if (attitude.values.pitch < 0)
1645 buff[0] = SYM_PITCH_UP;
1646 osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3);
1647 break;
1648
1649 case OSD_ARTIFICIAL_HORIZON:
1650 {
1651
1652 elemPosX = 14;
1653 elemPosY = 6; // Center of the AH area
1654
1655 // Store the positions we draw over to erase only these at the next iteration
1656 static int8_t previous_written[AH_PREV_SIZE];
1657 static int8_t previous_orient = -1;
1658
1659 float pitch_rad_to_char = (float)(AH_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch);
1660
1661 float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
1662 float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
1663
1664 if (osdConfig()->ahi_reverse_roll) {
1665 rollAngle = -rollAngle;
1666 }
1667
1668 if (IS_DISPLAY_PAL) {
1669 ++elemPosY;
1670 }
1671
1672 float ky = sin_approx(rollAngle);
1673 float kx = cos_approx(rollAngle);
1674
1675 if (previous_orient != -1) {
1676 for (int i = 0; i < AH_PREV_SIZE; ++i) {
1677 if (previous_written[i] > -1) {
1678 int8_t dx = (previous_orient ? previous_written[i] : i) - AH_PREV_SIZE / 2;
1679 int8_t dy = (previous_orient ? i : previous_written[i]) - AH_PREV_SIZE / 2;
1680 displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, SYM_BLANK);
1681 previous_written[i] = -1;
1682 }
1683 }
1684 }
1685
1686 if (ABS(ky) < ABS(kx)) {
1687
1688 previous_orient = 0;
1689
1690 for (int8_t dx = -AH_WIDTH / 2; dx <= AH_WIDTH / 2; dx++) {
1691 float fy = dx * (ky / kx) + pitchAngle * pitch_rad_to_char + 0.49f;
1692 int8_t dy = floorf(fy);
1693 const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
1694 uint8_t c;
1695
1696 if ((dy >= -AH_HEIGHT / 2) && (dy <= AH_HEIGHT / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
1697 c = SYM_AH_H_START + ((AH_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * AH_H_SYM_COUNT));
1698 displayWriteChar(osdDisplayPort, elemPosX + dx, elemPosY - dy, c);
1699 previous_written[dx + AH_PREV_SIZE / 2] = dy + AH_PREV_SIZE / 2;
1700 }
1701 }
1702
1703 } else {
1704
1705 previous_orient = 1;
1706
1707 for (int8_t dy = -AH_HEIGHT / 2; dy <= AH_HEIGHT / 2; dy++) {
1708 const float fx = (dy - pitchAngle * pitch_rad_to_char) * (kx / ky) + 0.5f;
1709 const int8_t dx = floorf(fx);
1710 const uint8_t chX = elemPosX + dx, chY = elemPosY - dy;
1711 uint8_t c;
1712
1713 if ((dx >= -AH_WIDTH / 2) && (dx <= AH_WIDTH / 2) && displayReadCharWithAttr(osdDisplayPort, chX, chY, &c, NULL) && (c == SYM_BLANK)) {
1714 c = SYM_AH_V_START + (fx - dx) * AH_V_SYM_COUNT;
1715 displayWriteChar(osdDisplayPort, chX, chY, c);
1716 previous_written[dy + AH_PREV_SIZE / 2] = dx + AH_PREV_SIZE / 2;
1717 }
1718 }
1719
1720 }
1721
1722 osdDrawSingleElement(OSD_HORIZON_SIDEBARS);
1723 osdDrawSingleElement(OSD_CROSSHAIRS);
1724
1725 return true;
1726 }
1727
1728 case OSD_HORIZON_SIDEBARS:
1729 {
1730 elemPosX = 14;
1731 elemPosY = 6;
1732
1733 if (IS_DISPLAY_PAL) {
1734 ++elemPosY;
1735 }
1736
1737 static osd_sidebar_t left;
1738 static osd_sidebar_t right;
1739
1740 timeMs_t currentTimeMs = millis();
1741 uint8_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs);
1742 uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
1743
1744 const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
1745 const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
1746
1747 // Arrows
1748 if (osdConfig()->sidebar_scroll_arrows) {
1749 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY - hudheight - 1,
1750 left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
1751
1752 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY - hudheight - 1,
1753 right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
1754
1755 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + hudheight + 1,
1756 left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
1757
1758 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + hudheight + 1,
1759 right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
1760 }
1761
1762 // Draw AH sides
1763 for (int y = -hudheight; y <= hudheight; y++) {
1764 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, leftDecoration);
1765 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, rightDecoration);
1766 }
1767
1768 // AH level indicators
1769 displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
1770 displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
1771
1772 return true;
1773 }
1774
1775 #if defined(USE_BARO) || defined(USE_GPS)
1776 case OSD_VARIO:
1777 {
1778 int16_t v = getEstimatedActualVelocity(Z) / 50; //50cm = 1 arrow
1779 uint8_t vchars[] = {SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK, SYM_BLANK};
1780
1781 if (v >= 6)
1782 vchars[0] = SYM_VARIO_UP_2A;
1783 else if (v == 5)
1784 vchars[0] = SYM_VARIO_UP_1A;
1785 if (v >=4)
1786 vchars[1] = SYM_VARIO_UP_2A;
1787 else if (v == 3)
1788 vchars[1] = SYM_VARIO_UP_1A;
1789 if (v >=2)
1790 vchars[2] = SYM_VARIO_UP_2A;
1791 else if (v == 1)
1792 vchars[2] = SYM_VARIO_UP_1A;
1793 if (v <= -2)
1794 vchars[2] = SYM_VARIO_DOWN_2A;
1795 else if (v == -1)
1796 vchars[2] = SYM_VARIO_DOWN_1A;
1797 if (v <= -4)
1798 vchars[3] = SYM_VARIO_DOWN_2A;
1799 else if (v == -3)
1800 vchars[3] = SYM_VARIO_DOWN_1A;
1801 if (v <= -6)
1802 vchars[4] = SYM_VARIO_DOWN_2A;
1803 else if (v == -5)
1804 vchars[4] = SYM_VARIO_DOWN_1A;
1805
1806 displayWriteChar(osdDisplayPort, elemPosX, elemPosY, vchars[0]);
1807 displayWriteChar(osdDisplayPort, elemPosX, elemPosY+1, vchars[1]);
1808 displayWriteChar(osdDisplayPort, elemPosX, elemPosY+2, vchars[2]);
1809 displayWriteChar(osdDisplayPort, elemPosX, elemPosY+3, vchars[3]);
1810 displayWriteChar(osdDisplayPort, elemPosX, elemPosY+4, vchars[4]);
1811 return true;
1812 }
1813
1814 case OSD_VARIO_NUM:
1815 {
1816 int16_t value = getEstimatedActualVelocity(Z);
1817 char sym;
1818 switch ((osd_unit_e)osdConfig()->units) {
1819 case OSD_UNIT_IMPERIAL:
1820 // Convert to centifeet/s
1821 value = CENTIMETERS_TO_CENTIFEET(value);
1822 sym = SYM_FTS;
1823 break;
1824 case OSD_UNIT_UK:
1825 FALLTHROUGH;
1826 case OSD_UNIT_METRIC:
1827 // Already in cm/s
1828 sym = SYM_MS;
1829 break;
1830 }
1831
1832 osdFormatCentiNumber(buff, value, 0, 1, 0, 3);
1833 buff[3] = sym;
1834 buff[4] = '\0';
1835 break;
1836 }
1837 #endif
1838
1839 case OSD_ROLL_PIDS:
1840 osdDisplayPIDValues(elemPosX, elemPosY, "ROL", &pidBank()->pid[PID_ROLL], ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
1841 return true;
1842
1843 case OSD_PITCH_PIDS:
1844 osdDisplayPIDValues(elemPosX, elemPosY, "PIT", &pidBank()->pid[PID_PITCH], ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
1845 return true;
1846
1847 case OSD_YAW_PIDS:
1848 osdDisplayPIDValues(elemPosX, elemPosY, "YAW", &pidBank()->pid[PID_YAW], ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
1849 return true;
1850
1851 case OSD_LEVEL_PIDS:
1852 osdDisplayPIDValues(elemPosX, elemPosY, "LEV", &pidBank()->pid[PID_LEVEL], ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
1853 return true;
1854
1855 case OSD_POS_XY_PIDS:
1856 osdDisplayPIDValues(elemPosX, elemPosY, "PXY", &pidBank()->pid[PID_POS_XY], ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
1857 return true;
1858
1859 case OSD_POS_Z_PIDS:
1860 osdDisplayPIDValues(elemPosX, elemPosY, "PZ", &pidBank()->pid[PID_POS_Z], ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
1861 return true;
1862
1863 case OSD_VEL_XY_PIDS:
1864 if (STATE(FIXED_WING))
1865 osdDisplayPIDValues(elemPosX, elemPosY, "VXY", &pidBank()->pid[PID_VEL_XY], ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
1866 return true;
1867
1868 case OSD_VEL_Z_PIDS:
1869 if (STATE(FIXED_WING))
1870 osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
1871 return true;
1872
1873 case OSD_HEADING_P:
1874 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "HP", 0, pidBank()->pid[PID_HEADING].P, 3, 0, ADJUSTMENT_HEADING_P);
1875 return true;
1876
1877 case OSD_BOARD_ALIGN_ROLL:
1878 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AR", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->rollDeciDegrees), 4, 1, ADJUSTMENT_ROLL_BOARD_ALIGNMENT);
1879 return true;
1880
1881 case OSD_BOARD_ALIGN_PITCH:
1882 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "AP", 0, DECIDEGREES_TO_DEGREES((float)boardAlignment()->pitchDeciDegrees), 4, 1, ADJUSTMENT_PITCH_BOARD_ALIGNMENT);
1883 return true;
1884
1885 case OSD_RC_EXPO:
1886 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO);
1887 return true;
1888
1889 case OSD_RC_YAW_EXPO:
1890 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO);
1891 return true;
1892
1893 case OSD_THROTTLE_EXPO:
1894 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO);
1895 return true;
1896
1897 case OSD_PITCH_RATE:
1898 displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR");
1899
1900 elemAttr = TEXT_ATTRIBUTES_NONE;
1901 tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
1902 if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
1903 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1904 displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1905 return true;
1906
1907 case OSD_ROLL_RATE:
1908 displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR");
1909
1910 elemAttr = TEXT_ATTRIBUTES_NONE;
1911 tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
1912 if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
1913 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1914 displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1915 return true;
1916
1917 case OSD_YAW_RATE:
1918 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
1919 return true;
1920
1921 case OSD_MANUAL_RC_EXPO:
1922 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO);
1923 return true;
1924
1925 case OSD_MANUAL_RC_YAW_EXPO:
1926 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO);
1927 return true;
1928
1929 case OSD_MANUAL_PITCH_RATE:
1930 displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR");
1931
1932 elemAttr = TEXT_ATTRIBUTES_NONE;
1933 tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]);
1934 if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
1935 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1936 displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1937 return true;
1938
1939 case OSD_MANUAL_ROLL_RATE:
1940 displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR");
1941
1942 elemAttr = TEXT_ATTRIBUTES_NONE;
1943 tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]);
1944 if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
1945 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
1946 displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
1947 return true;
1948
1949 case OSD_MANUAL_YAW_RATE:
1950 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE);
1951 return true;
1952
1953 case OSD_NAV_FW_CRUISE_THR:
1954 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
1955 return true;
1956
1957 case OSD_NAV_FW_PITCH2THR:
1958 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "P2T", 0, navConfig()->fw.pitch_to_throttle, 3, 0, ADJUSTMENT_NAV_FW_PITCH2THR);
1959 return true;
1960
1961 case OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
1962 osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "0TP", 0, (float)mixerConfig()->fwMinThrottleDownPitchAngle / 10, 3, 1, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE);
1963 return true;
1964
1965 case OSD_FW_ALT_PID_OUTPUTS:
1966 {
1967 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1968 osdFormatPidControllerOutput(buff, "PZO", &nav_pids->fw_alt, 10, true); // display requested pitch degrees
1969 break;
1970 }
1971
1972 case OSD_FW_POS_PID_OUTPUTS:
1973 {
1974 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers(); // display requested roll degrees
1975 osdFormatPidControllerOutput(buff, "PXYO", &nav_pids->fw_nav, 1, true);
1976 break;
1977 }
1978
1979 case OSD_MC_VEL_Z_PID_OUTPUTS:
1980 {
1981 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1982 osdFormatPidControllerOutput(buff, "VZO", &nav_pids->vel[Z], 100, false); // display throttle adjustment µs
1983 break;
1984 }
1985
1986 case OSD_MC_VEL_X_PID_OUTPUTS:
1987 {
1988 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1989 osdFormatPidControllerOutput(buff, "VXO", &nav_pids->vel[X], 100, false); // display requested acceleration cm/s^2
1990 break;
1991 }
1992
1993 case OSD_MC_VEL_Y_PID_OUTPUTS:
1994 {
1995 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1996 osdFormatPidControllerOutput(buff, "VYO", &nav_pids->vel[Y], 100, false); // display requested acceleration cm/s^2
1997 break;
1998 }
1999
2000 case OSD_MC_POS_XYZ_P_OUTPUTS:
2001 {
2002 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
2003 strcpy(buff, "POSO ");
2004 // display requested velocity cm/s
2005 tfp_sprintf(buff + 5, "%4d", lrintf(nav_pids->pos[X].output_constrained * 100));
2006 buff[9] = ' ';
2007 tfp_sprintf(buff + 10, "%4d", lrintf(nav_pids->pos[Y].output_constrained * 100));
2008 buff[14] = ' ';
2009 tfp_sprintf(buff + 15, "%4d", lrintf(nav_pids->pos[Z].output_constrained * 100));
2010 buff[19] = '\0';
2011 break;
2012 }
2013
2014 case OSD_POWER:
2015 {
2016 buff[0] = SYM_WATT;
2017 osdFormatCentiNumber(buff + 1, getPower(), 0, 2, 0, 3);
2018 break;
2019 }
2020
2021 case OSD_AIR_SPEED:
2022 {
2023 #ifdef USE_PITOT
2024 buff[0] = SYM_AIR;
2025 osdFormatVelocityStr(buff + 1, pitot.airSpeed, false);
2026 #else
2027 return false;
2028 #endif
2029 break;
2030 }
2031
2032 case OSD_RTC_TIME:
2033 {
2034 // RTC not configured will show 00:00
2035 dateTime_t dateTime;
2036 rtcGetDateTimeLocal(&dateTime);
2037 buff[0] = SYM_CLOCK;
2038 tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes);
2039 break;
2040 }
2041
2042 case OSD_MESSAGES:
2043 {
2044 const char *message = NULL;
2045 char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
2046 if (ARMING_FLAG(ARMED)) {
2047 // Aircraft is armed. We might have up to 4
2048 // messages to show.
2049 const char *messages[4];
2050 unsigned messageCount = 0;
2051 if (FLIGHT_MODE(FAILSAFE_MODE)) {
2052 // In FS mode while being armed too
2053 const char *failsafePhaseMessage = osdFailsafePhaseMessage();
2054 const char *failsafeInfoMessage = osdFailsafeInfoMessage();
2055 const char *navStateFSMessage = navigationStateMessage();
2056 if (failsafePhaseMessage) {
2057 messages[messageCount++] = failsafePhaseMessage;
2058 }
2059 if (failsafeInfoMessage) {
2060 messages[messageCount++] = failsafeInfoMessage;
2061 }
2062 if (navStateFSMessage) {
2063 messages[messageCount++] = navStateFSMessage;
2064 }
2065 if (messageCount > 0) {
2066 message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
2067 if (message == failsafeInfoMessage) {
2068 // failsafeInfoMessage is not useful for recovering
2069 // a lost model, but might help avoiding a crash.
2070 // Blink to grab user attention.
2071 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
2072 }
2073 // We're shoing either failsafePhaseMessage or
2074 // navStateFSMessage. Don't BLINK here since
2075 // having this text available might be crucial
2076 // during a lost aircraft recovery and blinking
2077 // will cause it to be missing from some frames.
2078 }
2079 } else {
2080 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
2081 const char *navStateMessage = navigationStateMessage();
2082 if (navStateMessage) {
2083 messages[messageCount++] = navStateMessage;
2084 }
2085 } else {
2086 if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
2087 // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
2088 // when it doesn't require ANGLE mode (required only in FW
2089 // right now). If if requires ANGLE, its display is handled
2090 // by OSD_FLYMODE.
2091 messages[messageCount++] = "(ALTITUDE HOLD)";
2092 }
2093 if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
2094 messages[messageCount++] = "(AUTOTRIM)";
2095 }
2096 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
2097 messages[messageCount++] = "(AUTOTUNE)";
2098 }
2099 if (FLIGHT_MODE(HEADFREE_MODE)) {
2100 messages[messageCount++] = "(HEADFREE)";
2101 }
2102 }
2103 // Pick one of the available messages. Each message lasts
2104 // a second.
2105 if (messageCount > 0) {
2106 message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];
2107 }
2108 }
2109 } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
2110 unsigned invalidIndex;
2111 // Check if we're unable to arm for some reason
2112 if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
2113 if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
2114 const setting_t *setting = settingGet(invalidIndex);
2115 settingGetName(setting, messageBuf);
2116 for (int ii = 0; messageBuf[ii]; ii++) {
2117 messageBuf[ii] = sl_toupper(messageBuf[ii]);
2118 }
2119 message = messageBuf;
2120 } else {
2121 message = "INVALID SETTING";
2122 TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
2123 }
2124 } else {
2125 if (OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
2126 message = "UNABLE TO ARM";
2127 TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
2128 } else {
2129 // Show the reason for not arming
2130 message = osdArmingDisabledReasonMessage();
2131 }
2132 }
2133 }
2134 osdFormatMessage(buff, sizeof(buff), message);
2135 break;
2136 }
2137
2138 case OSD_MAIN_BATT_CELL_VOLTAGE:
2139 {
2140 osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
2141 return true;
2142 }
2143
2144 case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
2145 {
2146 osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatterySagCompensatedAverageCellVoltage(), 3, 2);
2147 return true;
2148 }
2149
2150 case OSD_THROTTLE_POS_AUTO_THR:
2151 {
2152 osdFormatThrottlePosition(buff, true, &elemAttr);
2153 break;
2154 }
2155
2156 case OSD_HEADING_GRAPH:
2157 {
2158 static const uint8_t graph[] = {
2159 SYM_HEADING_LINE,
2160 SYM_HEADING_E,
2161 SYM_HEADING_LINE,
2162 SYM_HEADING_DIVIDED_LINE,
2163 SYM_HEADING_LINE,
2164 SYM_HEADING_S,
2165 SYM_HEADING_LINE,
2166 SYM_HEADING_DIVIDED_LINE,
2167 SYM_HEADING_LINE,
2168 SYM_HEADING_W,
2169 SYM_HEADING_LINE,
2170 SYM_HEADING_DIVIDED_LINE,
2171 SYM_HEADING_LINE,
2172 SYM_HEADING_N,
2173 SYM_HEADING_LINE,
2174 SYM_HEADING_DIVIDED_LINE,
2175 SYM_HEADING_LINE,
2176 SYM_HEADING_E,
2177 SYM_HEADING_LINE,
2178 SYM_HEADING_DIVIDED_LINE,
2179 SYM_HEADING_LINE,
2180 SYM_HEADING_S,
2181 SYM_HEADING_LINE,
2182 SYM_HEADING_DIVIDED_LINE,
2183 SYM_HEADING_LINE,
2184 SYM_HEADING_W,
2185 SYM_HEADING_LINE,
2186 };
2187 if (osdIsHeadingValid()) {
2188 int16_t h = DECIDEGREES_TO_DEGREES(osdGetHeading());
2189 if (h >= 180) {
2190 h -= 360;
2191 }
2192 int hh = h * 4;
2193 hh = hh + 720 + 45;
2194 hh = hh / 90;
2195 memcpy_fn(buff, graph + hh + 1, 9);
2196 } else {
2197 buff[0] = buff[2] = buff[4] = buff[6] = buff[8] = SYM_HEADING_LINE;
2198 buff[1] = buff[3] = buff[5] = buff[7] = SYM_HEADING_DIVIDED_LINE;
2199 }
2200 buff[9] = '\0';
2201 break;
2202 }
2203
2204 case OSD_EFFICIENCY_MAH_PER_KM:
2205 {
2206 // amperage is in centi amps, speed is in cms/s. We want
2207 // mah/km. Values over 999 are considered useless and
2208 // displayed as "---""
2209 static pt1Filter_t eFilterState;
2210 static timeUs_t efficiencyUpdated = 0;
2211 int32_t value = 0;
2212 timeUs_t currentTimeUs = micros();
2213 timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
2214 if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
2215 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
2216 value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
2217 1, efficiencyTimeDelta * 1e-6f);
2218
2219 efficiencyUpdated = currentTimeUs;
2220 } else {
2221 value = eFilterState.state;
2222 }
2223 }
2224 if (value > 0 && value <= 999) {
2225 tfp_sprintf(buff, "%3d", value);
2226 } else {
2227 buff[0] = buff[1] = buff[2] = '-';
2228 }
2229 buff[3] = SYM_MAH_KM_0;
2230 buff[4] = SYM_MAH_KM_1;
2231 buff[5] = '\0';
2232 break;
2233 }
2234
2235 case OSD_EFFICIENCY_WH_PER_KM:
2236 {
2237 // amperage is in centi amps, speed is in cms/s. We want
2238 // mWh/km. Values over 999Wh/km are considered useless and
2239 // displayed as "---""
2240 static pt1Filter_t eFilterState;
2241 static timeUs_t efficiencyUpdated = 0;
2242 int32_t value = 0;
2243 timeUs_t currentTimeUs = micros();
2244 timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
2245 if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
2246 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
2247 value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
2248 1, efficiencyTimeDelta * 1e-6f);
2249
2250 efficiencyUpdated = currentTimeUs;
2251 } else {
2252 value = eFilterState.state;
2253 }
2254 }
2255 if (value > 0 && value <= 999999) {
2256 osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3);
2257 } else {
2258 buff[0] = buff[1] = buff[2] = '-';
2259 }
2260 buff[3] = SYM_WH_KM_0;
2261 buff[4] = SYM_WH_KM_1;
2262 buff[5] = '\0';
2263 break;
2264 }
2265
2266 case OSD_DEBUG:
2267 {
2268 // Longest representable string is -32768, hence 6 characters
2269 tfp_sprintf(buff, "[0]=%6d [1]=%6d", debug[0], debug[1]);
2270 displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
2271 elemPosY++;
2272 tfp_sprintf(buff, "[2]=%6d [3]=%6d", debug[2], debug[3]);
2273 break;
2274 }
2275
2276 case OSD_TEMPERATURE:
2277 {
2278 int16_t temperature = getCurrentTemperature();
2279 osdFormatTemperatureSymbol(buff, temperature);
2280 break;
2281 }
2282
2283 case OSD_WIND_SPEED_HORIZONTAL:
2284 #ifdef USE_WIND_ESTIMATOR
2285 {
2286 bool valid = isEstimatedWindSpeedValid();
2287 float horizontalWindSpeed;
2288 if (valid) {
2289 uint16_t angle;
2290 horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
2291 int16_t windDirection = osdGetHeadingAngle((int)angle - DECIDEGREES_TO_DEGREES(attitude.values.yaw));
2292 buff[1] = SYM_DIRECTION + (windDirection * 2 / 90);
2293 } else {
2294 horizontalWindSpeed = 0;
2295 buff[1] = SYM_BLANK;
2296 }
2297 buff[0] = SYM_WIND_HORIZONTAL;
2298 osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
2299 break;
2300 }
2301 #else
2302 return false;
2303 #endif
2304
2305 case OSD_WIND_SPEED_VERTICAL:
2306 #ifdef USE_WIND_ESTIMATOR
2307 {
2308 buff[0] = SYM_WIND_VERTICAL;
2309 buff[1] = SYM_BLANK;
2310 bool valid = isEstimatedWindSpeedValid();
2311 float verticalWindSpeed;
2312 if (valid) {
2313 verticalWindSpeed = getEstimatedWindSpeed(Z);
2314 if (verticalWindSpeed < 0) {
2315 buff[1] = SYM_AH_DECORATION_DOWN;
2316 verticalWindSpeed = -verticalWindSpeed;
2317 } else if (verticalWindSpeed > 0) {
2318 buff[1] = SYM_AH_DECORATION_UP;
2319 }
2320 } else {
2321 verticalWindSpeed = 0;
2322 }
2323 osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
2324 break;
2325 }
2326 #else
2327 return false;
2328 #endif
2329
2330 default:
2331 return false;
2332 }
2333
2334 displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
2335 return true;
2336 }
2337
2338 static uint8_t osdIncElementIndex(uint8_t elementIndex)
2339 {
2340 ++elementIndex;
2341
2342 if (elementIndex == OSD_ARTIFICIAL_HORIZON)
2343 ++elementIndex;
2344
2345 if (!sensors(SENSOR_ACC)) {
2346 if (elementIndex == OSD_CROSSHAIRS) {
2347 elementIndex = OSD_ONTIME;
2348 }
2349 }
2350
2351 if (!feature(FEATURE_VBAT)) {
2352 if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
2353 elementIndex = OSD_LEVEL_PIDS;
2354 }
2355 }
2356
2357 if (!feature(FEATURE_CURRENT_METER)) {
2358 if (elementIndex == OSD_CURRENT_DRAW) {
2359 elementIndex = OSD_GPS_SPEED;
2360 }
2361 if (elementIndex == OSD_EFFICIENCY_MAH_PER_KM) {
2362 elementIndex = OSD_TRIP_DIST;
2363 }
2364 if (elementIndex == OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH) {
2365 elementIndex = OSD_HOME_HEADING_ERROR;
2366 }
2367 if (elementIndex == OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE) {
2368 elementIndex = OSD_LEVEL_PIDS;
2369 }
2370 }
2371
2372 if (!feature(FEATURE_GPS)) {
2373 if (elementIndex == OSD_GPS_SPEED) {
2374 elementIndex = OSD_ALTITUDE;
2375 }
2376 if (elementIndex == OSD_GPS_LON) {
2377 elementIndex = OSD_VARIO;
2378 }
2379 if (elementIndex == OSD_GPS_HDOP) {
2380 elementIndex = OSD_MAIN_BATT_CELL_VOLTAGE;
2381 }
2382 if (elementIndex == OSD_TRIP_DIST) {
2383 elementIndex = OSD_ATTITUDE_PITCH;
2384 }
2385 if (elementIndex == OSD_MAP_NORTH) {
2386 elementIndex = OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE;
2387 }
2388 if (elementIndex == OSD_3D_SPEED) {
2389 elementIndex++;
2390 }
2391
2392 }
2393
2394 if (elementIndex == OSD_ITEM_COUNT) {
2395 elementIndex = 0;
2396 }
2397 return elementIndex;
2398 }
2399
2400 void osdDrawNextElement(void)
2401 {
2402 static uint8_t elementIndex = 0;
2403 // Prevent infinite loop when no elements are enabled
2404 uint8_t index = elementIndex;
2405 do {
2406 elementIndex = osdIncElementIndex(elementIndex);
2407 } while(!osdDrawSingleElement(elementIndex) && index != elementIndex);
2408
2409 // Draw artificial horizon last
2410 osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
2411 }
2412
2413 void pgResetFn_osdConfig(osdConfig_t *osdConfig)
2414 {
2415 osdConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
2416 osdConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
2417 osdConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
2418
2419 osdConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
2420 //line 2
2421 osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
2422 osdConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2);
2423 osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
2424 osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
2425 osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
2426 osdConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
2427
2428 osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
2429 osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
2430 osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
2431 osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
2432 osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
2433 osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
2434 osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG;
2435 osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
2436 osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
2437 osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
2438 osdConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7);
2439 osdConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8);
2440
2441 osdConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5);
2442 osdConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5);
2443
2444 osdConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7);
2445 osdConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8);
2446
2447 // avoid OSD_VARIO under OSD_CROSSHAIRS
2448 osdConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5);
2449 // OSD_VARIO_NUM at the right of OSD_VARIO
2450 osdConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7);
2451 osdConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11);
2452 osdConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
2453 osdConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
2454
2455 osdConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
2456 osdConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
2457
2458 osdConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
2459 osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
2460 osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
2461 osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
2462 osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
2463 osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
2464
2465 osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
2466 osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
2467
2468 osdConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
2469 osdConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(12, 12) | OSD_VISIBLE_FLAG;
2470 osdConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12);
2471
2472 osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
2473 osdConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
2474 osdConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
2475 osdConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12);
2476 osdConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12);
2477 osdConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12);
2478 osdConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12);
2479 osdConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12);
2480 osdConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12);
2481 osdConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10);
2482 osdConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11);
2483 osdConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12);
2484 osdConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12);
2485 osdConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12);
2486 osdConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12);
2487 osdConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12);
2488 osdConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12);
2489 osdConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12);
2490 osdConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12);
2491 osdConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12);
2492 osdConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12);
2493 osdConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12);
2494 osdConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12);
2495 osdConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12);
2496 osdConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12);
2497 osdConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
2498 osdConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
2499 osdConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
2500 osdConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
2501 osdConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
2502 osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
2503
2504 osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
2505 osdConfig->item_pos[0][OSD_TEMPERATURE] = OSD_POS(23, 2);
2506
2507 osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
2508 osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
2509 osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7);
2510
2511 // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
2512 osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
2513
2514 for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) {
2515 for (unsigned jj = 0; jj < ARRAYLEN(osdConfig->item_pos[0]); jj++) {
2516 osdConfig->item_pos[ii][jj] = osdConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG;
2517 }
2518 }
2519
2520 osdConfig->rssi_alarm = 20;
2521 osdConfig->time_alarm = 10;
2522 osdConfig->alt_alarm = 100;
2523 osdConfig->dist_alarm = 1000;
2524 osdConfig->neg_alt_alarm = 5;
2525
2526 osdConfig->video_system = VIDEO_SYSTEM_AUTO;
2527
2528 osdConfig->ahi_reverse_roll = 0;
2529 osdConfig->ahi_max_pitch = AH_MAX_PITCH_DEFAULT;
2530 osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT;
2531 osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
2532 osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
2533 osdConfig->sidebar_scroll_arrows = 0;
2534
2535 osdConfig->units = OSD_UNIT_METRIC;
2536 osdConfig->main_voltage_decimals = 1;
2537
2538 osdConfig->estimations_wind_compensation = true;
2539 osdConfig->coordinate_digits = 9;
2540
2541 osdConfig->osd_failsafe_switch_layout = false;
2542 }
2543
2544 static void osdSetNextRefreshIn(uint32_t timeMs) {
2545 resumeRefreshAt = micros() + timeMs * 1000;
2546 refreshWaitForResumeCmdRelease = true;
2547 }
2548
2549 void osdInit(displayPort_t *osdDisplayPortToUse)
2550 {
2551 if (!osdDisplayPortToUse)
2552 return;
2553
2554 BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
2555
2556 osdDisplayPort = osdDisplayPortToUse;
2557
2558 #ifdef USE_CMS
2559 cmsDisplayPortRegister(osdDisplayPort);
2560 #endif
2561
2562 armState = ARMING_FLAG(ARMED);
2563
2564 displayClearScreen(osdDisplayPort);
2565
2566 uint8_t y = 4;
2567 char string_buffer[30];
2568 tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING);
2569 displayWrite(osdDisplayPort, 5, y++, string_buffer);
2570 #ifdef USE_CMS
2571 displayWrite(osdDisplayPort, 7, y++, CMS_STARTUP_HELP_TEXT1);
2572 displayWrite(osdDisplayPort, 11, y++, CMS_STARTUP_HELP_TEXT2);
2573 displayWrite(osdDisplayPort, 11, y++, CMS_STARTUP_HELP_TEXT3);
2574 #endif
2575
2576 #ifdef USE_STATS
2577 #define STATS_LABEL_X_POS 4
2578 #define STATS_VALUE_X_POS 24
2579 if (statsConfig()->stats_enabled) {
2580 displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "ODOMETER:");
2581 if (osdConfig()->units == OSD_UNIT_IMPERIAL) {
2582 tfp_sprintf(string_buffer, "%5d", statsConfig()->stats_total_dist / METERS_PER_MILE);
2583 string_buffer[5] = SYM_MI;
2584 } else {
2585 tfp_sprintf(string_buffer, "%5d", statsConfig()->stats_total_dist / METERS_PER_KILOMETER);
2586 string_buffer[5] = SYM_KM;
2587 }
2588 string_buffer[6] = '\0';
2589 displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer);
2590
2591 displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "TOTAL TIME:");
2592 uint32_t tot_mins = statsConfig()->stats_total_time / 60;
2593 tfp_sprintf(string_buffer, "%2d:%02dHM", tot_mins / 60, tot_mins % 60);
2594 displayWrite(osdDisplayPort, STATS_VALUE_X_POS-5, y, string_buffer);
2595
2596 #ifdef USE_ADC
2597 if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
2598 displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "TOTAL ENERGY:");
2599 osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4);
2600 strcat(string_buffer, "\xAB"); // SYM_WH
2601 displayWrite(osdDisplayPort, STATS_VALUE_X_POS-4, y, string_buffer);
2602
2603 displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:");
2604 if (statsConfig()->stats_total_dist) {
2605 uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
2606 osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3);
2607 } else
2608 strcpy(string_buffer, "---");
2609 string_buffer[3] = SYM_WH_KM_0;
2610 string_buffer[4] = SYM_WH_KM_1;
2611 string_buffer[5] = '\0';
2612 displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y, string_buffer);
2613 }
2614 #endif // USE_ADC
2615 }
2616 #endif
2617
2618 displayResync(osdDisplayPort);
2619 osdSetNextRefreshIn(SPLASH_SCREEN_DISPLAY_TIME);
2620 }
2621
2622 static void osdResetStats(void)
2623 {
2624 stats.max_current = 0;
2625 stats.max_power = 0;
2626 stats.max_speed = 0;
2627 stats.min_voltage = 5000;
2628 stats.min_rssi = 99;
2629 stats.max_altitude = 0;
2630 }
2631
2632 static void osdUpdateStats(void)
2633 {
2634 int16_t value;
2635
2636 if (feature(FEATURE_GPS)) {
2637 value = osdGet3DSpeed();
2638 if (stats.max_speed < value)
2639 stats.max_speed = value;
2640
2641 if (stats.max_distance < GPS_distanceToHome)
2642 stats.max_distance = GPS_distanceToHome;
2643 }
2644
2645 value = getBatteryVoltage();
2646 if (stats.min_voltage > value)
2647 stats.min_voltage = value;
2648
2649 value = abs(getAmperage() / 100);
2650 if (stats.max_current < value)
2651 stats.max_current = value;
2652
2653 value = abs(getPower() / 100);
2654 if (stats.max_power < value)
2655 stats.max_power = value;
2656
2657 value = osdConvertRSSI();
2658 if (stats.min_rssi > value)
2659 stats.min_rssi = value;
2660
2661 stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude());
2662 }
2663
2664 /* Attention: NTSC screen only has 12 fully visible lines - it is FULL now! */
2665 static void osdShowStats(void)
2666 {
2667 const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" };
2668 uint8_t top = 1; /* first fully visible line */
2669 const uint8_t statNameX = 1;
2670 const uint8_t statValuesX = 20;
2671 char buff[10];
2672
2673 displayClearScreen(osdDisplayPort);
2674 if (IS_DISPLAY_PAL)
2675 displayWrite(osdDisplayPort, statNameX, top++, " --- STATS ---");
2676
2677 if (STATE(GPS_FIX)) {
2678 displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :");
2679 osdFormatVelocityStr(buff, stats.max_speed, true);
2680 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2681
2682 displayWrite(osdDisplayPort, statNameX, top, "MAX DISTANCE :");
2683 osdFormatDistanceStr(buff, stats.max_distance*100);
2684 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2685
2686 displayWrite(osdDisplayPort, statNameX, top, "TRAVELED DISTANCE:");
2687 osdFormatDistanceStr(buff, getTotalTravelDistance());
2688 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2689 }
2690
2691 displayWrite(osdDisplayPort, statNameX, top, "MAX ALTITUDE :");
2692 osdFormatAltitudeStr(buff, stats.max_altitude);
2693 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2694
2695 displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
2696 osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
2697 strcat(buff, "V");
2698 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2699
2700 displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
2701 itoa(stats.min_rssi, buff, 10);
2702 strcat(buff, "%");
2703 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2704
2705 if (feature(FEATURE_CURRENT_METER)) {
2706 displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :");
2707 itoa(stats.max_current, buff, 10);
2708 strcat(buff, "A");
2709 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2710
2711 displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :");
2712 itoa(stats.max_power, buff, 10);
2713 strcat(buff, "W");
2714 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2715
2716 if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
2717 displayWrite(osdDisplayPort, statNameX, top, "USED MAH :");
2718 tfp_sprintf(buff, "%d%c", getMAhDrawn(), SYM_MAH);
2719 } else {
2720 displayWrite(osdDisplayPort, statNameX, top, "USED WH :");
2721 osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
2722 strcat(buff, "\xAB"); // SYM_WH
2723 }
2724 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2725
2726 int32_t totalDistance = getTotalTravelDistance();
2727 if (totalDistance > 0) {
2728 displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :");
2729 if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH)
2730 tfp_sprintf(buff, "%d%c%c", getMAhDrawn() * 100000 / totalDistance,
2731 SYM_MAH_KM_0, SYM_MAH_KM_1);
2732 else {
2733 osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3);
2734 buff[3] = SYM_WH_KM_0;
2735 buff[4] = SYM_WH_KM_1;
2736 buff[5] = '\0';
2737 }
2738 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2739 }
2740 }
2741
2742 displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
2743 uint16_t flySeconds = getFlightTime();
2744 uint16_t flyMinutes = flySeconds / 60;
2745 flySeconds %= 60;
2746 uint16_t flyHours = flyMinutes / 60;
2747 flyMinutes %= 60;
2748 tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
2749 displayWrite(osdDisplayPort, statValuesX, top++, buff);
2750
2751 displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :");
2752 displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
2753 }
2754
2755 // called when motors armed
2756 static void osdShowArmed(void)
2757 {
2758 dateTime_t dt;
2759 char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
2760 char *date;
2761 char *time;
2762 // We need 6 visible rows
2763 uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 6 - 1);
2764
2765 displayClearScreen(osdDisplayPort);
2766 displayWrite(osdDisplayPort, 12, y, "ARMED");
2767 y += 2;
2768
2769 #if defined(USE_GPS)
2770 if (feature(FEATURE_GPS)) {
2771 if (STATE(GPS_FIX_HOME)) {
2772 osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
2773 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
2774 osdFormatCoordinate(buf, SYM_LON, GPS_home.lon);
2775 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
2776 y += 3;
2777 } else {
2778 strcpy(buf, "!NO HOME POSITION!");
2779 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
2780 y += 1;
2781 }
2782 }
2783 #endif
2784
2785 if (rtcGetDateTime(&dt)) {
2786 dateTimeFormatLocal(buf, &dt);
2787 dateTimeSplitFormatted(buf, &date, &time);
2788
2789 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
2790 displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
2791 }
2792 }
2793
2794 static void osdRefresh(timeUs_t currentTimeUs)
2795 {
2796 #ifdef USE_CMS
2797 if (IS_RC_MODE_ACTIVE(BOXOSD) && (!cmsInMenu) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) {
2798 #else
2799 if (IS_RC_MODE_ACTIVE(BOXOSD) && !(osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE))) {
2800 #endif
2801 displayClearScreen(osdDisplayPort);
2802 armState = ARMING_FLAG(ARMED);
2803 return;
2804 }
2805
2806 // detect arm/disarm
2807 if (armState != ARMING_FLAG(ARMED)) {
2808 if (ARMING_FLAG(ARMED)) {
2809 osdResetStats();
2810 osdShowArmed(); // reset statistic etc
2811 osdSetNextRefreshIn(ARMED_SCREEN_DISPLAY_TIME);
2812 } else {
2813 osdShowStats(); // show statistic
2814 osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
2815 }
2816
2817 armState = ARMING_FLAG(ARMED);
2818 }
2819
2820 if (resumeRefreshAt) {
2821 // If we already reached he time for the next refresh,
2822 // or THR is high or PITCH is high, resume refreshing.
2823 // Clear the screen first to erase other elements which
2824 // might have been drawn while the OSD wasn't refreshing.
2825
2826 if (!DELAYED_REFRESH_RESUME_COMMAND)
2827 refreshWaitForResumeCmdRelease = false;
2828
2829 if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) {
2830 displayClearScreen(osdDisplayPort);
2831 resumeRefreshAt = 0;
2832 } else {
2833 displayHeartbeat(osdDisplayPort);
2834 }
2835 return;
2836 }
2837
2838 #ifdef USE_CMS
2839 if (!displayIsGrabbed(osdDisplayPort)) {
2840 if (fullRedraw) {
2841 displayClearScreen(osdDisplayPort);
2842 fullRedraw = false;
2843 }
2844 osdDrawNextElement();
2845 displayHeartbeat(osdDisplayPort);
2846 #ifdef OSD_CALLS_CMS
2847 } else {
2848 cmsUpdate(currentTimeUs);
2849 #endif
2850 }
2851 #endif
2852 }
2853
2854 /*
2855 * Called periodically by the scheduler
2856 */
2857 void osdUpdate(timeUs_t currentTimeUs)
2858 {
2859 static uint32_t counter = 0;
2860
2861 // don't touch buffers if DMA transaction is in progress
2862 if (displayIsTransferInProgress(osdDisplayPort)) {
2863 return;
2864 }
2865
2866 #if defined(OSD_ALTERNATE_LAYOUT_COUNT) && OSD_ALTERNATE_LAYOUT_COUNT > 0
2867 // Check if the layout has changed. Higher numbered
2868 // boxes take priority.
2869 unsigned activeLayout;
2870 if (layoutOverride >= 0) {
2871 activeLayout = layoutOverride;
2872 } else if (osdConfig()->osd_failsafe_switch_layout && FLIGHT_MODE(FAILSAFE_MODE)) {
2873 activeLayout = 0;
2874 } else {
2875 #if OSD_ALTERNATE_LAYOUT_COUNT > 2
2876 if (IS_RC_MODE_ACTIVE(BOXOSDALT3))
2877 activeLayout = 3;
2878 else
2879 #endif
2880 #if OSD_ALTERNATE_LAYOUT_COUNT > 1
2881 if (IS_RC_MODE_ACTIVE(BOXOSDALT2))
2882 activeLayout = 2;
2883 else
2884 #endif
2885 if (IS_RC_MODE_ACTIVE(BOXOSDALT1))
2886 activeLayout = 1;
2887 else
2888 activeLayout = 0;
2889 }
2890 if (currentLayout != activeLayout) {
2891 currentLayout = activeLayout;
2892 osdStartFullRedraw();
2893 }
2894 #endif
2895
2896 #define DRAW_FREQ_DENOM 4
2897 #define STATS_FREQ_DENOM 50
2898 counter++;
2899
2900 if ((counter % STATS_FREQ_DENOM) == 0) {
2901 osdUpdateStats();
2902 }
2903
2904 if ((counter & DRAW_FREQ_DENOM) == 0) {
2905 // redraw values in buffer
2906 osdRefresh(currentTimeUs);
2907 } else {
2908 // rest of time redraw screen
2909 displayDrawScreen(osdDisplayPort);
2910 }
2911
2912 #ifdef USE_CMS
2913 // do not allow ARM if we are in menu
2914 if (displayIsGrabbed(osdDisplayPort)) {
2915 ENABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU);
2916 } else {
2917 DISABLE_ARMING_FLAG(ARMING_DISABLED_OSD_MENU);
2918 }
2919 #endif
2920 }
2921
2922 void osdStartFullRedraw(void)
2923 {
2924 fullRedraw = true;
2925 }
2926
2927 void osdOverrideLayout(int layout)
2928 {
2929 layoutOverride = constrain(layout, -1, ARRAYLEN(osdConfig()->item_pos) - 1);
2930 }
2931
2932 bool osdItemIsFixed(osd_items_e item)
2933 {
2934 return item == OSD_CROSSHAIRS ||
2935 item == OSD_ARTIFICIAL_HORIZON ||
2936 item == OSD_HORIZON_SIDEBARS;
2937 }
2938
2939 #endif // OSD
INAV: Navigation-enabled flight control software