src/main/io/osd.c

   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 <stdio.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30 #include <ctype.h>
  31
  32 #include "platform.h"
  33
  34 #ifdef OSD
  35
  36 #include "blackbox/blackbox.h"
  37 #include "blackbox/blackbox_io.h"
  38
  39 #include "build/debug.h"
  40 #include "build/version.h"
  41
  42 #include "cms/cms.h"
  43 #include "cms/cms_types.h"
  44 #include "cms/cms_menu_osd.h"
  45
  46 #include "common/maths.h"
  47 #include "common/printf.h"
  48 #include "common/typeconversion.h"
  49 #include "common/utils.h"
  50
  51 #include "config/config_profile.h"
  52 #include "config/feature.h"
  53 #include "config/parameter_group.h"
  54 #include "config/parameter_group_ids.h"
  55
  56 #include "drivers/max7456_symbols.h"
  57 #include "drivers/display.h"
  58 #include "drivers/system.h"
  59 #ifdef USE_RTC6705
  60 #include "drivers/vtx_soft_spi_rtc6705.h"
  61 #include "drivers/vtx_soft_spi_rtc6705.h"
  62 #elif defined(VTX)
  63 #include "drivers/vtx_rtc6705.h"
  64 #endif
  65
  66 #include "io/asyncfatfs/asyncfatfs.h"
  67 #include "io/flashfs.h"
  68 #include "io/gps.h"
  69 #include "io/osd.h"
  70 #include "io/vtx.h"
  71 #include "io/vtx_string.h"
  72
  73 #include "fc/config.h"
  74 #include "fc/rc_controls.h"
  75 #include "fc/runtime_config.h"
  76
  77 #include "flight/imu.h"
  78
  79 #include "rx/rx.h"
  80
  81 #include "sensors/barometer.h"
  82 #include "sensors/battery.h"
  83 #include "sensors/sensors.h"
  84
  85 #ifdef USE_HARDWARE_REVISION_DETECTION
  86 #include "hardware_revision.h"
  87 #endif
  88
  89 #define VIDEO_BUFFER_CHARS_PAL    480
  90
  91 // Character coordinate
  92
  93 #define OSD_POSITION_BITS 5 // 5 bits gives a range 0-31
  94 #define OSD_POS(x,y)  ((x & 0x001F) | ((y & 0x001F) << OSD_POSITION_BITS))
  95 #define OSD_X(x)      (x & 0x001F)
  96 #define OSD_Y(x)      ((x >> OSD_POSITION_BITS) & 0x001F)
  97
  98 // Blink control
  99
 100 bool blinkState = true;
 101
 102 static uint32_t blinkBits[(OSD_ITEM_COUNT + 31)/32];
 103 #define SET_BLINK(item) (blinkBits[(item) / 32] |= (1 << ((item) % 32)))
 104 #define CLR_BLINK(item) (blinkBits[(item) / 32] &= ~(1 << ((item) % 32)))
 105 #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32)))
 106 #define BLINK(item) (IS_BLINK(item) && blinkState)
 107
 108 // Things in both OSD and CMS
 109
 110 #define IS_HI(X)  (rcData[X] > 1750)
 111 #define IS_LO(X)  (rcData[X] < 1250)
 112 #define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
 113
 114 //extern uint8_t RSSI; // TODO: not used?
 115
 116 static uint16_t flyTime = 0;
 117 static uint8_t statRssi;
 118
 119 typedef struct statistic_s {
 120     int16_t max_speed;
 121     int16_t min_voltage; // /10
 122     int16_t max_current; // /10
 123     int16_t min_rssi;
 124     int16_t max_altitude;
 125 } statistic_t;
 126
 127 static statistic_t stats;
 128
 129 uint16_t refreshTimeout = 0;
 130 #define REFRESH_1S    12
 131
 132 static uint8_t armState;
 133
 134 static displayPort_t *osdDisplayPort;
 135
 136
 137 #define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees
 138 #define AH_MAX_ROLL 400  // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees
 139 #define AH_SIDEBAR_WIDTH_POS 7
 140 #define AH_SIDEBAR_HEIGHT_POS 3
 141
 142 PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
 143
 144 /**
 145  * Gets the correct altitude symbol for the current unit system
 146  */
 147 static char osdGetAltitudeSymbol()
 148 {
 149     switch (osdConfig()->units) {
 150         case OSD_UNIT_IMPERIAL:
 151             return 0xF;
 152         default:
 153             return 0xC;
 154     }
 155 }
 156
 157 /**
 158  * Converts altitude based on the current unit system.
 159  * @param alt Raw altitude (i.e. as taken from BaroAlt)
 160  */
 161 static int32_t osdGetAltitude(int32_t alt)
 162 {
 163     switch (osdConfig()->units) {
 164         case OSD_UNIT_IMPERIAL:
 165             return (alt * 328) / 100; // Convert to feet / 100
 166         default:
 167             return alt;               // Already in metre / 100
 168     }
 169 }
 170
 171 static void osdDrawSingleElement(uint8_t item)
 172 {
 173     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item))
 174         return;
 175
 176     uint8_t elemPosX = OSD_X(osdConfig()->item_pos[item]);
 177     uint8_t elemPosY = OSD_Y(osdConfig()->item_pos[item]);
 178
 179     uint8_t elemOffsetX = 0;
 180
 181     char buff[32];
 182
 183     switch(item) {
 184         case OSD_RSSI_VALUE:
 185         {
 186             uint16_t osdRssi = rssi * 100 / 1024; // change range
 187             if (osdRssi >= 100)
 188                 osdRssi = 99;
 189
 190             buff[0] = SYM_RSSI;
 191             sprintf(buff + 1, "%d", osdRssi);
 192             break;
 193         }
 194
 195         case OSD_MAIN_BATT_VOLTAGE:
 196         {
 197             buff[0] = SYM_BATT_5;
 198             sprintf(buff + 1, "%d.%1dV", getBatteryVoltage() / 10, getBatteryVoltage() % 10);
 199             break;
 200         }
 201
 202         case OSD_CURRENT_DRAW:
 203         {
 204             int32_t amperage = getAmperage();
 205             buff[0] = SYM_AMP;
 206             sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100);
 207             break;
 208         }
 209
 210         case OSD_MAH_DRAWN:
 211         {
 212             buff[0] = SYM_MAH;
 213             sprintf(buff + 1, "%d", getMAhDrawn());
 214             break;
 215         }
 216
 217 #ifdef GPS
 218         case OSD_GPS_SATS:
 219         {
 220             buff[0] = 0x1f;
 221             sprintf(buff + 1, "%d", GPS_numSat);
 222             break;
 223         }
 224
 225         case OSD_GPS_SPEED:
 226         {
 227             // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
 228             sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
 229             break;
 230         }
 231
 232         case OSD_GPS_LAT:
 233         case OSD_GPS_LON:
 234         {
 235             int32_t val;
 236             if (item == OSD_GPS_LAT) {
 237                 buff[0] = 0x64; // right arrow
 238                 val = GPS_coord[LAT];
 239             } else {
 240                 buff[0] = 0x60; // down arrow
 241                 val = GPS_coord[LON];
 242             }
 243             if (val >= 0) {
 244                 val += 1000000000;
 245             } else {
 246                 val -= 1000000000;
 247             }
 248             itoa(val, &buff[1], 10);
 249             buff[1] = buff[2];
 250             buff[2] = buff[3];
 251             buff[3] = '.';
 252             break;
 253         }
 254
 255 #endif // GPS
 256
 257         case OSD_ALTITUDE:
 258         {
 259             int32_t alt = osdGetAltitude(baro.BaroAlt);
 260             sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
 261             break;
 262         }
 263
 264         case OSD_ONTIME:
 265         {
 266             uint32_t seconds = micros() / 1000000;
 267             buff[0] = SYM_ON_M;
 268             sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60);
 269             break;
 270         }
 271
 272         case OSD_FLYTIME:
 273         {
 274             buff[0] = SYM_FLY_M;
 275             sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60);
 276             break;
 277         }
 278
 279         case OSD_FLYMODE:
 280         {
 281             char *p = "ACRO";
 282
 283             if (isAirmodeActive())
 284                 p = "AIR";
 285
 286             if (FLIGHT_MODE(FAILSAFE_MODE))
 287                 p = "!FS";
 288             else if (FLIGHT_MODE(ANGLE_MODE))
 289                 p = "STAB";
 290             else if (FLIGHT_MODE(HORIZON_MODE))
 291                 p = "HOR";
 292
 293             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
 294             return;
 295         }
 296
 297         case OSD_CRAFT_NAME:
 298         {
 299             if (strlen(systemConfig()->name) == 0)
 300                 strcpy(buff, "CRAFT_NAME");
 301             else {
 302                 for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) {
 303                     buff[i] = toupper((unsigned char)systemConfig()->name[i]);
 304                     if (systemConfig()->name[i] == 0)
 305                         break;
 306                 }
 307             }
 308
 309             break;
 310         }
 311
 312         case OSD_THROTTLE_POS:
 313         {
 314             buff[0] = SYM_THR;
 315             buff[1] = SYM_THR1;
 316             sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
 317             break;
 318         }
 319
 320 #if defined(VTX) || defined(USE_RTC6705)
 321         case OSD_VTX_CHANNEL:
 322         {
 323             // FIXME cleanup this when the VTX API is aligned for software vs hardware support of the RTC6705 - See SPRACINGF3NEO/SINGULARITY/SIRINFPV targets.
 324 #if defined(VTX)
 325             const char vtxBandLetter = vtx58BandLetter[vtxConfig()->vtx_band + 1];
 326             const char *vtxChannelName = vtx58ChannelNames[vtxConfig()->vtx_channel + 1];
 327             sprintf(buff, "%c:%s", vtxBandLetter, vtxChannelName);
 328 #elif defined(USE_RTC6705)
 329             sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1);
 330 #endif
 331             break;
 332         }
 333 #endif // VTX
 334
 335         case OSD_CROSSHAIRS:
 336             elemPosX = 14 - 1; // Offset for 1 char to the left
 337             elemPosY = 6;
 338             if (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL) {
 339                 ++elemPosY;
 340             }
 341             buff[0] = SYM_AH_CENTER_LINE;
 342             buff[1] = SYM_AH_CENTER;
 343             buff[2] = SYM_AH_CENTER_LINE_RIGHT;
 344             buff[3] = 0;
 345             break;
 346
 347         case OSD_ARTIFICIAL_HORIZON:
 348         {
 349             elemPosX = 14;
 350             elemPosY = 6 - 4; // Top center of the AH area
 351
 352             int rollAngle = attitude.values.roll;
 353             int pitchAngle = attitude.values.pitch;
 354
 355             if (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL) {
 356                 ++elemPosY;
 357             }
 358
 359             if (pitchAngle > AH_MAX_PITCH)
 360                 pitchAngle = AH_MAX_PITCH;
 361             if (pitchAngle < -AH_MAX_PITCH)
 362                 pitchAngle = -AH_MAX_PITCH;
 363             if (rollAngle > AH_MAX_ROLL)
 364                 rollAngle = AH_MAX_ROLL;
 365             if (rollAngle < -AH_MAX_ROLL)
 366                 rollAngle = -AH_MAX_ROLL;
 367
 368             // Convert pitchAngle to y compensation value
 369             pitchAngle = (pitchAngle / 8) - 41; // 41 = 4 * 9 + 5
 370
 371             for (int8_t x = -4; x <= 4; x++) {
 372                 int y = (-rollAngle * x) / 64;
 373                 y -= pitchAngle;
 374                 // y += 41; // == 4 * 9 + 5
 375                 if (y >= 0 && y <= 81) {
 376                     displayWriteChar(osdDisplayPort, elemPosX + x, elemPosY + (y / 9), (SYM_AH_BAR9_0 + (y % 9)));
 377                 }
 378             }
 379
 380             osdDrawSingleElement(OSD_HORIZON_SIDEBARS);
 381
 382             return;
 383         }
 384
 385         case OSD_HORIZON_SIDEBARS:
 386         {
 387             elemPosX = 14;
 388             elemPosY = 6;
 389
 390             if (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL) {
 391                 ++elemPosY;
 392             }
 393
 394             // Draw AH sides
 395             int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
 396             int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
 397             for (int8_t y = -hudheight; y <= hudheight; y++) {
 398                 displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, SYM_AH_DECORATION);
 399                 displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, SYM_AH_DECORATION);
 400             }
 401
 402             // AH level indicators
 403             displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_LEFT);
 404             displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_RIGHT);
 405
 406             return;
 407         }
 408
 409         case OSD_ROLL_PIDS:
 410         {
 411             const pidProfile_t *pidProfile = currentPidProfile;
 412             sprintf(buff, "ROL %3d %3d %3d", pidProfile->P8[PIDROLL], pidProfile->I8[PIDROLL], pidProfile->D8[PIDROLL]);
 413             break;
 414         }
 415
 416         case OSD_PITCH_PIDS:
 417         {
 418             const pidProfile_t *pidProfile = currentPidProfile;
 419             sprintf(buff, "PIT %3d %3d %3d", pidProfile->P8[PIDPITCH], pidProfile->I8[PIDPITCH], pidProfile->D8[PIDPITCH]);
 420             break;
 421         }
 422
 423         case OSD_YAW_PIDS:
 424         {
 425             const pidProfile_t *pidProfile = currentPidProfile;
 426             sprintf(buff, "YAW %3d %3d %3d", pidProfile->P8[PIDYAW], pidProfile->I8[PIDYAW], pidProfile->D8[PIDYAW]);
 427             break;
 428         }
 429
 430         case OSD_POWER:
 431         {
 432             sprintf(buff, "%dW", getAmperage() * getBatteryVoltage() / 1000);
 433             break;
 434         }
 435
 436         case OSD_PIDRATE_PROFILE:
 437         {
 438             const uint8_t pidProfileIndex = getCurrentPidProfileIndex();
 439             const uint8_t rateProfileIndex = getCurrentControlRateProfileIndex();
 440             sprintf(buff, "%d-%d", pidProfileIndex + 1, rateProfileIndex + 1);
 441             break;
 442         }
 443
 444         case OSD_MAIN_BATT_WARNING:
 445         {
 446             switch(getBatteryState()) {
 447                 case BATTERY_WARNING:
 448                     sprintf(buff, "LOW BATTERY");
 449                     break;
 450
 451                 case BATTERY_CRITICAL:
 452                     sprintf(buff, "LAND NOW");
 453                     elemOffsetX += 1;
 454                     break;
 455
 456                 default:
 457                     return;
 458             }
 459             break;
 460         }
 461
 462         case OSD_AVG_CELL_VOLTAGE:
 463         {
 464             uint16_t cellV = getBatteryVoltage() * 10 / getBatteryCellCount();
 465             buff[0] = SYM_BATT_5;
 466             sprintf(buff + 1, "%d.%dV", cellV / 100, cellV % 100);
 467             break;
 468         }
 469
 470         default:
 471             return;
 472     }
 473
 474     displayWrite(osdDisplayPort, elemPosX + elemOffsetX, elemPosY, buff);
 475 }
 476
 477 void osdDrawElements(void)
 478 {
 479     displayClearScreen(osdDisplayPort);
 480
 481     /* Hide OSD when OSDSW mode is active */
 482     if (IS_RC_MODE_ACTIVE(BOXOSD))
 483       return;
 484
 485 #if 0
 486     if (currentElement)
 487         osdDrawElementPositioningHelp();
 488 #else
 489     if (false)
 490         ;
 491 #endif
 492 #ifdef CMS
 493     else if (sensors(SENSOR_ACC) || displayIsGrabbed(osdDisplayPort))
 494 #else
 495     else if (sensors(SENSOR_ACC))
 496 #endif
 497     {
 498         osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
 499         osdDrawSingleElement(OSD_CROSSHAIRS);
 500     }
 501
 502     osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE);
 503     osdDrawSingleElement(OSD_RSSI_VALUE);
 504     osdDrawSingleElement(OSD_FLYTIME);
 505     osdDrawSingleElement(OSD_ONTIME);
 506     osdDrawSingleElement(OSD_FLYMODE);
 507     osdDrawSingleElement(OSD_THROTTLE_POS);
 508     osdDrawSingleElement(OSD_VTX_CHANNEL);
 509     osdDrawSingleElement(OSD_CURRENT_DRAW);
 510     osdDrawSingleElement(OSD_MAH_DRAWN);
 511     osdDrawSingleElement(OSD_CRAFT_NAME);
 512     osdDrawSingleElement(OSD_ALTITUDE);
 513     osdDrawSingleElement(OSD_ROLL_PIDS);
 514     osdDrawSingleElement(OSD_PITCH_PIDS);
 515     osdDrawSingleElement(OSD_YAW_PIDS);
 516     osdDrawSingleElement(OSD_POWER);
 517     osdDrawSingleElement(OSD_PIDRATE_PROFILE);
 518     osdDrawSingleElement(OSD_MAIN_BATT_WARNING);
 519     osdDrawSingleElement(OSD_AVG_CELL_VOLTAGE);
 520
 521 #ifdef GPS
 522 #ifdef CMS
 523     if (sensors(SENSOR_GPS) || displayIsGrabbed(osdDisplayPort))
 524 #else
 525     if (sensors(SENSOR_GPS))
 526 #endif
 527     {
 528         osdDrawSingleElement(OSD_GPS_SATS);
 529         osdDrawSingleElement(OSD_GPS_SPEED);
 530         osdDrawSingleElement(OSD_GPS_LAT);
 531         osdDrawSingleElement(OSD_GPS_LON);
 532     }
 533 #endif // GPS
 534 }
 535
 536 void pgResetFn_osdConfig(osdConfig_t *osdProfile)
 537 {
 538     osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(8, 1) | VISIBLE_FLAG;
 539     osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1) | VISIBLE_FLAG;
 540     osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG;
 541     osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG;
 542     osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 1) | VISIBLE_FLAG;
 543     osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(1, 1) | VISIBLE_FLAG;
 544     osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(13, 11) | VISIBLE_FLAG;
 545     osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(10, 12) | VISIBLE_FLAG;
 546     osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 7) | VISIBLE_FLAG;
 547     osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(25, 11) | VISIBLE_FLAG;
 548     osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 12) | VISIBLE_FLAG;
 549     osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(1, 11) | VISIBLE_FLAG;
 550     osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(26, 6) | VISIBLE_FLAG;
 551     osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(19, 1) | VISIBLE_FLAG;
 552     osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(23, 7) | VISIBLE_FLAG;
 553     osdProfile->item_pos[OSD_ROLL_PIDS] = OSD_POS(7, 13) | VISIBLE_FLAG;
 554     osdProfile->item_pos[OSD_PITCH_PIDS] = OSD_POS(7, 14) | VISIBLE_FLAG;
 555     osdProfile->item_pos[OSD_YAW_PIDS] = OSD_POS(7, 15) | VISIBLE_FLAG;
 556     osdProfile->item_pos[OSD_POWER] = OSD_POS(1, 10) | VISIBLE_FLAG;
 557     osdProfile->item_pos[OSD_PIDRATE_PROFILE] = OSD_POS(25, 10) | VISIBLE_FLAG;
 558     osdProfile->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(9, 10) | VISIBLE_FLAG;
 559     osdProfile->item_pos[OSD_AVG_CELL_VOLTAGE] = OSD_POS(12, 2) | VISIBLE_FLAG;
 560
 561     osdProfile->item_pos[OSD_GPS_LAT] = OSD_POS(18, 14) | VISIBLE_FLAG;
 562     osdProfile->item_pos[OSD_GPS_LON] = OSD_POS(18, 15) | VISIBLE_FLAG;
 563
 564     osdProfile->units = OSD_UNIT_METRIC;
 565     osdProfile->rssi_alarm = 20;
 566     osdProfile->cap_alarm = 2200;
 567     osdProfile->time_alarm = 10; // in minutes
 568     osdProfile->alt_alarm = 100; // meters or feet depend on configuration
 569 }
 570
 571 static void osdDrawLogo(int x, int y)
 572 {
 573     // display logo and help
 574     char fontOffset = 160;
 575     for (int row = 0; row < 4; row++) {
 576         for (int column = 0; column < 24; column++) {
 577             if (fontOffset != 255) // FIXME magic number
 578                 displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
 579         }
 580     }
 581 }
 582
 583 void osdInit(displayPort_t *osdDisplayPortToUse)
 584 {
 585     if (!osdDisplayPortToUse)
 586         return;
 587
 588     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(31,31));
 589
 590     osdDisplayPort = osdDisplayPortToUse;
 591 #ifdef CMS
 592     cmsDisplayPortRegister(osdDisplayPort);
 593 #endif
 594
 595     armState = ARMING_FLAG(ARMED);
 596
 597     memset(blinkBits, 0, sizeof(blinkBits));
 598
 599     displayClearScreen(osdDisplayPort);
 600
 601     osdDrawLogo(3, 1);
 602
 603     char string_buffer[30];
 604     sprintf(string_buffer, "V%s", FC_VERSION_STRING);
 605     displayWrite(osdDisplayPort, 20, 6, string_buffer);
 606 #ifdef CMS
 607     displayWrite(osdDisplayPort, 7, 8,  CMS_STARTUP_HELP_TEXT1);
 608     displayWrite(osdDisplayPort, 11, 9, CMS_STARTUP_HELP_TEXT2);
 609     displayWrite(osdDisplayPort, 11, 10, CMS_STARTUP_HELP_TEXT3);
 610 #endif
 611
 612     displayResync(osdDisplayPort);
 613
 614     refreshTimeout = 4 * REFRESH_1S;
 615 }
 616
 617 void osdUpdateAlarms(void)
 618 {
 619     // This is overdone?
 620     // uint16_t *itemPos = osdConfig()->item_pos;
 621
 622     int32_t alt = osdGetAltitude(baro.BaroAlt) / 100;
 623     statRssi = rssi * 100 / 1024;
 624
 625     if (statRssi < osdConfig()->rssi_alarm)
 626         SET_BLINK(OSD_RSSI_VALUE);
 627     else
 628         CLR_BLINK(OSD_RSSI_VALUE);
 629
 630     if (getBatteryState() == BATTERY_OK) {
 631         CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
 632         CLR_BLINK(OSD_MAIN_BATT_WARNING);
 633         CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
 634     } else {
 635         SET_BLINK(OSD_MAIN_BATT_VOLTAGE);
 636         SET_BLINK(OSD_MAIN_BATT_WARNING);
 637         SET_BLINK(OSD_AVG_CELL_VOLTAGE);
 638     }
 639
 640     if (STATE(GPS_FIX) == 0)
 641         SET_BLINK(OSD_GPS_SATS);
 642     else
 643         CLR_BLINK(OSD_GPS_SATS);
 644
 645     if (flyTime / 60 >= osdConfig()->time_alarm && ARMING_FLAG(ARMED))
 646         SET_BLINK(OSD_FLYTIME);
 647     else
 648         CLR_BLINK(OSD_FLYTIME);
 649
 650     if (getMAhDrawn() >= osdConfig()->cap_alarm)
 651         SET_BLINK(OSD_MAH_DRAWN);
 652     else
 653         CLR_BLINK(OSD_MAH_DRAWN);
 654
 655     if (alt >= osdConfig()->alt_alarm)
 656         SET_BLINK(OSD_ALTITUDE);
 657     else
 658         CLR_BLINK(OSD_ALTITUDE);
 659 }
 660
 661 void osdResetAlarms(void)
 662 {
 663     CLR_BLINK(OSD_RSSI_VALUE);
 664     CLR_BLINK(OSD_MAIN_BATT_VOLTAGE);
 665     CLR_BLINK(OSD_MAIN_BATT_WARNING);
 666     CLR_BLINK(OSD_GPS_SATS);
 667     CLR_BLINK(OSD_FLYTIME);
 668     CLR_BLINK(OSD_MAH_DRAWN);
 669     CLR_BLINK(OSD_ALTITUDE);
 670     CLR_BLINK(OSD_AVG_CELL_VOLTAGE);
 671 }
 672
 673 static void osdResetStats(void)
 674 {
 675     stats.max_current = 0;
 676     stats.max_speed = 0;
 677     stats.min_voltage = 500;
 678     stats.max_current = 0;
 679     stats.min_rssi = 99;
 680     stats.max_altitude = 0;
 681 }
 682
 683 static void osdUpdateStats(void)
 684 {
 685     int16_t value = 0;
 686 #ifdef GPS
 687     value = CM_S_TO_KM_H(GPS_speed);
 688 #endif
 689     if (stats.max_speed < value)
 690         stats.max_speed = value;
 691
 692     if (stats.min_voltage > getBatteryVoltage())
 693         stats.min_voltage = getBatteryVoltage();
 694
 695     value = getAmperage() / 100;
 696     if (stats.max_current < value)
 697         stats.max_current = value;
 698
 699     if (stats.min_rssi > statRssi)
 700         stats.min_rssi = statRssi;
 701
 702     if (stats.max_altitude < baro.BaroAlt)
 703         stats.max_altitude = baro.BaroAlt;
 704 }
 705
 706 static void osdGetBlackboxStatusString(char * buff, uint8_t len)
 707 {
 708     bool storageDeviceIsWorking = false;
 709     uint32_t storageUsed = 0;
 710     uint32_t storageTotal = 0;
 711
 712     switch (blackboxConfig()->device)
 713     {
 714 #ifdef USE_SDCARD
 715     case BLACKBOX_DEVICE_SDCARD:
 716         storageDeviceIsWorking = sdcard_isInserted() && sdcard_isFunctional() && (afatfs_getFilesystemState() == AFATFS_FILESYSTEM_STATE_READY);
 717         if (storageDeviceIsWorking) {
 718             storageTotal = sdcard_getMetadata()->numBlocks / 2000;
 719             storageUsed = storageTotal - (afatfs_getContiguousFreeSpace() / 1024000);
 720         }
 721         break;
 722 #endif
 723
 724 #ifdef USE_FLASHFS
 725     case BLACKBOX_DEVICE_FLASH:
 726         storageDeviceIsWorking = flashfsIsReady();
 727         if (storageDeviceIsWorking) {
 728             const flashGeometry_t *geometry = flashfsGetGeometry();
 729             storageTotal = geometry->totalSize / 1024;
 730             storageUsed = flashfsGetOffset() / 1024;
 731         }
 732         break;
 733 #endif
 734
 735     default:
 736         storageDeviceIsWorking = true;
 737     }
 738
 739     if (storageDeviceIsWorking) {
 740         uint16_t storageUsedPercent = (storageUsed * 100) / storageTotal;
 741         snprintf(buff, len, "%d%%", storageUsedPercent);
 742     } else {
 743         snprintf(buff, len, "FAULT");
 744     }
 745 }
 746
 747 static void osdShowStats(void)
 748 {
 749     uint8_t top = 2;
 750     char buff[10];
 751
 752     displayClearScreen(osdDisplayPort);
 753     displayWrite(osdDisplayPort, 2, top++, "  --- STATS ---");
 754
 755     if (STATE(GPS_FIX)) {
 756         displayWrite(osdDisplayPort, 2, top, "MAX SPEED        :");
 757         itoa(stats.max_speed, buff, 10);
 758         displayWrite(osdDisplayPort, 22, top++, buff);
 759     }
 760
 761     displayWrite(osdDisplayPort, 2, top, "MIN BATTERY      :");
 762     sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10);
 763     displayWrite(osdDisplayPort, 22, top++, buff);
 764
 765     displayWrite(osdDisplayPort, 2, top, "MIN RSSI         :");
 766     itoa(stats.min_rssi, buff, 10);
 767     strcat(buff, "%");
 768     displayWrite(osdDisplayPort, 22, top++, buff);
 769
 770     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 771         displayWrite(osdDisplayPort, 2, top, "MAX CURRENT      :");
 772         itoa(stats.max_current, buff, 10);
 773         strcat(buff, "A");
 774         displayWrite(osdDisplayPort, 22, top++, buff);
 775
 776         displayWrite(osdDisplayPort, 2, top, "USED MAH         :");
 777         itoa(getMAhDrawn(), buff, 10);
 778         strcat(buff, "\x07");
 779         displayWrite(osdDisplayPort, 22, top++, buff);
 780     }
 781
 782     displayWrite(osdDisplayPort, 2, top, "MAX ALTITUDE     :");
 783     int32_t alt = osdGetAltitude(stats.max_altitude);
 784     sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
 785     displayWrite(osdDisplayPort, 22, top++, buff);
 786
 787     if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
 788         displayWrite(osdDisplayPort, 2, top, "BLACKBOX         :");
 789         osdGetBlackboxStatusString(buff, 10);
 790         displayWrite(osdDisplayPort, 22, top++, buff);
 791     }
 792
 793     refreshTimeout = 60 * REFRESH_1S;
 794 }
 795
 796 // called when motors armed
 797 static void osdArmMotors(void)
 798 {
 799     displayClearScreen(osdDisplayPort);
 800     displayWrite(osdDisplayPort, 12, 7, "ARMED");
 801     refreshTimeout = REFRESH_1S / 2;
 802     osdResetStats();
 803 }
 804
 805 static void osdRefresh(timeUs_t currentTimeUs)
 806 {
 807     static uint8_t lastSec = 0;
 808     uint8_t sec;
 809
 810     // detect arm/disarm
 811     if (armState != ARMING_FLAG(ARMED)) {
 812         if (ARMING_FLAG(ARMED))
 813             osdArmMotors(); // reset statistic etc
 814         else
 815             osdShowStats(); // show statistic
 816
 817         armState = ARMING_FLAG(ARMED);
 818     }
 819
 820     osdUpdateStats();
 821
 822     sec = currentTimeUs / 1000000;
 823
 824     if (ARMING_FLAG(ARMED) && sec != lastSec) {
 825         flyTime++;
 826         lastSec = sec;
 827     }
 828
 829     if (refreshTimeout) {
 830         if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics
 831             refreshTimeout = 1;
 832         refreshTimeout--;
 833         if (!refreshTimeout)
 834             displayClearScreen(osdDisplayPort);
 835         return;
 836     }
 837
 838     blinkState = (currentTimeUs / 200000) % 2;
 839
 840 #ifdef CMS
 841     if (!displayIsGrabbed(osdDisplayPort)) {
 842         osdUpdateAlarms();
 843         osdDrawElements();
 844         displayHeartbeat(osdDisplayPort); // heartbeat to stop Minim OSD going back into native mode
 845 #ifdef OSD_CALLS_CMS
 846     } else {
 847         cmsUpdate(currentTimeUs);
 848 #endif
 849     }
 850 #endif
 851 }
 852
 853 /*
 854  * Called periodically by the scheduler
 855  */
 856 void osdUpdate(timeUs_t currentTimeUs)
 857 {
 858     static uint32_t counter = 0;
 859 #ifdef MAX7456_DMA_CHANNEL_TX
 860     // don't touch buffers if DMA transaction is in progress
 861     if (displayIsTransferInProgress(osdDisplayPort)) {
 862         return;
 863     }
 864 #endif // MAX7456_DMA_CHANNEL_TX
 865
 866     // redraw values in buffer
 867 #ifdef USE_MAX7456
 868 #define DRAW_FREQ_DENOM 5
 869 #else
 870 #define DRAW_FREQ_DENOM 10 // MWOSD @ 115200 baud
 871 #endif
 872     if (counter++ % DRAW_FREQ_DENOM == 0) {
 873         osdRefresh(currentTimeUs);
 874     } else { // rest of time redraw screen 10 chars per idle so it doesn't lock the main idle
 875         displayDrawScreen(osdDisplayPort);
 876     }
 877
 878 #ifdef CMS
 879     // do not allow ARM if we are in menu
 880     if (displayIsGrabbed(osdDisplayPort)) {
 881         DISABLE_ARMING_FLAG(OK_TO_ARM);
 882     }
 883 #endif
 884 }
 885 #endif // OSD