src/main/io/dashboard.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 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdarg.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_DASHBOARD
  26
  27 #include "build/version.h"
  28 #include "build/build_config.h"
  29
  30 #include "drivers/time.h"
  31 #include "drivers/display_ug2864hsweg01.h"
  32
  33 #include "cms/cms.h"
  34
  35 #include "common/printf.h"
  36 #include "common/maths.h"
  37 #include "common/axis.h"
  38 #include "common/typeconversion.h"
  39
  40 #include "fc/config.h"
  41 #include "fc/controlrate_profile.h"
  42 #include "fc/runtime_config.h"
  43 #include "fc/rc_controls.h"
  44
  45 #include "flight/pid.h"
  46 #include "flight/imu.h"
  47 #include "flight/failsafe.h"
  48
  49 #include "io/dashboard.h"
  50 #include "io/displayport_oled.h"
  51
  52 #ifdef USE_GPS
  53 #include "io/gps.h"
  54 #endif
  55
  56 #include "navigation/navigation.h"
  57
  58 #include "rx/rx.h"
  59
  60 #include "sensors/battery.h"
  61 #include "sensors/sensors.h"
  62 #include "sensors/compass.h"
  63 #include "sensors/acceleration.h"
  64 #include "sensors/boardalignment.h"
  65 #include "sensors/gyro.h"
  66 #include "sensors/barometer.h"
  67
  68 #include "config/feature.h"
  69
  70
  71 controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
  72
  73 #define MICROSECONDS_IN_A_SECOND (1000 * 1000)
  74
  75 #define DASHBOARD_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
  76 #define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
  77
  78 static timeUs_t nextDisplayUpdateAt = 0;
  79 static bool displayPresent = false;
  80
  81 static displayPort_t *displayPort;
  82
  83 #define PAGE_TITLE_LINE_COUNT 1
  84
  85 static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
  86
  87 #define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
  88 #define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
  89
  90 #if defined(DASHBOARD_ARMED_BITMAP)
  91 static uint8_t armedBitmapRLE [] = { 128, 64,
  92         '\x00','\x00','\x04','\x80','\x80','\x03','\xc0','\xc0', // 0x0008
  93         '\x02','\x80','\x80','\x03','\x00','\x00','\x0b','\x80', // 0x0010
  94         '\x80','\x02','\xc0','\xc0','\x02','\x80','\x80','\x03', // 0x0018
  95         '\x00','\x00','\x63','\xfc','\xfe','\xdf','\x03','\x01', // 0x0020
  96         '\x71','\x79','\xf9','\xf1','\xc1','\x83','\x8f','\xfe', // 0x0028
  97         '\xfc','\x00','\x00','\x04','\xf8','\xfe','\xff','\x07', // 0x0030
  98         '\x83','\xf1','\xf1','\x02','\x79','\x71','\x21','\x03', // 0x0038
  99         '\x87','\xfe','\xfc','\x70','\x00','\x00','\x07','\xff', // 0x0040
 100         '\xff','\x03','\x00','\x00','\x05','\xff','\xff','\x03', // 0x0048
 101         '\x3f','\x7c','\xf8','\xf0','\xc0','\x80','\x00','\x00', // 0x0050
 102         '\x0b','\xfe','\xff','\xff','\x03','\x00','\x00','\x0c', // 0x0058
 103         '\xc0','\xf0','\xfe','\x7f','\x0f','\x7f','\xfe','\xf8', // 0x0060
 104         '\xc0','\x00','\x00','\x06','\x07','\x1f','\xff','\xfc', // 0x0068
 105         '\xf0','\x80','\x00','\x00','\x0e','\xe0','\xf8','\xfe', // 0x0070
 106         '\x3f','\x0f','\x01','\x00','\x00','\x03','\x01','\x03', // 0x0078
 107         '\x07','\x0e','\x0e','\x02','\x0c','\x0c','\x03','\x0d', // 0x0080
 108         '\x0b','\x03','\xff','\xfe','\xfe','\x02','\x86','\x07', // 0x0088
 109         '\x07','\x02','\x87','\xce','\xfe','\xff','\x23','\x03', // 0x0090
 110         '\x0d','\x0c','\x0c','\x04','\x0e','\x07','\x03','\x01', // 0x0098
 111         '\x00','\x00','\x08','\xff','\xff','\x03','\x00','\x00', // 0x00a0
 112         '\x05','\xff','\xff','\x03','\xfe','\x00','\x00','\x02', // 0x00a8
 113         '\x03','\x07','\x1f','\x3e','\x7c','\xf8','\xe0','\xc0', // 0x00b0
 114         '\x00','\x00','\x06','\xff','\xff','\x04','\x00','\x00', // 0x00b8
 115         '\x09','\xe0','\xf8','\xff','\x3f','\x07','\x01','\x00', // 0x00c0
 116         '\x00','\x03','\x01','\x07','\x3f','\xff','\xf8','\xe0', // 0x00c8
 117         '\x80','\x00','\x00','\x05','\x03','\x1f','\x7f','\xfe', // 0x00d0
 118         '\xf0','\xc0','\x00','\x00','\x08','\xe0','\xfc','\xff', // 0x00d8
 119         '\x3f','\x07','\x01','\x00','\x00','\x06','\xe0','\xf8', // 0x00e0
 120         '\x7c','\x1c','\x0e','\xc6','\xc6','\x03','\xe6','\x74', // 0x00e8
 121         '\x38','\x78','\xe3','\xe7','\x1f','\x3f','\x3f','\x02', // 0x00f0
 122         '\x1f','\xef','\xe7','\xf9','\x38','\x78','\xf6','\xe6', // 0x00f8
 123         '\xc6','\xc6','\x02','\x8e','\x0c','\x3c','\xf8','\xf0', // 0x0100
 124         '\xc0','\x00','\x00','\x07','\xff','\xff','\x03','\x00', // 0x0108
 125         '\x00','\x05','\xff','\xff','\x04','\x00','\x00','\x07', // 0x0110
 126         '\x01','\x03','\x07','\x1f','\x3e','\xfc','\xf0','\xe0', // 0x0118
 127         '\x80','\x0f','\xff','\xff','\x03','\x00','\x00','\x05', // 0x0120
 128         '\x80','\xe0','\xfc','\xff','\x3f','\x0f','\x0c','\x0c', // 0x0128
 129         '\x0b','\x0f','\x1f','\xff','\xfc','\xf0','\x80','\x00', // 0x0130
 130         '\x00','\x05','\x03','\x0f','\x7f','\xfe','\xf8','\xc0', // 0x0138
 131         '\x00','\x80','\xf0','\xfc','\xff','\x3f','\x07','\x00', // 0x0140
 132         '\x00','\x0a','\x07','\x1f','\x1e','\x38','\x30','\x71', // 0x0148
 133         '\x63','\x63','\x02','\x71','\x30','\x38','\x1e','\x0f', // 0x0150
 134         '\x07','\x00','\x00','\x04','\x03','\x0f','\x1f','\x38', // 0x0158
 135         '\x30','\x71','\x63','\x63','\x02','\x73','\x31','\x38', // 0x0160
 136         '\x3c','\x1f','\x0f','\x03','\x00','\x00','\x07','\x1f', // 0x0168
 137         '\x3f','\x3f','\x02','\x00','\x00','\x05','\x1f','\x3f', // 0x0170
 138         '\x3f','\x02','\x0f','\x00','\x00','\x0d','\x01','\x03', // 0x0178
 139         '\x0f','\x1f','\x3f','\x3f','\x02','\x1f','\x00','\x00', // 0x0180
 140         '\x03','\x10','\x3c','\x3f','\x1f','\x03','\x00','\x00', // 0x0188
 141         '\x11','\x03','\x1f','\x3f','\x3e','\x18','\x00','\x00', // 0x0190
 142         '\x06','\x01','\x0f','\x1f','\x3f','\x3f','\x02','\x1f', // 0x0198
 143         '\x03','\x00','\x00','\xaf','\xc0','\xf0','\xf8','\x38', // 0x01a0
 144         '\xf8','\xf8','\x02','\xe0','\x00','\x00','\x04','\xf8', // 0x01a8
 145         '\xf8','\x03','\x18','\x18','\x03','\xf8','\xf8','\x02', // 0x01b0
 146         '\xf0','\x00','\x00','\x02','\xf8','\xf8','\x03','\x38', // 0x01b8
 147         '\xf8','\xe0','\x00','\xe0','\xf8','\x38','\xf8','\xf8', // 0x01c0
 148         '\x03','\x00','\x00','\x02','\xf8','\xf8','\x03','\x98', // 0x01c8
 149         '\x98','\x05','\x18','\x00','\x00','\x02','\xf8','\xf8', // 0x01d0
 150         '\x03','\x18','\x18','\x04','\x38','\xf8','\xf0','\xe0', // 0x01d8
 151         '\x00','\x00','\x42','\x20','\x38','\x3e','\x3f','\x0f', // 0x01e0
 152         '\x07','\x06','\x06','\x02','\x0f','\x3f','\x3f','\x02', // 0x01e8
 153         '\x3c','\x20','\x00','\x3f','\x3f','\x03','\x00','\x03', // 0x01f0
 154         '\x1f','\x3f','\x39','\x20','\x00','\x00','\x02','\x3f', // 0x01f8
 155         '\x3f','\x03','\x00','\x0f','\x3f','\x3c','\x0f','\x01', // 0x0200
 156         '\x00','\x3f','\x3f','\x03','\x00','\x00','\x02','\x3f', // 0x0208
 157         '\x3f','\x03','\x31','\x31','\x05','\x30','\x00','\x00', // 0x0210
 158         '\x02','\x3f','\x3f','\x03','\x30','\x30','\x04','\x3c', // 0x0218
 159         '\x1f','\x0f','\x07','\x00','\x00','\x22',
 160 };
 161 #endif
 162
 163 static const char* const pageTitles[] = {
 164     FC_FIRMWARE_NAME,
 165     "ARMED",
 166     "STATUS"
 167 };
 168
 169 static const char* const gpsFixTypeText[] = {
 170     "NO",
 171     "2D",
 172     "3D"
 173 };
 174
 175 static const char* tickerCharacters = "|/-\\"; // use 2/4/8 characters so that the divide is optimal.
 176 #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
 177
 178 static timeUs_t nextPageAt;
 179 static bool forcePageChange;
 180 static pageId_e currentPageId;
 181
 182 static void resetDisplay(void)
 183 {
 184     displayPresent = ug2864hsweg01InitI2C();
 185 }
 186
 187 static void LCDprint(uint8_t i)
 188 {
 189    i2c_OLED_send_char(i);
 190 }
 191
 192 static void padLineBufferToChar(uint8_t toChar)
 193 {
 194     uint8_t length = strlen(lineBuffer);
 195     while (length < toChar - 1) {
 196         lineBuffer[length++] = ' ';
 197     }
 198     lineBuffer[length] = 0;
 199 }
 200
 201 #ifdef USE_GPS
 202 static void padLineBuffer(void)
 203 {
 204     padLineBufferToChar(sizeof(lineBuffer));
 205 }
 206
 207 static void padHalfLineBuffer(void)
 208 {
 209     uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
 210     padLineBufferToChar(halfLineIndex);
 211 }
 212 #endif
 213
 214 // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
 215 static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
 216     uint8_t i, j;
 217
 218     if (percent > 100)
 219         percent = 100;
 220
 221     j = (width * percent) / 100;
 222
 223     for (i = 0; i < j; i++)
 224         LCDprint(159); // full
 225
 226     if (j < width)
 227         LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill
 228
 229     for (i = j + 1; i < width; i++)
 230         LCDprint(154); // empty
 231 }
 232
 233 static void updateTicker(void)
 234 {
 235     static uint8_t tickerIndex = 0;
 236     i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
 237     i2c_OLED_send_char(tickerCharacters[tickerIndex]);
 238     tickerIndex++;
 239     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 240 }
 241
 242 static void updateRxStatus(void)
 243 {
 244     i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
 245     char rxStatus = '!';
 246     if (rxIsReceivingSignal()) {
 247         rxStatus = 'r';
 248     } if (rxAreFlightChannelsValid()) {
 249         rxStatus = 'R';
 250     }
 251     i2c_OLED_send_char(rxStatus);
 252 }
 253
 254 static void updateFailsafeStatus(void)
 255 {
 256     char failsafeIndicator = '?';
 257     switch (failsafePhase()) {
 258         case FAILSAFE_IDLE:
 259             failsafeIndicator = '-';
 260             break;
 261         case FAILSAFE_RX_LOSS_DETECTED:
 262             failsafeIndicator = 'R';
 263             break;
 264         case FAILSAFE_RX_LOSS_IDLE:
 265             failsafeIndicator = 'I';
 266             break;
 267         case FAILSAFE_RETURN_TO_HOME:
 268             failsafeIndicator = 'H';
 269             break;
 270         case FAILSAFE_LANDING:
 271             failsafeIndicator = 'l';
 272             break;
 273         case FAILSAFE_LANDED:
 274             failsafeIndicator = 'L';
 275             break;
 276         case FAILSAFE_RX_LOSS_MONITORING:
 277             failsafeIndicator = 'M';
 278             break;
 279         case FAILSAFE_RX_LOSS_RECOVERED:
 280             failsafeIndicator = 'r';
 281             break;
 282     }
 283     i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
 284     i2c_OLED_send_char(failsafeIndicator);
 285 }
 286
 287 static void showTitle(void)
 288 {
 289 #if defined(DASHBOARD_ARMED_BITMAP)
 290     if (currentPageId != PAGE_ARMED) {
 291         i2c_OLED_set_line(0);
 292         i2c_OLED_send_string(pageTitles[currentPageId]);
 293     }
 294 #else
 295     i2c_OLED_set_line(0);
 296     i2c_OLED_send_string(pageTitles[currentPageId]);
 297 #endif
 298 }
 299
 300 static void showWelcomePage(void)
 301 {
 302     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 303
 304     tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
 305     i2c_OLED_set_line(rowIndex++);
 306     i2c_OLED_send_string(lineBuffer);
 307
 308     i2c_OLED_set_line(rowIndex++);
 309     i2c_OLED_send_string(targetName);
 310 }
 311
 312 #if defined(DASHBOARD_ARMED_BITMAP)
 313 // RLE compressed bitmaps must be 128 width with vertical data orientation, and size included in file.
 314 static void bitmapDecompressAndShow(uint8_t *bitmap)
 315 {
 316     uint8_t data = 0, count = 0;
 317     uint16_t i;
 318     uint8_t width = *bitmap;
 319     bitmap++;
 320     uint8_t height = *bitmap;
 321     bitmap++;
 322     uint16_t bitmapSize = (width * height) / 8;
 323     for (i = 0; i < bitmapSize; i++) {
 324         if (count == 0) {
 325             data = *bitmap;
 326             bitmap++;
 327             if (data == *bitmap) {
 328                 bitmap++;
 329                 count = *bitmap;
 330                 bitmap++;
 331             }
 332             else {
 333                 count = 1;
 334             }
 335         }
 336         count--;
 337         i2c_OLED_send_byte(data);
 338     }
 339 }
 340
 341 static void showArmedPage(void)
 342 {
 343     i2c_OLED_set_line(2);
 344     bitmapDecompressAndShow(armedBitmapRLE);
 345 }
 346 #else
 347 void showArmedPage(void)
 348 {
 349 }
 350 #endif
 351
 352 static void showStatusPage(void)
 353 {
 354     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 355
 356     if (feature(FEATURE_VBAT)) {
 357         i2c_OLED_set_line(rowIndex++);
 358         tfp_sprintf(lineBuffer, "V: %d.%02d ", getBatteryVoltage() / 100, getBatteryVoltage() % 100);
 359         padLineBufferToChar(12);
 360         i2c_OLED_send_string(lineBuffer);
 361
 362         uint8_t batteryPercentage = calculateBatteryPercentage();
 363         drawHorizonalPercentageBar(10, batteryPercentage);
 364     }
 365
 366     if (feature(FEATURE_CURRENT_METER)) {
 367         i2c_OLED_set_line(rowIndex++);
 368         tfp_sprintf(lineBuffer, "mAh: %d", (int)getMAhDrawn());
 369         padLineBufferToChar(12);
 370         i2c_OLED_send_string(lineBuffer);
 371
 372         uint8_t capacityPercentage = calculateBatteryPercentage();
 373         drawHorizonalPercentageBar(10, capacityPercentage);
 374     }
 375
 376 #ifdef USE_GPS
 377     if (feature(FEATURE_GPS)) {
 378         tfp_sprintf(lineBuffer, "Sats: %d", gpsSol.numSat);
 379         padHalfLineBuffer();
 380         i2c_OLED_set_line(rowIndex);
 381         i2c_OLED_send_string(lineBuffer);
 382
 383         tfp_sprintf(lineBuffer, "Fix: %s", gpsFixTypeText[gpsSol.fixType]);
 384         padHalfLineBuffer();
 385         i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
 386         i2c_OLED_send_string(lineBuffer);
 387
 388         tfp_sprintf(lineBuffer, "HDOP: %d.%02d", gpsSol.hdop / 100, gpsSol.hdop % 100);
 389         padLineBuffer();
 390         i2c_OLED_set_line(rowIndex++);
 391         i2c_OLED_send_string(lineBuffer);
 392
 393         tfp_sprintf(lineBuffer, "La/Lo: %d/%d", (int)(gpsSol.llh.lat / GPS_DEGREES_DIVIDER), (int)(gpsSol.llh.lon / GPS_DEGREES_DIVIDER));
 394         padLineBuffer();
 395         i2c_OLED_set_line(rowIndex++);
 396         i2c_OLED_send_string(lineBuffer);
 397
 398     }
 399 #endif
 400
 401 #ifdef USE_MAG
 402     if (sensors(SENSOR_MAG)) {
 403         tfp_sprintf(lineBuffer, "HDG: %d", DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 404         padHalfLineBuffer();
 405         i2c_OLED_set_line(rowIndex);
 406         i2c_OLED_send_string(lineBuffer);
 407     }
 408 #endif
 409
 410 #ifdef USE_BARO
 411     if (sensors(SENSOR_BARO)) {
 412         int32_t alt = baroCalculateAltitude();
 413         tfp_sprintf(lineBuffer, "Alt: %d", (int)(alt / 100));
 414         padHalfLineBuffer();
 415         i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex);
 416         i2c_OLED_send_string(lineBuffer);
 417     }
 418 #endif
 419 }
 420
 421 void dashboardUpdate(timeUs_t currentTimeUs)
 422 {
 423     static uint8_t previousArmedState = 0;
 424     bool pageChanging;
 425
 426 #ifdef USE_CMS
 427     static bool wasGrabbed = false;
 428     if (displayIsGrabbed(displayPort)) {
 429         wasGrabbed = true;
 430         return;
 431     } else if (wasGrabbed) {
 432         pageChanging = true;
 433         wasGrabbed = false;
 434     } else {
 435         pageChanging = false;
 436     }
 437 #else
 438     pageChanging = false;
 439 #endif
 440
 441     bool updateNow = (int32_t)(currentTimeUs - nextDisplayUpdateAt) >= 0L;
 442
 443     if (!updateNow) {
 444         return;
 445     }
 446
 447     nextDisplayUpdateAt = currentTimeUs + DASHBOARD_UPDATE_FREQUENCY;
 448
 449     bool armedState = ARMING_FLAG(ARMED) ? true : false;
 450     bool armedStateChanged = armedState != previousArmedState;
 451     previousArmedState = armedState;
 452
 453     if (armedState) {
 454         if (!armedStateChanged) {
 455             return;
 456         }
 457         currentPageId = PAGE_ARMED;
 458         pageChanging = true;
 459     } else {
 460         if (armedStateChanged) {
 461             currentPageId = PAGE_STATUS;
 462             pageChanging = true;
 463         }
 464
 465         if ((currentPageId == PAGE_WELCOME) && ((int32_t)(currentTimeUs - nextPageAt) >= 0L)) {
 466             currentPageId = PAGE_STATUS;
 467             pageChanging = true;
 468         }
 469
 470         if (forcePageChange) {
 471             pageChanging = true;
 472             forcePageChange = false;
 473         }
 474     }
 475
 476     if (pageChanging) {
 477         // Some OLED displays do not respond on the first initialisation so refresh the display
 478         // when the page changes in the hopes the hardware responds.  This also allows the
 479         // user to power off/on the display or connect it while powered.
 480         if (!displayPresent) {
 481             resetDisplay();
 482         }
 483
 484         if (!displayPresent) {
 485             return;
 486         }
 487
 488         i2c_OLED_clear_display_quick();
 489         showTitle();
 490     }
 491
 492     if (!displayPresent) {
 493         return;
 494     }
 495
 496     switch (currentPageId) {
 497         case PAGE_WELCOME:
 498             showWelcomePage();
 499             break;
 500         case PAGE_ARMED:
 501             showArmedPage();
 502             break;
 503         case PAGE_STATUS:
 504             showStatusPage();
 505             break;
 506     }
 507
 508     if (!armedState) {
 509         updateFailsafeStatus();
 510         updateRxStatus();
 511         updateTicker();
 512     }
 513 }
 514
 515 void dashboardSetPage(pageId_e newPageId)
 516 {
 517     currentPageId = newPageId;
 518     forcePageChange = true;
 519 }
 520
 521 void dashboardInit(void)
 522 {
 523     delay(200);
 524     resetDisplay();
 525     delay(200);
 526
 527     displayPort = displayPortOledInit();
 528 #if defined(USE_CMS)
 529     cmsDisplayPortRegister(displayPort);
 530 #endif
 531
 532     dashboardSetPage(PAGE_WELCOME);
 533     const timeUs_t now = micros();
 534     dashboardSetNextPageChangeAt(now + 5 * MICROSECONDS_IN_A_SECOND);
 535
 536     dashboardUpdate(now);
 537 }
 538
 539 void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
 540 {
 541     nextPageAt = futureMicros;
 542 }
 543
 544 #endif