mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
777 lines
23 KiB
C
777 lines
23 KiB
C
/*
|
|
* This file is part of Cleanflight and Betaflight.
|
|
*
|
|
* Cleanflight and Betaflight are free software. You can redistribute
|
|
* this software and/or modify this software under the terms of the
|
|
* GNU General Public License as published by the Free Software
|
|
* Foundation, either version 3 of the License, or (at your option)
|
|
* any later version.
|
|
*
|
|
* Cleanflight and Betaflight are distributed in the hope that they
|
|
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#ifdef USE_DASHBOARD
|
|
|
|
#include "common/utils.h"
|
|
|
|
#include "build/version.h"
|
|
#include "build/debug.h"
|
|
|
|
#include "build/build_config.h"
|
|
|
|
#include "drivers/bus.h"
|
|
#include "drivers/display.h"
|
|
#include "drivers/display_ug2864hsweg01.h"
|
|
#include "drivers/time.h"
|
|
|
|
#include "cms/cms.h"
|
|
|
|
#include "common/printf.h"
|
|
#include "common/maths.h"
|
|
#include "common/axis.h"
|
|
#include "common/typeconversion.h"
|
|
|
|
#include "config/feature.h"
|
|
#include "pg/dashboard.h"
|
|
#include "pg/rx.h"
|
|
|
|
#include "config/config.h"
|
|
#include "fc/controlrate_profile.h"
|
|
#include "fc/rc_controls.h"
|
|
#include "fc/runtime_config.h"
|
|
|
|
#include "flight/pid.h"
|
|
#include "flight/imu.h"
|
|
#include "flight/failsafe.h"
|
|
|
|
#include "io/gps.h"
|
|
#include "io/dashboard.h"
|
|
#include "io/displayport_oled.h"
|
|
|
|
#include "blackbox/blackbox_io.h"
|
|
#include "blackbox/blackbox.h"
|
|
|
|
#include "rx/rx.h"
|
|
|
|
#include "scheduler/scheduler.h"
|
|
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/battery.h"
|
|
#include "sensors/compass.h"
|
|
#include "sensors/gyro.h"
|
|
#include "sensors/sensors.h"
|
|
|
|
#define MICROSECONDS_IN_A_SECOND (1000 * 1000)
|
|
|
|
#define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
|
|
#define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
|
|
|
|
static extDevice_t *dev;
|
|
|
|
static uint32_t nextDisplayUpdateAt = 0;
|
|
static bool dashboardPresent = false;
|
|
|
|
static displayPort_t *displayPort;
|
|
|
|
#define PAGE_TITLE_LINE_COUNT 1
|
|
|
|
static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
|
|
|
|
#define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
|
|
#define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
|
|
|
|
typedef void (*pageFnPtr)(void);
|
|
|
|
#define PAGE_FLAGS_NONE 0
|
|
#define PAGE_FLAGS_SKIP_CYCLING (1 << 0)
|
|
|
|
typedef struct pageEntry_s {
|
|
pageId_e id;
|
|
char *title;
|
|
pageFnPtr drawFn;
|
|
uint8_t flags;
|
|
} pageEntry_t;
|
|
|
|
static const char tickerCharacters[] = "|/-\\"; // use 2/4/8 characters so that the divide is optimal.
|
|
#define TICKER_CHARACTER_COUNT ARRAYLEN(tickerCharacters)
|
|
|
|
typedef enum {
|
|
PAGE_STATE_FLAG_NONE = 0,
|
|
PAGE_STATE_FLAG_CYCLE_ENABLED = (1 << 0),
|
|
PAGE_STATE_FLAG_FORCE_PAGE_CHANGE = (1 << 1),
|
|
} pageFlags_e;
|
|
|
|
typedef struct pageState_s {
|
|
bool pageChanging;
|
|
const pageEntry_t *page;
|
|
uint8_t pageFlags;
|
|
uint8_t cycleIndex;
|
|
uint32_t nextPageAt;
|
|
} pageState_t;
|
|
|
|
static pageState_t pageState;
|
|
|
|
static void resetDisplay(void)
|
|
{
|
|
dashboardPresent = ug2864hsweg01InitI2C(dev);
|
|
}
|
|
|
|
void LCDprint(uint8_t i)
|
|
{
|
|
i2c_OLED_send_char(dev, i);
|
|
}
|
|
|
|
static void padLineBuffer(void)
|
|
{
|
|
uint8_t length = strlen(lineBuffer);
|
|
while (length < sizeof(lineBuffer) - 1) {
|
|
lineBuffer[length++] = ' ';
|
|
}
|
|
lineBuffer[length] = 0;
|
|
}
|
|
|
|
#ifdef USE_GPS
|
|
static void padHalfLineBuffer(void)
|
|
{
|
|
uint8_t halfLineIndex = sizeof(lineBuffer) / 2;
|
|
uint8_t length = strlen(lineBuffer);
|
|
while (length < halfLineIndex - 1) {
|
|
lineBuffer[length++] = ' ';
|
|
}
|
|
lineBuffer[length] = 0;
|
|
}
|
|
#endif
|
|
|
|
// LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
|
|
static void drawHorizonalPercentageBar(uint8_t width,uint8_t percent)
|
|
{
|
|
uint8_t i, j;
|
|
|
|
if (percent > 100)
|
|
percent = 100;
|
|
|
|
j = (width * percent) / 100;
|
|
|
|
for (i = 0; i < j; i++)
|
|
LCDprint(159); // full
|
|
|
|
if (j < width)
|
|
LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill
|
|
|
|
for (i = j + 1; i < width; i++)
|
|
LCDprint(154); // empty
|
|
}
|
|
|
|
#if 0
|
|
static void fillScreenWithCharacters(void)
|
|
{
|
|
for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
|
|
for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
|
|
i2c_OLED_set_xy(dev, column, row);
|
|
i2c_OLED_send_char(dev, 'A' + column);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
static void updateTicker(void)
|
|
{
|
|
static uint8_t tickerIndex = 0;
|
|
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
|
|
i2c_OLED_send_char(dev, tickerCharacters[tickerIndex]);
|
|
tickerIndex++;
|
|
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
|
|
}
|
|
|
|
static void updateRxStatus(void)
|
|
{
|
|
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
|
|
char rxStatus = '!';
|
|
if (isRxReceivingSignal()) {
|
|
rxStatus = 'r';
|
|
} if (rxAreFlightChannelsValid()) {
|
|
rxStatus = 'R';
|
|
}
|
|
i2c_OLED_send_char(dev, rxStatus);
|
|
}
|
|
|
|
static void updateFailsafeStatus(void)
|
|
{
|
|
char failsafeIndicator = '?';
|
|
switch (failsafePhase()) {
|
|
case FAILSAFE_IDLE:
|
|
failsafeIndicator = '-';
|
|
break;
|
|
case FAILSAFE_RX_LOSS_DETECTED:
|
|
failsafeIndicator = 'R';
|
|
break;
|
|
case FAILSAFE_LANDING:
|
|
failsafeIndicator = 'l';
|
|
break;
|
|
case FAILSAFE_LANDED:
|
|
failsafeIndicator = 'L';
|
|
break;
|
|
case FAILSAFE_RX_LOSS_MONITORING:
|
|
failsafeIndicator = 'M';
|
|
break;
|
|
case FAILSAFE_RX_LOSS_RECOVERED:
|
|
failsafeIndicator = 'r';
|
|
break;
|
|
case FAILSAFE_GPS_RESCUE:
|
|
failsafeIndicator = 'G';
|
|
break;
|
|
}
|
|
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
|
|
i2c_OLED_send_char(dev, failsafeIndicator);
|
|
}
|
|
|
|
static void showTitle(void)
|
|
{
|
|
i2c_OLED_set_line(dev, 0);
|
|
i2c_OLED_send_string(dev, pageState.page->title);
|
|
}
|
|
|
|
static void handlePageChange(void)
|
|
{
|
|
i2c_OLED_clear_display_quick(dev);
|
|
showTitle();
|
|
}
|
|
|
|
static void drawRxChannel(uint8_t channelIndex, uint8_t width)
|
|
{
|
|
LCDprint(rcChannelLetters[channelIndex]);
|
|
|
|
const uint32_t percentage = (constrain(rcData[channelIndex], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / PWM_RANGE;
|
|
drawHorizonalPercentageBar(width - 1, percentage);
|
|
}
|
|
|
|
#define RX_CHANNELS_PER_PAGE_COUNT 14
|
|
static void showRxPage(void)
|
|
{
|
|
for (int channelIndex = 0; channelIndex < rxRuntimeState.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
|
|
i2c_OLED_set_line(dev, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
|
|
|
|
drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
|
|
|
|
if (channelIndex >= rxRuntimeState.channelCount) {
|
|
continue;
|
|
}
|
|
|
|
if (IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD) {
|
|
LCDprint(' ');
|
|
}
|
|
|
|
drawRxChannel(channelIndex + PAGE_TITLE_LINE_COUNT, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
|
|
}
|
|
}
|
|
|
|
static void showWelcomePage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
|
|
tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, targetName);
|
|
}
|
|
|
|
static void showArmedPage(void)
|
|
{
|
|
}
|
|
|
|
static void showProfilePage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
|
|
tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
|
|
const pidProfile_t *pidProfile = currentPidProfile;
|
|
for (int axis = 0; axis < 3; ++axis) {
|
|
tfp_sprintf(lineBuffer, "%s P:%3d I:%3d D:%3d",
|
|
axisTitles[axis],
|
|
pidProfile->pid[axis].P,
|
|
pidProfile->pid[axis].I,
|
|
pidProfile->pid[axis].D
|
|
);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
}
|
|
|
|
static void showRateProfilePage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
|
|
const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
|
|
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
|
|
|
|
tfp_sprintf(lineBuffer, " R P Y");
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "RcRate %3d %3d %3d",
|
|
controlRateConfig->rcRates[FD_ROLL],
|
|
controlRateConfig->rcRates[FD_PITCH],
|
|
controlRateConfig->rcRates[FD_YAW]
|
|
);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "Super %3d %3d %3d",
|
|
controlRateConfig->rates[FD_ROLL],
|
|
controlRateConfig->rates[FD_PITCH],
|
|
controlRateConfig->rates[FD_YAW]
|
|
);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "Expo %3d %3d %3d",
|
|
controlRateConfig->rcExpo[FD_ROLL],
|
|
controlRateConfig->rcExpo[FD_PITCH],
|
|
controlRateConfig->rcExpo[FD_YAW]
|
|
);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
|
|
#define SATELLITE_COUNT ARRAYLEN(GPS_svinfo)
|
|
#define SATELLITE_GRAPH_LEFT_OFFSET ((SCREEN_CHARACTER_COLUMN_COUNT - SATELLITE_COUNT) / 2)
|
|
|
|
#ifdef USE_GPS
|
|
static void showGpsPage(void)
|
|
{
|
|
if (!featureIsEnabled(FEATURE_GPS)) {
|
|
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
|
|
return;
|
|
}
|
|
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
|
|
static uint8_t gpsTicker = 0;
|
|
static uint32_t lastGPSSvInfoReceivedCount = 0;
|
|
if (dashboardGpsNavSvInfoRcvCount != lastGPSSvInfoReceivedCount) {
|
|
lastGPSSvInfoReceivedCount = dashboardGpsNavSvInfoRcvCount;
|
|
gpsTicker++;
|
|
gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
|
|
}
|
|
|
|
i2c_OLED_set_xy(dev, 0, rowIndex);
|
|
i2c_OLED_send_char(dev, tickerCharacters[gpsTicker]);
|
|
|
|
i2c_OLED_set_xy(dev, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
|
|
|
|
uint32_t index;
|
|
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
|
|
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo[index].cno * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
|
|
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
|
|
i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
|
|
}
|
|
|
|
|
|
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
|
|
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "RX: %d", dashboardGpsPacketCount);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "Dt: %d", gpsSol.navIntervalMs);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
strncpy(lineBuffer, dashboardGpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
|
|
padHalfLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
#endif
|
|
|
|
static void showBatteryPage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
|
|
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
|
|
tfp_sprintf(lineBuffer, "Volts: %d.%02d Cells: %d", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount());
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
|
|
}
|
|
|
|
if (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) {
|
|
|
|
int32_t amperage = getAmperage();
|
|
// 123456789012345678901
|
|
// Amp: DDD.D mAh: DDDDD
|
|
tfp_sprintf(lineBuffer, "Amp: %d.%d mAh: %d", amperage / 100, (amperage % 100) / 10, getMAhDrawn());
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
|
|
}
|
|
}
|
|
|
|
static void showSensorsPage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
static const char *format = "%s %5d %5d %5d";
|
|
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, " X Y Z");
|
|
|
|
#if defined(USE_ACC)
|
|
if (sensors(SENSOR_ACC)) {
|
|
tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC.x), lrintf(acc.accADC.y), lrintf(acc.accADC.z));
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
#endif
|
|
|
|
if (sensors(SENSOR_GYRO)) {
|
|
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
|
|
#ifdef USE_MAG
|
|
if (sensors(SENSOR_MAG)) {
|
|
tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC.x), lrintf(mag.magADC.y), lrintf(mag.magADC.z));
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
#endif
|
|
|
|
tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
/*
|
|
uint8_t length;
|
|
|
|
ftoa(EstG.A[X], lineBuffer);
|
|
length = strlen(lineBuffer);
|
|
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
|
|
lineBuffer[length++] = ' ';
|
|
lineBuffer[length+1] = 0;
|
|
}
|
|
ftoa(EstG.A[Y], lineBuffer + length);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
|
|
ftoa(EstG.A[Z], lineBuffer);
|
|
length = strlen(lineBuffer);
|
|
while (length < HALF_SCREEN_CHARACTER_COLUMN_COUNT) {
|
|
lineBuffer[length++] = ' ';
|
|
lineBuffer[length+1] = 0;
|
|
}
|
|
ftoa(smallAngle, lineBuffer + length);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
*/
|
|
|
|
}
|
|
|
|
static void showTasksPage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
static const char *format = "%2d%6d%5d%4d%4d";
|
|
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, "Task max avg mx% av%");
|
|
taskInfo_t taskInfo;
|
|
for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
|
|
getTaskInfo(taskId, &taskInfo);
|
|
if (taskInfo.isEnabled && taskId != TASK_SERIAL) {// don't waste a line of the display showing serial taskInfo
|
|
const int taskFrequency = (int)(1000000.0f / ((float)taskInfo.latestDeltaTimeUs));
|
|
const int maxLoad = taskInfo.maxExecutionTimeUs == 0 ? 0 : (taskInfo.maxExecutionTimeUs * taskFrequency) / 1000;
|
|
const int averageLoad = taskInfo.averageExecutionTime10thUs == 0 ? 0 : (taskInfo.averageExecutionTime10thUs * taskFrequency) / 10000;
|
|
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTime10thUs / 10, maxLoad, averageLoad);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef USE_BLACKBOX
|
|
static void showBBPage(void)
|
|
{
|
|
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
|
|
int8_t fileNo;
|
|
if (isBlackboxDeviceWorking()) {
|
|
switch (blackboxConfig()->device) {
|
|
case BLACKBOX_DEVICE_SDCARD:
|
|
fileNo = blackboxGetLogFileNo();
|
|
if( fileNo > 0) {
|
|
tfp_sprintf(lineBuffer, "File no: %d", fileNo);
|
|
} else {
|
|
tfp_sprintf(lineBuffer, "Not ready yet");
|
|
}
|
|
break;
|
|
default:
|
|
tfp_sprintf(lineBuffer, "Not supp. dev.");
|
|
break;
|
|
}
|
|
} else {
|
|
tfp_sprintf(lineBuffer, "BB not working");
|
|
}
|
|
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex++);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
#endif
|
|
|
|
#ifdef ENABLE_DEBUG_DASHBOARD_PAGE
|
|
|
|
static void showDebugPage(void)
|
|
{
|
|
for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
|
|
tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
|
|
padLineBuffer();
|
|
i2c_OLED_set_line(dev, rowIndex + PAGE_TITLE_LINE_COUNT);
|
|
i2c_OLED_send_string(dev, lineBuffer);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
static const pageEntry_t pages[PAGE_COUNT] = {
|
|
{ PAGE_WELCOME, FC_FIRMWARE_NAME, showWelcomePage, PAGE_FLAGS_SKIP_CYCLING },
|
|
{ PAGE_ARMED, "ARMED", showArmedPage, PAGE_FLAGS_SKIP_CYCLING },
|
|
{ PAGE_PROFILE, "PROFILE", showProfilePage, PAGE_FLAGS_NONE },
|
|
{ PAGE_RPROF, "RATE PROFILE", showRateProfilePage,PAGE_FLAGS_NONE },
|
|
#ifdef USE_GPS
|
|
{ PAGE_GPS, "GPS", showGpsPage, PAGE_FLAGS_NONE },
|
|
#endif
|
|
{ PAGE_RX, "RX", showRxPage, PAGE_FLAGS_NONE },
|
|
{ PAGE_BATTERY, "BATTERY", showBatteryPage, PAGE_FLAGS_NONE },
|
|
{ PAGE_SENSORS, "SENSORS", showSensorsPage, PAGE_FLAGS_NONE },
|
|
{ PAGE_TASKS, "TASKS", showTasksPage, PAGE_FLAGS_NONE },
|
|
#ifdef USE_BLACKBOX
|
|
{ PAGE_BB, "BLACK BOX", showBBPage, PAGE_FLAGS_NONE },
|
|
#endif
|
|
#ifdef ENABLE_DEBUG_DASHBOARD_PAGE
|
|
{ PAGE_DEBUG, "DEBUG", showDebugPage, PAGE_FLAGS_NONE },
|
|
#endif
|
|
};
|
|
|
|
|
|
void dashboardSetPage(pageId_e pageId)
|
|
{
|
|
for (int i = 0; i < PAGE_COUNT; i++) {
|
|
const pageEntry_t *candidatePage = &pages[i];
|
|
if (candidatePage->id == pageId) {
|
|
pageState.page = candidatePage;
|
|
}
|
|
}
|
|
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
|
|
}
|
|
|
|
void dashboardUpdate(timeUs_t currentTimeUs)
|
|
{
|
|
static uint8_t previousArmedState = 0;
|
|
|
|
#ifdef USE_CMS
|
|
if (displayIsGrabbed(displayPort)) {
|
|
return;
|
|
}
|
|
#endif
|
|
|
|
const bool updateNow = (int32_t)(currentTimeUs - nextDisplayUpdateAt) >= 0L;
|
|
if (!updateNow) {
|
|
return;
|
|
}
|
|
|
|
nextDisplayUpdateAt = currentTimeUs + DISPLAY_UPDATE_FREQUENCY;
|
|
|
|
bool armedState = ARMING_FLAG(ARMED) ? true : false;
|
|
bool armedStateChanged = armedState != previousArmedState;
|
|
previousArmedState = armedState;
|
|
|
|
if (armedState) {
|
|
if (!armedStateChanged) {
|
|
return;
|
|
}
|
|
dashboardSetPage(PAGE_ARMED);
|
|
pageState.pageChanging = true;
|
|
} else {
|
|
if (armedStateChanged) {
|
|
pageState.pageFlags |= PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
|
|
}
|
|
|
|
pageState.pageChanging = (pageState.pageFlags & PAGE_STATE_FLAG_FORCE_PAGE_CHANGE) ||
|
|
(((int32_t)(currentTimeUs - pageState.nextPageAt) >= 0L && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)));
|
|
if (pageState.pageChanging && (pageState.pageFlags & PAGE_STATE_FLAG_CYCLE_ENABLED)) {
|
|
|
|
do {
|
|
pageState.cycleIndex++;
|
|
pageState.cycleIndex = pageState.cycleIndex % PAGE_COUNT;
|
|
pageState.page = &pages[pageState.cycleIndex];
|
|
} while (pageState.page->flags & PAGE_FLAGS_SKIP_CYCLING);
|
|
}
|
|
}
|
|
|
|
if (pageState.pageChanging) {
|
|
pageState.pageFlags &= ~PAGE_STATE_FLAG_FORCE_PAGE_CHANGE;
|
|
pageState.nextPageAt = currentTimeUs + PAGE_CYCLE_FREQUENCY;
|
|
|
|
// Some OLED displays do not respond on the first initialisation so refresh the display
|
|
// when the page changes in the hopes the hardware responds. This also allows the
|
|
// user to power off/on the display or connect it while powered.
|
|
resetDisplay();
|
|
|
|
if (!dashboardPresent) {
|
|
return;
|
|
}
|
|
handlePageChange();
|
|
}
|
|
|
|
if (!dashboardPresent) {
|
|
return;
|
|
}
|
|
|
|
pageState.page->drawFn();
|
|
|
|
if (!armedState) {
|
|
updateFailsafeStatus();
|
|
updateRxStatus();
|
|
updateTicker();
|
|
}
|
|
|
|
}
|
|
|
|
void dashboardInit(void)
|
|
{
|
|
static extDevice_t dashBoardDev;
|
|
// TODO Use bus singleton
|
|
static busDevice_t dashBoardBus;
|
|
|
|
dashBoardDev.bus = &dashBoardBus;
|
|
|
|
dashBoardBus.busType_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
|
|
dashBoardDev.busType_u.i2c.address = dashboardConfig()->address;
|
|
dev = &dashBoardDev;
|
|
|
|
delay(200);
|
|
resetDisplay();
|
|
delay(200);
|
|
|
|
displayPort = displayPortOledInit(dev);
|
|
#if defined(USE_CMS)
|
|
if (dashboardPresent) {
|
|
cmsDisplayPortRegister(displayPort);
|
|
}
|
|
#endif
|
|
|
|
memset(&pageState, 0, sizeof(pageState));
|
|
dashboardSetPage(PAGE_WELCOME);
|
|
|
|
uint32_t now = micros();
|
|
dashboardUpdate(now);
|
|
|
|
dashboardSetNextPageChangeAt(now + PAGE_CYCLE_FREQUENCY);
|
|
}
|
|
|
|
void dashboardShowFixedPage(pageId_e pageId)
|
|
{
|
|
dashboardSetPage(pageId);
|
|
dashboardDisablePageCycling();
|
|
}
|
|
|
|
void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
|
|
{
|
|
pageState.nextPageAt = futureMicros;
|
|
}
|
|
|
|
void dashboardEnablePageCycling(void)
|
|
{
|
|
pageState.pageFlags |= PAGE_STATE_FLAG_CYCLE_ENABLED;
|
|
}
|
|
|
|
void dashboardResetPageCycling(void)
|
|
{
|
|
pageState.cycleIndex = PAGE_COUNT - 1; // start at first page
|
|
|
|
}
|
|
|
|
void dashboardDisablePageCycling(void)
|
|
{
|
|
pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED;
|
|
}
|
|
#endif // USE_DASHBOARD
|