src/main/io/dashboard.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_DASHBOARD
  29
  30 #include "common/utils.h"
  31
  32 #include "build/version.h"
  33 #include "build/debug.h"
  34
  35 #include "build/build_config.h"
  36
  37 #include "drivers/bus.h"
  38 #include "drivers/display.h"
  39 #include "drivers/display_ug2864hsweg01.h"
  40 #include "drivers/time.h"
  41
  42 #include "cms/cms.h"
  43
  44 #include "common/printf.h"
  45 #include "common/maths.h"
  46 #include "common/axis.h"
  47 #include "common/typeconversion.h"
  48
  49 #include "config/feature.h"
  50 #include "pg/dashboard.h"
  51 #include "pg/rx.h"
  52
  53 #include "config/config.h"
  54 #include "fc/controlrate_profile.h"
  55 #include "fc/rc_controls.h"
  56 #include "fc/runtime_config.h"
  57
  58 #include "flight/pid.h"
  59 #include "flight/imu.h"
  60 #include "flight/failsafe.h"
  61
  62 #include "io/gps.h"
  63 #include "io/dashboard.h"
  64 #include "io/displayport_oled.h"
  65
  66 #include "blackbox/blackbox_io.h"
  67 #include "blackbox/blackbox.h"
  68
  69 #include "rx/rx.h"
  70
  71 #include "scheduler/scheduler.h"
  72
  73 #include "sensors/acceleration.h"
  74 #include "sensors/battery.h"
  75 #include "sensors/compass.h"
  76 #include "sensors/gyro.h"
  77 #include "sensors/sensors.h"
  78
  79 #define MICROSECONDS_IN_A_SECOND (1000 * 1000)
  80
  81 #define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
  82 #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
  83
  84 static extDevice_t *dev;
  85
  86 static uint32_t nextDisplayUpdateAt = 0;
  87 static bool dashboardPresent = false;
  88
  89 static displayPort_t *displayPort;
  90
  91 #define PAGE_TITLE_LINE_COUNT 1
  92
  93 static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
  94
  95 #define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
  96 #define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
  97
  98 typedef void (*pageFnPtr)(void);
  99
 100 #define PAGE_FLAGS_NONE         0
 101 #define PAGE_FLAGS_SKIP_CYCLING (1 << 0)
 102
 103 typedef struct pageEntry_s {
 104     pageId_e id;
 105     char *title;
 106     pageFnPtr drawFn;
 107     uint8_t flags;
 108 } pageEntry_t;
 109
 110 static const char tickerCharacters[] = "|/-\\"; // use 2/4/8 characters so that the divide is optimal.
 111 #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
 112
 113 typedef enum {
 114     PAGE_STATE_FLAG_NONE = 0,
 115     PAGE_STATE_FLAG_CYCLE_ENABLED = (1 << 0),
 116     PAGE_STATE_FLAG_FORCE_PAGE_CHANGE = (1 << 1),
 117 } pageFlags_e;
 118
 119 typedef struct pageState_s {
 120     bool pageChanging;
 121     const pageEntry_t *page;
 122     uint8_t pageFlags;
 123     uint8_t cycleIndex;
 124     uint32_t nextPageAt;
 125 } pageState_t;
 126
 127 static pageState_t pageState;
 128
 129 static void resetDisplay(void)
 130 {
 131     dashboardPresent = ug2864hsweg01InitI2C(dev);
 132 }
 133
 134 void LCDprint(uint8_t i)
 135 {
 136    i2c_OLED_send_char(dev, i);
 137 }
 138
 139 static void padLineBuffer(void)
 140 {
 141     uint8_t length = strlen(lineBuffer);
 142     while (length < sizeof(lineBuffer) - 1) {
 143         lineBuffer[length++] = ' ';
 144     }
 145     lineBuffer[length] = 0;
 146 }
 147
 148 #ifdef USE_GPS
 149 static void padHalfLineBuffer(void)
 150 {
 151     uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
 152     uint8_t length = strlen(lineBuffer);
 153     while (length < halfLineIndex - 1) {
 154         lineBuffer[length++] = ' ';
 155     }
 156     lineBuffer[length] = 0;
 157 }
 158 #endif
 159
 160 // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
 161 static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
 162 {
 163     uint8_t i, j;
 164
 165     if (percent > 100)
 166         percent = 100;
 167
 168     j = (width * percent) / 100;
 169
 170     for (i = 0; i < j; i++)
 171         LCDprint(159); // full
 172
 173     if (j < width)
 174         LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill
 175
 176     for (i = j + 1; i < width; i++)
 177         LCDprint(154); // empty
 178 }
 179
 180 #if 0
 181 static void fillScreenWithCharacters(void)
 182 {
 183     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
 184         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
 185             i2c_OLED_set_xy(dev, column, row);
 186             i2c_OLED_send_char(dev, 'A' + column);
 187         }
 188     }
 189 }
 190 #endif
 191
 192
 193 static void updateTicker(void)
 194 {
 195     static uint8_t tickerIndex = 0;
 196     i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
 197     i2c_OLED_send_char(dev, tickerCharacters[tickerIndex]);
 198     tickerIndex++;
 199     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 200 }
 201
 202 static void updateRxStatus(void)
 203 {
 204     i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
 205     char rxStatus = '!';
 206     if (rxIsReceivingSignal()) {
 207         rxStatus = 'r';
 208     } if (rxAreFlightChannelsValid()) {
 209         rxStatus = 'R';
 210     }
 211     i2c_OLED_send_char(dev, rxStatus);
 212 }
 213
 214 static void updateFailsafeStatus(void)
 215 {
 216     char failsafeIndicator = '?';
 217     switch (failsafePhase()) {
 218     case FAILSAFE_IDLE:
 219         failsafeIndicator = '-';
 220         break;
 221     case FAILSAFE_RX_LOSS_DETECTED:
 222         failsafeIndicator = 'R';
 223         break;
 224     case FAILSAFE_LANDING:
 225         failsafeIndicator = 'l';
 226         break;
 227     case FAILSAFE_LANDED:
 228         failsafeIndicator = 'L';
 229         break;
 230     case FAILSAFE_RX_LOSS_MONITORING:
 231         failsafeIndicator = 'M';
 232         break;
 233     case FAILSAFE_RX_LOSS_RECOVERED:
 234         failsafeIndicator = 'r';
 235         break;
 236     case FAILSAFE_GPS_RESCUE:
 237         failsafeIndicator = 'G';
 238         break;
 239     }
 240     i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
 241     i2c_OLED_send_char(dev, failsafeIndicator);
 242 }
 243
 244 static void showTitle(void)
 245 {
 246     i2c_OLED_set_line(dev, 0);
 247     i2c_OLED_send_string(dev, pageState.page->title);
 248 }
 249
 250 static void handlePageChange(void)
 251 {
 252     i2c_OLED_clear_display_quick(dev);
 253     showTitle();
 254 }
 255
 256 static void drawRxChannel(uint8_t channelIndex, uint8_t width)
 257 {
 258     LCDprint(rcChannelLetters[channelIndex]);
 259
 260     const uint32_t percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 261     drawHorizonalPercentageBar(width - 1, percentage);
 262 }
 263
 264 #define RX_CHANNELS_PER_PAGE_COUNT 14
 265 static void showRxPage(void)
 266 {
 267     for (int channelIndex = 0; channelIndex < rxRuntimeState.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
 268         i2c_OLED_set_line(dev, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
 269
 270         drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 271
 272         if (channelIndex >= rxRuntimeState.channelCount) {
 273             continue;
 274         }
 275
 276         if (IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD) {
 277             LCDprint(' ');
 278         }
 279
 280         drawRxChannel(channelIndex + PAGE_TITLE_LINE_COUNT, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 281     }
 282 }
 283
 284 static void showWelcomePage(void)
 285 {
 286     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 287
 288     tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
 289     i2c_OLED_set_line(dev, rowIndex++);
 290     i2c_OLED_send_string(dev, lineBuffer);
 291
 292     i2c_OLED_set_line(dev, rowIndex++);
 293     i2c_OLED_send_string(dev, targetName);
 294 }
 295
 296 static void showArmedPage(void)
 297 {
 298 }
 299
 300 static void showProfilePage(void)
 301 {
 302     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 303
 304     tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
 305     i2c_OLED_set_line(dev, rowIndex++);
 306     i2c_OLED_send_string(dev, lineBuffer);
 307
 308     static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
 309     const pidProfile_t *pidProfile = currentPidProfile;
 310     for (int axis = 0; axis < 3; ++axis) {
 311         tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d",
 312             axisTitles[axis],
 313             pidProfile->pid[axis].P,
 314             pidProfile->pid[axis].I,
 315             pidProfile->pid[axis].D
 316         );
 317         padLineBuffer();
 318         i2c_OLED_set_line(dev, rowIndex++);
 319         i2c_OLED_send_string(dev, lineBuffer);
 320     }
 321 }
 322
 323 static void showRateProfilePage(void)
 324 {
 325     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 326
 327     const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
 328     tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
 329     i2c_OLED_set_line(dev, rowIndex++);
 330     i2c_OLED_send_string(dev, lineBuffer);
 331
 332     const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
 333
 334     tfp_sprintf(lineBuffer, "         R   P   Y");
 335     padLineBuffer();
 336     i2c_OLED_set_line(dev, rowIndex++);
 337     i2c_OLED_send_string(dev, lineBuffer);
 338
 339     tfp_sprintf(lineBuffer, "RcRate %3d %3d %3d",
 340         controlRateConfig->rcRates[FD_ROLL],
 341         controlRateConfig->rcRates[FD_PITCH],
 342         controlRateConfig->rcRates[FD_YAW]
 343     );
 344     padLineBuffer();
 345     i2c_OLED_set_line(dev, rowIndex++);
 346     i2c_OLED_send_string(dev, lineBuffer);
 347
 348     tfp_sprintf(lineBuffer, "Super  %3d %3d %3d",
 349         controlRateConfig->rates[FD_ROLL],
 350         controlRateConfig->rates[FD_PITCH],
 351         controlRateConfig->rates[FD_YAW]
 352     );
 353     padLineBuffer();
 354     i2c_OLED_set_line(dev, rowIndex++);
 355     i2c_OLED_send_string(dev, lineBuffer);
 356
 357     tfp_sprintf(lineBuffer, "Expo   %3d %3d %3d",
 358         controlRateConfig->rcExpo[FD_ROLL],
 359         controlRateConfig->rcExpo[FD_PITCH],
 360         controlRateConfig->rcExpo[FD_YAW]
 361     );
 362     padLineBuffer();
 363     i2c_OLED_set_line(dev, rowIndex++);
 364     i2c_OLED_send_string(dev, lineBuffer);
 365 }
 366
 367 #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
 368 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
 369
 370 #ifdef USE_GPS
 371 static void showGpsPage(void)
 372 {
 373     if (!featureIsEnabled(FEATURE_GPS)) {
 374         pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 375         return;
 376     }
 377
 378     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 379
 380     static uint8_t gpsTicker = 0;
 381     static uint32_t lastGPSSvInfoReceivedCount = 0;
 382     if (GPS_svInfoReceivedCount != lastGPSSvInfoReceivedCount) {
 383         lastGPSSvInfoReceivedCount = GPS_svInfoReceivedCount;
 384         gpsTicker++;
 385         gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
 386     }
 387
 388     i2c_OLED_set_xy(dev, 0, rowIndex);
 389     i2c_OLED_send_char(dev, tickerCharacters[gpsTicker]);
 390
 391     i2c_OLED_set_xy(dev, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 392
 393     uint32_t index;
 394     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
 395         uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
 396         bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
 397         i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
 398     }
 399
 400
 401     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
 402     tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
 403     padLineBuffer();
 404     i2c_OLED_set_line(dev, rowIndex++);
 405     i2c_OLED_send_string(dev, lineBuffer);
 406
 407     tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
 408     padLineBuffer();
 409     i2c_OLED_set_line(dev, rowIndex++);
 410     i2c_OLED_send_string(dev, lineBuffer);
 411
 412     tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
 413     padHalfLineBuffer();
 414     i2c_OLED_set_line(dev, rowIndex);
 415     i2c_OLED_send_string(dev, lineBuffer);
 416
 417     tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
 418     padHalfLineBuffer();
 419     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
 420     i2c_OLED_send_string(dev, lineBuffer);
 421
 422     tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
 423     padHalfLineBuffer();
 424     i2c_OLED_set_line(dev, rowIndex);
 425     i2c_OLED_send_string(dev, lineBuffer);
 426
 427     tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors);
 428     padHalfLineBuffer();
 429     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
 430     i2c_OLED_send_string(dev, lineBuffer);
 431
 432     tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
 433     padHalfLineBuffer();
 434     i2c_OLED_set_line(dev, rowIndex);
 435     i2c_OLED_send_string(dev, lineBuffer);
 436
 437     tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
 438     padHalfLineBuffer();
 439     i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
 440     i2c_OLED_send_string(dev, lineBuffer);
 441
 442     strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
 443     padHalfLineBuffer();
 444     i2c_OLED_set_line(dev, rowIndex++);
 445     i2c_OLED_send_string(dev, lineBuffer);
 446 }
 447 #endif
 448
 449 static void showBatteryPage(void)
 450 {
 451     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 452
 453     if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
 454         tfp_sprintf(lineBuffer, "Volts: %d.%02d Cells: %d", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount());
 455         padLineBuffer();
 456         i2c_OLED_set_line(dev, rowIndex++);
 457         i2c_OLED_send_string(dev, lineBuffer);
 458
 459         uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
 460         i2c_OLED_set_line(dev, rowIndex++);
 461         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
 462     }
 463
 464     if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
 465
 466         int32_t amperage = getAmperage();
 467         // 123456789012345678901
 468         // Amp: DDD.D mAh: DDDDD
 469         tfp_sprintf(lineBuffer, "Amp: %d.%d mAh: %d", amperage / 100, (amperage % 100) / 10, getMAhDrawn());
 470         padLineBuffer();
 471         i2c_OLED_set_line(dev, rowIndex++);
 472         i2c_OLED_send_string(dev, lineBuffer);
 473
 474         uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
 475         i2c_OLED_set_line(dev, rowIndex++);
 476         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
 477     }
 478 }
 479
 480 static void showSensorsPage(void)
 481 {
 482     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 483     static const char *format = "%s %5d %5d %5d";
 484
 485     i2c_OLED_set_line(dev, rowIndex++);
 486     i2c_OLED_send_string(dev, "        X     Y     Z");
 487
 488 #if defined(USE_ACC)
 489     if (sensors(SENSOR_ACC)) {
 490         tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z]));
 491         padLineBuffer();
 492         i2c_OLED_set_line(dev, rowIndex++);
 493         i2c_OLED_send_string(dev, lineBuffer);
 494     }
 495 #endif
 496
 497     if (sensors(SENSOR_GYRO)) {
 498         tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
 499         padLineBuffer();
 500         i2c_OLED_set_line(dev, rowIndex++);
 501         i2c_OLED_send_string(dev, lineBuffer);
 502     }
 503
 504 #ifdef USE_MAG
 505     if (sensors(SENSOR_MAG)) {
 506         tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z]));
 507         padLineBuffer();
 508         i2c_OLED_set_line(dev, rowIndex++);
 509         i2c_OLED_send_string(dev, lineBuffer);
 510     }
 511 #endif
 512
 513     tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 514     padLineBuffer();
 515     i2c_OLED_set_line(dev, rowIndex++);
 516     i2c_OLED_send_string(dev, lineBuffer);
 517
 518     /*
 519     uint8_t length;
 520
 521     ftoa(EstG.A[X], lineBuffer);
 522     length = strlen(lineBuffer);
 523     while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
 524         lineBuffer[length++] = ' ';
 525         lineBuffer[length+1] = 0;
 526     }
 527     ftoa(EstG.A[Y], lineBuffer + length);
 528     padLineBuffer();
 529     i2c_OLED_set_line(dev, rowIndex++);
 530     i2c_OLED_send_string(dev, lineBuffer);
 531
 532     ftoa(EstG.A[Z], lineBuffer);
 533     length = strlen(lineBuffer);
 534     while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
 535         lineBuffer[length++] = ' ';
 536         lineBuffer[length+1] = 0;
 537     }
 538     ftoa(smallAngle, lineBuffer + length);
 539     padLineBuffer();
 540     i2c_OLED_set_line(dev, rowIndex++);
 541     i2c_OLED_send_string(dev, lineBuffer);
 542     */
 543
 544 }
 545
 546 static void showTasksPage(void)
 547 {
 548     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 549     static const char *format = "%2d%6d%5d%4d%4d";
 550
 551     i2c_OLED_set_line(dev, rowIndex++);
 552     i2c_OLED_send_string(dev, "Task max  avg mx% av%");
 553     taskInfo_t taskInfo;
 554     for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
 555         getTaskInfo(taskId, &taskInfo);
 556         if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo
 557             const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTimeUs));
 558             const int maxLoad = (taskInfo.maxExecutionTimeUs * taskFrequency + 5000) / 10000;
 559             const int averageLoad = (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 10000;
 560             tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs, maxLoad, averageLoad);
 561             padLineBuffer();
 562             i2c_OLED_set_line(dev, rowIndex++);
 563             i2c_OLED_send_string(dev, lineBuffer);
 564             if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
 565                 break;
 566             }
 567         }
 568     }
 569 }
 570
 571 #ifdef USE_BLACKBOX
 572 static void showBBPage(void)
 573 {
 574     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 575     int8_t fileNo;
 576     if (isBlackboxDeviceWorking()) {
 577         switch (blackboxConfig()->device) {
 578         case BLACKBOX_DEVICE_SDCARD:
 579             fileNo = blackboxGetLogFileNo();
 580             if( fileNo > 0) {
 581                 tfp_sprintf(lineBuffer, "File no: %d", fileNo);
 582             } else {
 583                 tfp_sprintf(lineBuffer, "Not ready yet");
 584             }
 585             break;
 586         default:
 587             tfp_sprintf(lineBuffer, "Not supp. dev.");
 588             break;
 589         }
 590     } else {
 591         tfp_sprintf(lineBuffer, "BB not working");
 592     }
 593
 594     padLineBuffer();
 595     i2c_OLED_set_line(dev, rowIndex++);
 596     i2c_OLED_send_string(dev, lineBuffer);
 597 }
 598 #endif
 599
 600 #ifdef ENABLE_DEBUG_DASHBOARD_PAGE
 601
 602 static void showDebugPage(void)
 603 {
 604     for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
 605         tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
 606         padLineBuffer();
 607         i2c_OLED_set_line(dev, rowIndex + PAGE_TITLE_LINE_COUNT);
 608         i2c_OLED_send_string(dev, lineBuffer);
 609     }
 610 }
 611 #endif
 612
 613
 614 static const pageEntry_t pages[PAGE_COUNT] = {
 615     { PAGE_WELCOME, FC_FIRMWARE_NAME,  showWelcomePage,    PAGE_FLAGS_SKIP_CYCLING },
 616     { PAGE_ARMED,   "ARMED",           showArmedPage,      PAGE_FLAGS_SKIP_CYCLING },
 617     { PAGE_PROFILE, "PROFILE",         showProfilePage,    PAGE_FLAGS_NONE },
 618     { PAGE_RPROF,   "RATE PROFILE",    showRateProfilePage,PAGE_FLAGS_NONE },
 619 #ifdef USE_GPS
 620     { PAGE_GPS,     "GPS",             showGpsPage,        PAGE_FLAGS_NONE },
 621 #endif
 622     { PAGE_RX,      "RX",              showRxPage,         PAGE_FLAGS_NONE },
 623     { PAGE_BATTERY, "BATTERY",         showBatteryPage,    PAGE_FLAGS_NONE },
 624     { PAGE_SENSORS, "SENSORS",         showSensorsPage,    PAGE_FLAGS_NONE },
 625     { PAGE_TASKS,   "TASKS",           showTasksPage,      PAGE_FLAGS_NONE },
 626 #ifdef USE_BLACKBOX
 627     { PAGE_BB,      "BLACK BOX",       showBBPage,         PAGE_FLAGS_NONE },
 628 #endif
 629 #ifdef ENABLE_DEBUG_DASHBOARD_PAGE
 630     { PAGE_DEBUG,   "DEBUG",           showDebugPage,      PAGE_FLAGS_NONE },
 631 #endif
 632 };
 633
 634
 635 void dashboardSetPage(pageId_e pageId)
 636 {
 637     for (int i = 0; i < PAGE_COUNT; i++) {
 638         const pageEntry_t *candidatePage = &pages[i];
 639         if (candidatePage->id == pageId) {
 640             pageState.page = candidatePage;
 641         }
 642     }
 643     pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 644 }
 645
 646 void dashboardUpdate(timeUs_t currentTimeUs)
 647 {
 648     static uint8_t previousArmedState = 0;
 649
 650 #ifdef USE_CMS
 651     if (displayIsGrabbed(displayPort)) {
 652         return;
 653     }
 654 #endif
 655
 656     const bool updateNow = (int32_t)(currentTimeUs - nextDisplayUpdateAt) >= 0L;
 657     if (!updateNow) {
 658         return;
 659     }
 660
 661     nextDisplayUpdateAt = currentTimeUs + DISPLAY_UPDATE_FREQUENCY;
 662
 663     bool armedState = ARMING_FLAG(ARMED) ? true : false;
 664     bool armedStateChanged = armedState != previousArmedState;
 665     previousArmedState = armedState;
 666
 667     if (armedState) {
 668         if (!armedStateChanged) {
 669             return;
 670         }
 671         dashboardSetPage(PAGE_ARMED);
 672         pageState.pageChanging = true;
 673     } else {
 674         if (armedStateChanged) {
 675             pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 676         }
 677
 678         pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) ||
 679                 (((int32_t)(currentTimeUs - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
 680         if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) {
 681
 682             do {
 683                 pageState.cycleIndex++;
 684                 pageState.cycleIndex = pageState.cycleIndex % PAGE_COUNT;
 685                 pageState.page = &pages[pageState.cycleIndex];
 686             } while (pageState.page->flags & PAGE_FLAGS_SKIP_CYCLING);
 687         }
 688     }
 689
 690     if (pageState.pageChanging) {
 691         pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 692         pageState.nextPageAt = currentTimeUs + PAGE_CYCLE_FREQUENCY;
 693
 694         // Some OLED displays do not respond on the first initialisation so refresh the display
 695         // when the page changes in the hopes the hardware responds.  This also allows the
 696         // user to power off/on the display or connect it while powered.
 697         resetDisplay();
 698
 699         if (!dashboardPresent) {
 700             return;
 701         }
 702         handlePageChange();
 703     }
 704
 705     if (!dashboardPresent) {
 706         return;
 707     }
 708
 709     pageState.page->drawFn();
 710
 711     if (!armedState) {
 712         updateFailsafeStatus();
 713         updateRxStatus();
 714         updateTicker();
 715     }
 716
 717 }
 718
 719 void dashboardInit(void)
 720 {
 721     static extDevice_t dashBoardDev;
 722     // TODO Use bus singleton
 723     static busDevice_t dashBoardBus;
 724
 725     dashBoardDev.bus = &dashBoardBus;
 726
 727     dashBoardBus.busType_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
 728     dashBoardDev.busType_u.i2c.address = dashboardConfig()->address;
 729     dev = &dashBoardDev;
 730
 731     delay(200);
 732     resetDisplay();
 733     delay(200);
 734
 735     displayPort = displayPortOledInit(dev);
 736 #if defined(USE_CMS)
 737     if (dashboardPresent) {
 738         cmsDisplayPortRegister(displayPort);
 739     }
 740 #endif
 741
 742     memset(&pageState, 0, sizeof(pageState));
 743     dashboardSetPage(PAGE_WELCOME);
 744
 745     uint32_t now = micros();
 746     dashboardUpdate(now);
 747
 748     dashboardSetNextPageChangeAt(now + PAGE_CYCLE_FREQUENCY);
 749 }
 750
 751 void dashboardShowFixedPage(pageId_e pageId)
 752 {
 753     dashboardSetPage(pageId);
 754     dashboardDisablePageCycling();
 755 }
 756
 757 void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
 758 {
 759     pageState.nextPageAt = futureMicros;
 760 }
 761
 762 void dashboardEnablePageCycling(void)
 763 {
 764     pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
 765 }
 766
 767 void dashboardResetPageCycling(void)
 768 {
 769     pageState.cycleIndex = PAGE_COUNT - 1; // start at first page
 770
 771 }
 772
 773 void dashboardDisablePageCycling(void)
 774 {
 775     pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
 776 }
 777 #endif // USE_DASHBOARD