src/main/io/display.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 #include "version.h"
  25
  26 #include "build_config.h"
  27
  28 #include "drivers/serial.h"
  29 #include "drivers/system.h"
  30 #include "drivers/display_ug2864hsweg01.h"
  31 #include "drivers/sensor.h"
  32 #include "drivers/accgyro.h"
  33 #include "drivers/compass.h"
  34
  35 #include "common/printf.h"
  36 #include "common/maths.h"
  37 #include "common/axis.h"
  38 #include "common/typeconversion.h"
  39
  40 #ifdef DISPLAY
  41
  42 #include "sensors/battery.h"
  43 #include "sensors/sensors.h"
  44 #include "sensors/compass.h"
  45 #include "sensors/acceleration.h"
  46 #include "sensors/gyro.h"
  47
  48 #include "rx/rx.h"
  49
  50 #include "io/rc_controls.h"
  51
  52 #include "flight/pid.h"
  53 #include "flight/imu.h"
  54
  55 #ifdef GPS
  56 #include "io/gps.h"
  57 #include "flight/navigation.h"
  58 #endif
  59
  60 #include "config/runtime_config.h"
  61
  62 #include "config/config.h"
  63
  64 #include "display.h"
  65
  66 controlRateConfig_t *getControlRateConfig(uint8_t profileIndex);
  67
  68 //#define ENABLE_DEBUG_OLED_PAGE
  69
  70 #define MILLISECONDS_IN_A_SECOND (1000 * 1000)
  71
  72 #define DISPLAY_UPDATE_FREQUENCY (MILLISECONDS_IN_A_SECOND / 10)
  73 #define PAGE_CYCLE_FREQUENCY (MILLISECONDS_IN_A_SECOND * 5)
  74
  75 static uint32_t nextDisplayUpdateAt = 0;
  76
  77 static rxConfig_t *rxConfig;
  78
  79 #define PAGE_TITLE_LINE_COUNT 1
  80
  81 static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
  82
  83 const char* pageTitles[] = {
  84     "CLEANFLIGHT",
  85     "ARMED",
  86     "BATTERY",
  87     "SENSORS",
  88     "RX",
  89     "PROFILE"
  90 #ifdef GPS
  91     ,"GPS"
  92 #endif
  93 #ifdef ENABLE_DEBUG_OLED_PAGE
  94     ,"DEBUG"
  95 #endif
  96 };
  97
  98 #define PAGE_COUNT (PAGE_RX + 1)
  99
 100 const uint8_t cyclePageIds[] = {
 101     PAGE_PROFILE,
 102 #ifdef GPS
 103     PAGE_GPS,
 104 #endif
 105     PAGE_RX,
 106     PAGE_BATTERY,
 107     PAGE_SENSORS
 108 #ifdef ENABLE_DEBUG_OLED_PAGE
 109     ,PAGE_DEBUG,
 110 #endif
 111 };
 112
 113 #define CYCLE_PAGE_ID_COUNT (sizeof(cyclePageIds) / sizeof(cyclePageIds[0]))
 114
 115 static const char* tickerCharacters = "|/-\\";
 116 #define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
 117
 118 typedef enum {
 119     PAGE_STATE_FLAG_NONE = 0,
 120     PAGE_STATE_FLAG_CYCLE_ENABLED = (1 << 0),
 121     PAGE_STATE_FLAG_FORCE_PAGE_CHANGE = (1 << 1)
 122 } pageFlags_e;
 123
 124 typedef struct pageState_s {
 125     bool pageChanging;
 126     pageId_e pageId;
 127     pageId_e pageIdBeforeArming;
 128     uint8_t pageFlags;
 129     uint8_t cycleIndex;
 130     uint32_t nextPageAt;
 131 } pageState_t;
 132
 133 static pageState_t pageState;
 134
 135 void resetDisplay(void) {
 136     ug2864hsweg01InitI2C();
 137 }
 138
 139 void LCDprint(uint8_t i) {
 140    i2c_OLED_send_char(i);
 141 }
 142
 143 void padLineBuffer(void)
 144 {
 145     uint8_t length = strlen(lineBuffer);
 146     while (length < sizeof(lineBuffer) - 1) {
 147         lineBuffer[length++] = ' ';
 148     }
 149 }
 150
 151 // LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
 152 void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
 153     uint8_t i, j;
 154
 155     if (percent > 100)
 156         percent = 100;
 157
 158     j = (width * percent) / 100;
 159
 160     for (i = 0; i < j; i++)
 161         LCDprint(159); // full
 162
 163     if (j < width)
 164         LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill
 165
 166     for (i = j + 1; i < width; i++)
 167         LCDprint(154); // empty
 168 }
 169
 170 #if 0
 171 void fillScreenWithCharacters()
 172 {
 173     for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
 174         for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
 175             i2c_OLED_set_xy(column, row);
 176             i2c_OLED_send_char('A' + column);
 177         }
 178     }
 179 }
 180 #endif
 181
 182 void updateTicker(void)
 183 {
 184     static uint8_t tickerIndex = 0;
 185     i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
 186     i2c_OLED_send_char(tickerCharacters[tickerIndex]);
 187     tickerIndex++;
 188     tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
 189 }
 190
 191 void showTitle()
 192 {
 193     i2c_OLED_set_line(0);
 194     i2c_OLED_send_string(pageTitles[pageState.pageId]);
 195 }
 196
 197 void handlePageChange(void)
 198 {
 199     // Some OLED displays do not respond on the first initialisation so refresh the display
 200     // when the page changes in the hopes the hardware responds.  This also allows the
 201     // user to power off/on the display or connect it while powered.
 202     resetDisplay();
 203
 204     i2c_OLED_clear_display_quick();
 205     showTitle();
 206 }
 207
 208 void drawRxChannel(uint8_t channelIndex, uint8_t width)
 209 {
 210     uint32_t percentage;
 211
 212     LCDprint(rcChannelLetters[channelIndex]);
 213
 214     percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN);
 215     drawHorizonalPercentageBar(width - 1, percentage);
 216 }
 217
 218 #define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
 219 #define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
 220
 221 #define RX_CHANNELS_PER_PAGE_COUNT 14
 222 void showRxPage(void)
 223 {
 224
 225     for (uint8_t channelIndex = 0; channelIndex < rxRuntimeConfig.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
 226         i2c_OLED_set_line((channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
 227
 228         drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 229
 230         if (channelIndex >= rxRuntimeConfig.channelCount) {
 231             continue;
 232         }
 233
 234         if (IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD) {
 235             LCDprint(' ');
 236         }
 237
 238         drawRxChannel(channelIndex + PAGE_TITLE_LINE_COUNT, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
 239     }
 240 }
 241
 242 void showWelcomePage(void)
 243 {
 244     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 245
 246     tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
 247     i2c_OLED_set_line(rowIndex++);
 248     i2c_OLED_send_string(lineBuffer);
 249
 250     tfp_sprintf(lineBuffer, "Target: %s", targetName);
 251     i2c_OLED_set_line(rowIndex++);
 252     i2c_OLED_send_string(lineBuffer);
 253 }
 254
 255 void showArmedPage(void)
 256 {
 257 }
 258
 259 void showProfilePage(void)
 260 {
 261     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 262
 263     tfp_sprintf(lineBuffer, "Profile: %d", getCurrentProfile());
 264     i2c_OLED_set_line(rowIndex++);
 265     i2c_OLED_send_string(lineBuffer);
 266
 267     uint8_t currentRateProfileIndex = getCurrentControlRateProfile();
 268     tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
 269     i2c_OLED_set_line(rowIndex++);
 270     i2c_OLED_send_string(lineBuffer);
 271
 272     controlRateConfig_t *controlRateConfig = getControlRateConfig(currentRateProfileIndex);
 273
 274     tfp_sprintf(lineBuffer, "RC Expo: %d", controlRateConfig->rcExpo8);
 275     padLineBuffer();
 276     i2c_OLED_set_line(rowIndex++);
 277     i2c_OLED_send_string(lineBuffer);
 278
 279     tfp_sprintf(lineBuffer, "RC Rate: %d", controlRateConfig->rcRate8);
 280     padLineBuffer();
 281     i2c_OLED_set_line(rowIndex++);
 282     i2c_OLED_send_string(lineBuffer);
 283
 284     tfp_sprintf(lineBuffer, "R&P Rate: %d", controlRateConfig->rollPitchRate);
 285     padLineBuffer();
 286     i2c_OLED_set_line(rowIndex++);
 287     i2c_OLED_send_string(lineBuffer);
 288
 289     tfp_sprintf(lineBuffer, "Yaw Rate: %d", controlRateConfig->yawRate);
 290     padLineBuffer();
 291     i2c_OLED_set_line(rowIndex++);
 292     i2c_OLED_send_string(lineBuffer);
 293
 294 }
 295 #define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
 296 #define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
 297
 298 #ifdef GPS
 299 void showGpsPage() {
 300     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 301
 302     i2c_OLED_set_xy(MAX(0, SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
 303
 304     uint32_t index;
 305     for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
 306         uint8_t bargraphValue = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
 307         bargraphValue = MIN(bargraphValue, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
 308         i2c_OLED_send_char(VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphValue);
 309     }
 310
 311     char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
 312     tfp_sprintf(lineBuffer, "Satellites: %d Fix: %c", GPS_numSat, fixChar);
 313     padLineBuffer();
 314     i2c_OLED_set_line(rowIndex++);
 315     i2c_OLED_send_string(lineBuffer);
 316
 317     tfp_sprintf(lineBuffer, "Lat: %d Lon: %d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER);
 318     padLineBuffer();
 319     i2c_OLED_set_line(rowIndex++);
 320     i2c_OLED_send_string(lineBuffer);
 321
 322     tfp_sprintf(lineBuffer, "Spd: %d cm/s GC: %d", GPS_speed, GPS_ground_course);
 323     padLineBuffer();
 324     i2c_OLED_set_line(rowIndex++);
 325     i2c_OLED_send_string(lineBuffer);
 326
 327     tfp_sprintf(lineBuffer, "RX: %d Delta: %d", GPS_packetCount, gpsData.lastMessage - gpsData.lastLastMessage);
 328     padLineBuffer();
 329     i2c_OLED_set_line(rowIndex++);
 330     i2c_OLED_send_string(lineBuffer);
 331
 332     tfp_sprintf(lineBuffer, "ERRs: %d TOs: %d", gpsData.errors, gpsData.timeouts);
 333     padLineBuffer();
 334     i2c_OLED_set_line(rowIndex++);
 335     i2c_OLED_send_string(lineBuffer);
 336
 337     strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
 338     padLineBuffer();
 339     i2c_OLED_set_line(rowIndex++);
 340     i2c_OLED_send_string(lineBuffer);
 341
 342 #ifdef GPS_PH_DEBUG
 343     tfp_sprintf(lineBuffer, "Angles: P:%d R:%d", GPS_angle[PITCH], GPS_angle[ROLL]);
 344     padLineBuffer();
 345     i2c_OLED_set_line(rowIndex++);
 346     i2c_OLED_send_string(lineBuffer);
 347 #endif
 348
 349 #if 0
 350     tfp_sprintf(lineBuffer, "%d %d %d %d", debug[0], debug[1], debug[2], debug[3]);
 351     padLineBuffer();
 352     i2c_OLED_set_line(rowIndex++);
 353     i2c_OLED_send_string(lineBuffer);
 354 #endif
 355 }
 356 #endif
 357
 358 void showBatteryPage(void)
 359 {
 360     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 361
 362     if (feature(FEATURE_VBAT)) {
 363         tfp_sprintf(lineBuffer, "Volts: %d.%1d Cells: %d", vbat / 10, vbat % 10, batteryCellCount);
 364         padLineBuffer();
 365         i2c_OLED_set_line(rowIndex++);
 366         i2c_OLED_send_string(lineBuffer);
 367
 368         uint8_t batteryPercentage = calculateBatteryPercentage();
 369         i2c_OLED_set_line(rowIndex++);
 370         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
 371     }
 372
 373     if (feature(FEATURE_CURRENT_METER)) {
 374         tfp_sprintf(lineBuffer, "Amps: %d.%2d mAh: %d", amperage / 100, amperage % 100, mAhDrawn);
 375         padLineBuffer();
 376         i2c_OLED_set_line(rowIndex++);
 377         i2c_OLED_send_string(lineBuffer);
 378
 379         uint8_t capacityPercentage = calculateBatteryCapacityRemainingPercentage();
 380         i2c_OLED_set_line(rowIndex++);
 381         drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
 382     }
 383 }
 384
 385 void showSensorsPage(void)
 386 {
 387     uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
 388     static const char *format = "%s %5d %5d %5d";
 389
 390     i2c_OLED_set_line(rowIndex++);
 391     i2c_OLED_send_string("        X     Y     Z");
 392
 393     if (sensors(SENSOR_ACC)) {
 394         tfp_sprintf(lineBuffer, format, "ACC", accSmooth[X], accSmooth[Y], accSmooth[Z]);
 395         padLineBuffer();
 396         i2c_OLED_set_line(rowIndex++);
 397         i2c_OLED_send_string(lineBuffer);
 398     }
 399
 400     if (sensors(SENSOR_GYRO)) {
 401         tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
 402         padLineBuffer();
 403         i2c_OLED_set_line(rowIndex++);
 404         i2c_OLED_send_string(lineBuffer);
 405     }
 406
 407 #ifdef MAG
 408     if (sensors(SENSOR_MAG)) {
 409         tfp_sprintf(lineBuffer, format, "MAG", magADC[X], magADC[Y], magADC[Z]);
 410         padLineBuffer();
 411         i2c_OLED_set_line(rowIndex++);
 412         i2c_OLED_send_string(lineBuffer);
 413     }
 414 #endif
 415
 416     tfp_sprintf(lineBuffer, format, "I&H", inclination.values.rollDeciDegrees, inclination.values.pitchDeciDegrees, heading);
 417     padLineBuffer();
 418     i2c_OLED_set_line(rowIndex++);
 419     i2c_OLED_send_string(lineBuffer);
 420
 421     uint8_t length;
 422
 423     ftoa(EstG.A[X], lineBuffer);
 424     length = strlen(lineBuffer);
 425     while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
 426         lineBuffer[length++] = ' ';
 427         lineBuffer[length+1] = 0;
 428     }
 429     ftoa(EstG.A[Y], lineBuffer + length);
 430     padLineBuffer();
 431     i2c_OLED_set_line(rowIndex++);
 432     i2c_OLED_send_string(lineBuffer);
 433
 434     ftoa(EstG.A[Z], lineBuffer);
 435     length = strlen(lineBuffer);
 436     while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
 437         lineBuffer[length++] = ' ';
 438         lineBuffer[length+1] = 0;
 439     }
 440     ftoa(smallAngle, lineBuffer + length);
 441     padLineBuffer();
 442     i2c_OLED_set_line(rowIndex++);
 443     i2c_OLED_send_string(lineBuffer);
 444
 445 }
 446
 447 #ifdef ENABLE_DEBUG_OLED_PAGE
 448
 449 void showDebugPage(void)
 450 {
 451     uint8_t rowIndex;
 452
 453     for (rowIndex = 0; rowIndex < 4; rowIndex++) {
 454         tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
 455         padLineBuffer();
 456         i2c_OLED_set_line(rowIndex + PAGE_TITLE_LINE_COUNT);
 457         i2c_OLED_send_string(lineBuffer);
 458     }
 459 }
 460 #endif
 461
 462 void updateDisplay(void)
 463 {
 464     uint32_t now = micros();
 465     static uint8_t previousArmedState = 0;
 466
 467     bool updateNow = (int32_t)(now - nextDisplayUpdateAt) >= 0L;
 468     if (!updateNow) {
 469         return;
 470     }
 471
 472     nextDisplayUpdateAt = now + DISPLAY_UPDATE_FREQUENCY;
 473
 474     bool armedState = ARMING_FLAG(ARMED) ? true : false;
 475     bool armedStateChanged = armedState != previousArmedState;
 476     previousArmedState = armedState;
 477
 478     if (armedState) {
 479         if (!armedStateChanged) {
 480             return;
 481         }
 482         pageState.pageIdBeforeArming = pageState.pageId;
 483         pageState.pageId = PAGE_ARMED;
 484         pageState.pageChanging = true;
 485     } else {
 486         if (armedStateChanged) {
 487             pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 488             pageState.pageId = pageState.pageIdBeforeArming;
 489         }
 490
 491         pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) ||
 492                 (((int32_t)(now - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
 493         if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) {
 494             pageState.cycleIndex++;
 495             pageState.cycleIndex = pageState.cycleIndex % CYCLE_PAGE_ID_COUNT;
 496             pageState.pageId = cyclePageIds[pageState.cycleIndex];
 497         }
 498     }
 499
 500     if (pageState.pageChanging) {
 501         handlePageChange();
 502         pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 503         pageState.nextPageAt = now + PAGE_CYCLE_FREQUENCY;
 504     }
 505
 506     switch(pageState.pageId) {
 507         case PAGE_WELCOME:
 508             showWelcomePage();
 509             break;
 510         case PAGE_ARMED:
 511             showArmedPage();
 512             break;
 513         case PAGE_BATTERY:
 514             showBatteryPage();
 515             break;
 516         case PAGE_SENSORS:
 517             showSensorsPage();
 518             break;
 519         case PAGE_RX:
 520             showRxPage();
 521             break;
 522         case PAGE_PROFILE:
 523             showProfilePage();
 524             break;
 525 #ifdef GPS
 526         case PAGE_GPS:
 527             if (feature(FEATURE_GPS)) {
 528                 showGpsPage();
 529             } else {
 530                 pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 531             }
 532             break;
 533 #endif
 534 #ifdef ENABLE_DEBUG_OLED_PAGE
 535         case PAGE_DEBUG:
 536             showDebugPage();
 537             break;
 538 #endif
 539     }
 540     if (!armedState) {
 541         updateTicker();
 542     }
 543 }
 544
 545 void displaySetPage(pageId_e pageId)
 546 {
 547     pageState.pageId = pageId;
 548     pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
 549 }
 550
 551 void displayInit(rxConfig_t *rxConfigToUse)
 552 {
 553     delay(200);
 554     resetDisplay();
 555     delay(200);
 556
 557     rxConfig = rxConfigToUse;
 558
 559     memset(&pageState, 0, sizeof(pageState));
 560     displaySetPage(PAGE_WELCOME);
 561
 562     updateDisplay();
 563
 564     displaySetNextPageChangeAt(micros() + (1000 * 1000 * 5));
 565 }
 566
 567 void displayShowFixedPage(pageId_e pageId)
 568 {
 569     displaySetPage(pageId);
 570     displayDisablePageCycling();
 571 }
 572
 573 void displaySetNextPageChangeAt(uint32_t futureMicros)
 574 {
 575     pageState.nextPageAt = futureMicros;
 576 }
 577
 578 void displayEnablePageCycling(void)
 579 {
 580     pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
 581 }
 582
 583 void displayResetPageCycling(void)
 584 {
 585     pageState.cycleIndex = CYCLE_PAGE_ID_COUNT - 1; // start at first page
 586
 587 }
 588
 589 void displayDisablePageCycling(void)
 590 {
 591     pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
 592 }
 593
 594 #endif