Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity
betaflight/src/main/io/cms.c
jflyper b42de4f2a4 Provision OSDMenuFuncPtr to return long
2016-11-01 01:31:09 +09:00

1335 lines
38 KiB
C
Raw Blame History

/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it 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 is distributed in the hope that it 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Created by Marcin Baliniak
OSD-CMS separation by jflyper
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "drivers/system.h"
#include "common/typeconversion.h"
#include "io/cms.h"
#include "io/cms_types.h"
#ifdef CANVAS
#include "io/canvas.h"
#endif
#include "io/flashfs.h"
#include "io/osd.h"
#include "io/display.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "io/cms.h"
#include "build/debug.h"
// Device management
#define CMS_MAX_DEVICE 4
cmsDeviceInitFuncPtr cmsDeviceInitFunc[CMS_MAX_DEVICE];
int cmsDeviceCount;
int cmsCurrentDevice = -1;
int cmsLastDevice = -1;
bool cmsDeviceRegister(cmsDeviceInitFuncPtr func)
{
if (cmsDeviceCount == CMS_MAX_DEVICE)
return false;
cmsDeviceInitFunc[cmsDeviceCount++] = func;
return true;
}
cmsDeviceInitFuncPtr cmsDeviceSelectCurrent(void)
{
if (cmsDeviceCount == 0)
return NULL;
if (cmsCurrentDevice < 0)
cmsCurrentDevice = 0;
return cmsDeviceInitFunc[cmsCurrentDevice];
}
cmsDeviceInitFuncPtr cmsDeviceSelectNext(void)
{
if (cmsDeviceCount == 0)
return NULL;
cmsCurrentDevice = (cmsCurrentDevice + 1) % cmsDeviceCount; // -1 Okay
return cmsDeviceInitFunc[cmsCurrentDevice];
}
#define CMS_UPDATE_INTERVAL 50 // msec
// XXX Why is this here? Something wrong?
// XXX Something like Drawing Context that holds all state variables would be the way...
int8_t lastCursorPos;
void cmsScreenClear(displayPort_t *instance)
{
instance->VTable->clear();
instance->cleared = true;
lastCursorPos = -1; // XXX Here
}
void cmsScreenBegin(displayPort_t *instance)
{
instance->VTable->begin();
instance->VTable->clear();
}
void cmsScreenEnd(displayPort_t *instance)
{
instance->VTable->end();
}
int cmsScreenWrite(displayPort_t *instance, uint8_t x, uint8_t y, char *s)
{
return instance->VTable->write(x, y, s);
}
void cmsScreenHeartBeat(displayPort_t *instance)
{
if (instance->VTable->heartbeat)
instance->VTable->heartbeat();
}
void cmsScreenResync(displayPort_t *instance)
{
if (instance->VTable->resync)
instance->VTable->resync();
}
void cmsScreenInit(displayPort_t *pDisp, cmsDeviceInitFuncPtr cmsDeviceInitFunc)
{
cmsDeviceInitFunc(pDisp);
}
// XXX LEFT_MENU_COLUMN and RIGHT_MENU_COLUMN must be adjusted
// dynamically depending on size of the active output device,
// or statically to accomodate sizes of all supported devices.
//
// Device characteristics
// OLED
// 21 cols x 8 rows
// 128x64 with 5x7 (6x8) : 21 cols x 8 rows
// MAX7456 (PAL)
// 30 cols x 16 rows
// MAX7456 (NTSC)
// 30 cols x 13 rows
// HoTT Telemetry Screen
// 21 cols x 8 rows
//
#define LEFT_MENU_COLUMN 1
#define RIGHT_MENU_COLUMN(p) ((p)->cols - 7)
#define MAX_MENU_ITEMS(p) ((p)->rows - 2)
bool cmsInMenu = false;
OSD_Entry *menuStack[10]; //tab to save menu stack
uint8_t menuStackHistory[10]; //current position in menu stack
uint8_t menuStackIdx = 0;
OSD_Entry menuMain[];
OSD_Entry *currentMenu = NULL;
OSD_Entry *nextPage = NULL;
int8_t currentCursorPos = 0;
uint8_t currentMenuIdx = 0;
uint16_t *currentElement = NULL;
#define IS_HI(X) (rcData[X] > 1750)
#define IS_LO(X) (rcData[X] < 1250)
#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750)
//key definiotion because API provide menu navigation over MSP/GUI app - not used NOW
#define KEY_ENTER 0
#define KEY_UP 1
#define KEY_DOWN 2
#define KEY_LEFT 3
#define KEY_RIGHT 4
#define KEY_ESC 5
#define BUTTON_TIME 250 // msec
#define BUTTON_PAUSE 500 // msec
void cmsUpdateMaxRows(displayPort_t *instance)
{
OSD_Entry *ptr;
currentMenuIdx = 0;
for (ptr = currentMenu; ptr->type != OME_END; ptr++)
currentMenuIdx++;
if (currentMenuIdx > MAX_MENU_ITEMS(instance))
currentMenuIdx = MAX_MENU_ITEMS(instance);
currentMenuIdx--;
}
static void cmsFtoa(int32_t value, char *floatString)
{
uint8_t k;
// np. 3450
itoa(100000 + value, floatString, 10); // Create string from abs of integer value
// 103450
floatString[0] = floatString[1];
floatString[1] = floatString[2];
floatString[2] = '.';
// 03.450
// usuwam koncowe zera i kropke
for (k = 5; k > 1; k--)
if (floatString[k] == '0' || floatString[k] == '.')
floatString[k] = 0;
else
break;
// oraz zero wiodonce
if (floatString[0] == '0')
floatString[0] = ' ';
}
void cmsPad(char *buf, int size)
{
int i;
for (i = 0 ; i < size ; i++) {
if (buf[i] == 0)
break;
}
for ( ; i < size ; i++) {
buf[i] = ' ';
}
buf[size] = 0;
}
int cmsDrawMenuEntry(displayPort_t *pDisplay, OSD_Entry *p, uint8_t row, bool drawPolled)
{
char buff[10];
int cnt = 0;
switch (p->type) {
case OME_Submenu:
if (IS_PRINTVALUE(p)) {
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, ">");
CLR_PRINTVALUE(p);
}
break;
case OME_Bool:
if (IS_PRINTVALUE(p) && p->data) {
if (*((uint8_t *)(p->data))) {
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
} else {
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
}
CLR_PRINTVALUE(p);
}
break;
case OME_TAB: {
if (IS_PRINTVALUE(p)) {
OSD_TAB_t *ptr = p->data;
//cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 5, row, (char *)ptr->names[*ptr->val]);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, (char *)ptr->names[*ptr->val]);
CLR_PRINTVALUE(p);
}
break;
}
case OME_VISIBLE:
#ifdef OSD
if (IS_PRINTVALUE(p) && p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
if (VISIBLE(*val)) {
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "YES");
} else {
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, "NO ");
}
CLR_PRINTVALUE(p);
}
#endif
break;
case OME_UINT8:
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT8_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPad(buff, 5);
//cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, " ");
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
CLR_PRINTVALUE(p);
}
break;
case OME_INT8:
if (IS_PRINTVALUE(p) && p->data) {
OSD_INT8_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPad(buff, 5);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
CLR_PRINTVALUE(p);
}
break;
case OME_UINT16:
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT16_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPad(buff, 5);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
CLR_PRINTVALUE(p);
}
break;
case OME_INT16:
if (IS_PRINTVALUE(p) && p->data) {
OSD_UINT16_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPad(buff, 5);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
CLR_PRINTVALUE(p);
}
break;
case OME_Poll_INT16:
if (p->data && drawPolled) {
OSD_UINT16_t *ptr = p->data;
itoa(*ptr->val, buff, 10);
cmsPad(buff, 5);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay), row, buff);
// PRINTVALUE not cleared on purpose
}
break;
case OME_FLOAT:
if (IS_PRINTVALUE(p) && p->data) {
OSD_FLOAT_t *ptr = p->data;
cmsFtoa(*ptr->val * ptr->multipler, buff);
cmsPad(buff, 5);
cnt = cmsScreenWrite(pDisplay, RIGHT_MENU_COLUMN(pDisplay) - 1, row, buff); // XXX One char left ???
CLR_PRINTVALUE(p);
}
break;
case OME_OSD_Exit:
case OME_Label:
case OME_END:
case OME_Back:
break;
}
return cnt;
}
void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTime)
{
UNUSED(currentTime);
uint8_t i;
OSD_Entry *p;
uint8_t top = (pDisplay->rows - currentMenuIdx) / 2 - 1;
// Polled (dynamic) value display denominator.
// XXX Need to denom based on absolute time
static uint8_t pollDenom = 0;
bool drawPolled = (++pollDenom % 8 == 0);
int16_t cnt = 0;
if (!currentMenu)
return;
if (pDisplay->cleared) {
for (p = currentMenu, i= 0; p->type != OME_END; p++, i++) {
SET_PRINTLABEL(p);
SET_PRINTVALUE(p);
}
if (i > MAX_MENU_ITEMS(pDisplay)) // max per page
{
nextPage = currentMenu + MAX_MENU_ITEMS(pDisplay);
if (nextPage->type == OME_END)
nextPage = NULL;
}
pDisplay->cleared = false;
}
// Cursor manipulation
while ((currentMenu + currentCursorPos)->type == OME_Label) // skip label
currentCursorPos++;
if (lastCursorPos >= 0 && currentCursorPos != lastCursorPos) {
cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, lastCursorPos + top, " ");
}
if (lastCursorPos != currentCursorPos) {
cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN, currentCursorPos + top, " >");
lastCursorPos = currentCursorPos;
}
// Print text labels
for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
if (IS_PRINTLABEL(p)) {
cnt += cmsScreenWrite(pDisplay, LEFT_MENU_COLUMN + 2, i + top, p->text);
CLR_PRINTLABEL(p);
if (cnt > pDisplay->batchsize)
return;
}
}
// Print values
// XXX Polled values at latter positions in the list may not be
// XXX printed if the cnt exceeds batchsize in the middle of the list.
for (i = 0, p = currentMenu; i < MAX_MENU_ITEMS(pDisplay) && p->type != OME_END; i++, p++) {
if (IS_PRINTVALUE(p)) {
cnt += cmsDrawMenuEntry(pDisplay, p, top + i, drawPolled);
if (cnt > pDisplay->batchsize)
return;
}
}
}
// XXX Needs separation
OSD_Entry menuPid[];
void cmsx_PidRead(void);
void cmsx_PidWriteback(void);
OSD_Entry menuRateExpo[];
void cmsx_RateExpoRead(void);
void cmsx_RateExpoWriteback(void);
long cmsMenuChange(displayPort_t *pDisplay, void *ptr)
{
if (ptr) {
// XXX (jflyper): This can be avoided by adding pre- and post-
// XXX (or onEnter and onExit) functions?
if (ptr == &menuPid[0])
cmsx_PidRead();
if (ptr == &menuRateExpo[0])
cmsx_RateExpoRead();
if ((OSD_Entry *)ptr != currentMenu) {
// Stack it and move to a new menu.
// (ptr == curretMenu case occurs when reopening for display sw)
menuStack[menuStackIdx] = currentMenu;
menuStackHistory[menuStackIdx] = currentCursorPos;
menuStackIdx++;
currentMenu = (OSD_Entry *)ptr;
currentCursorPos = 0;
}
cmsScreenClear(pDisplay);
cmsUpdateMaxRows(pDisplay);
}
return 0;
}
long cmsMenuBack(displayPort_t *pDisplay)
{
// becasue pids and rates may be stored in profiles we need some thicks to manipulate it
// hack to save pid profile
if (currentMenu == &menuPid[0])
cmsx_PidWriteback();
// hack - save rate config for current profile
if (currentMenu == &menuRateExpo[0])
cmsx_RateExpoWriteback();
if (menuStackIdx) {
cmsScreenClear(pDisplay);
menuStackIdx--;
nextPage = NULL;
currentMenu = menuStack[menuStackIdx];
currentCursorPos = menuStackHistory[menuStackIdx];
cmsUpdateMaxRows(pDisplay);
}
return 0;
}
// XXX This should go to device
void cmsComputeBatchsize(displayPort_t *pDisplay)
{
pDisplay->batchsize = (pDisplay->buftime < CMS_UPDATE_INTERVAL) ? pDisplay->bufsize : (pDisplay->bufsize * CMS_UPDATE_INTERVAL) / pDisplay->buftime;
}
// XXX Separation
void cmsx_FeatureRead(void);
void cmsx_FeatureWriteback(void);
void cmsx_InfoInit(void);
displayPort_t currentDisplay;
void cmsMenuOpen(void)
{
cmsDeviceInitFuncPtr initfunc;
if (!cmsInMenu) {
// New open
cmsInMenu = true;
DISABLE_ARMING_FLAG(OK_TO_ARM);
initfunc = cmsDeviceSelectCurrent();
cmsx_FeatureRead();
currentMenu = &menuMain[0];
} else {
// Switch display
cmsScreenEnd(&currentDisplay);
initfunc = cmsDeviceSelectNext();
}
if (!initfunc)
return;
cmsScreenInit(&currentDisplay, initfunc);
cmsComputeBatchsize(&currentDisplay);
cmsScreenBegin(&currentDisplay);
cmsMenuChange(&currentDisplay, currentMenu);
}
long cmsMenuExit(displayPort_t *pDisplay, void *ptr)
{
if (ptr) {
cmsScreenClear(pDisplay);
cmsScreenWrite(pDisplay, 5, 3, "REBOOTING...");
cmsScreenResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
delay(200);
// save local variables to configuration
cmsx_FeatureWriteback();
}
cmsInMenu = false;
cmsScreenEnd(pDisplay);
currentMenu = NULL;
if (ptr)
systemReset();
ENABLE_ARMING_FLAG(OK_TO_ARM);
return 0;
}
uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
{
uint16_t res = BUTTON_TIME;
OSD_Entry *p;
if (!currentMenu)
return res;
if (key == KEY_ESC) {
cmsMenuBack(pDisplay);
return BUTTON_PAUSE;
}
if (key == KEY_DOWN) {
if (currentCursorPos < currentMenuIdx) {
currentCursorPos++;
} else {
if (nextPage) // we have more pages
{
cmsScreenClear(pDisplay);
p = nextPage;
nextPage = currentMenu;
currentMenu = (OSD_Entry *)p;
currentCursorPos = 0;
cmsUpdateMaxRows(pDisplay);
} else {
currentCursorPos = 0;
}
}
}
if (key == KEY_UP) {
currentCursorPos--;
if ((currentMenu + currentCursorPos)->type == OME_Label && currentCursorPos > 0)
currentCursorPos--;
if (currentCursorPos == -1 || (currentMenu + currentCursorPos)->type == OME_Label) {
if (nextPage) {
cmsScreenClear(pDisplay);
p = nextPage;
nextPage = currentMenu;
currentMenu = (OSD_Entry *)p;
currentCursorPos = 0;
cmsUpdateMaxRows(pDisplay);
} else {
currentCursorPos = currentMenuIdx;
}
}
}
if (key == KEY_DOWN || key == KEY_UP)
return res;
p = currentMenu + currentCursorPos;
switch (p->type) {
case OME_Submenu:
case OME_OSD_Exit:
if (p->func && key == KEY_RIGHT) {
p->func(pDisplay, p->data);
res = BUTTON_PAUSE;
}
break;
case OME_Back:
cmsMenuBack(pDisplay);
res = BUTTON_PAUSE;
break;
case OME_Bool:
if (p->data) {
uint8_t *val = p->data;
if (key == KEY_RIGHT)
*val = 1;
else
*val = 0;
SET_PRINTVALUE(p);
}
break;
case OME_VISIBLE:
#ifdef OSD
if (p->data) {
uint32_t address = (uint32_t)p->data;
uint16_t *val;
val = (uint16_t *)address;
if (key == KEY_RIGHT)
*val |= VISIBLE_FLAG;
else
*val %= ~VISIBLE_FLAG;
SET_PRINTVALUE(p);
}
#endif
break;
case OME_UINT8:
case OME_FLOAT:
if (p->data) {
OSD_UINT8_t *ptr = p->data;
if (key == KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
else {
if (*ptr->val > ptr->min)
*ptr->val -= ptr->step;
}
SET_PRINTVALUE(p);
}
break;
case OME_TAB:
if (p->type == OME_TAB) {
OSD_TAB_t *ptr = p->data;
if (key == KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += 1;
}
else {
if (*ptr->val > 0)
*ptr->val -= 1;
}
if (p->func)
p->func(pDisplay, p->data);
SET_PRINTVALUE(p);
}
break;
case OME_INT8:
if (p->data) {
OSD_INT8_t *ptr = p->data;
if (key == KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
else {
if (*ptr->val > ptr->min)
*ptr->val -= ptr->step;
}
SET_PRINTVALUE(p);
}
break;
case OME_UINT16:
if (p->data) {
OSD_UINT16_t *ptr = p->data;
if (key == KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
else {
if (*ptr->val > ptr->min)
*ptr->val -= ptr->step;
}
SET_PRINTVALUE(p);
}
break;
case OME_INT16:
if (p->data) {
OSD_INT16_t *ptr = p->data;
if (key == KEY_RIGHT) {
if (*ptr->val < ptr->max)
*ptr->val += ptr->step;
}
else {
if (*ptr->val > ptr->min)
*ptr->val -= ptr->step;
}
SET_PRINTVALUE(p);
}
break;
case OME_Poll_INT16:
case OME_Label:
case OME_END:
break;
}
return res;
}
OSD_Entry menuRc[];
void cmsUpdate(displayPort_t *pDisplay, uint32_t currentTime)
{
static int16_t rcDelay = BUTTON_TIME;
static uint32_t lastCalled = 0;
static uint32_t lastCmsHeartBeat = 0;
uint8_t key = 0;
if (!cmsInMenu) {
// Detect menu invocation
if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
cmsMenuOpen();
rcDelay = BUTTON_PAUSE; // Tends to overshoot if BUTTON_TIME
}
} else {
if (rcDelay > 0) {
rcDelay -= (currentTime - lastCalled);
}
else if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
// Double enter = display switching
cmsMenuOpen();
rcDelay = BUTTON_PAUSE;
}
else if (IS_HI(PITCH)) {
key = KEY_UP;
rcDelay = BUTTON_TIME;
}
else if (IS_LO(PITCH)) {
key = KEY_DOWN;
rcDelay = BUTTON_TIME;
}
else if (IS_LO(ROLL)) {
key = KEY_LEFT;
rcDelay = BUTTON_TIME;
}
else if (IS_HI(ROLL)) {
key = KEY_RIGHT;
rcDelay = BUTTON_TIME;
}
else if ((IS_HI(YAW) || IS_LO(YAW)) && currentMenu != menuRc) // this menu is used to check transmitter signals so can't exit using YAW
{
key = KEY_ESC;
rcDelay = BUTTON_TIME;
}
//lastCalled = currentTime;
if (key && !currentElement) {
rcDelay = cmsHandleKey(&currentDisplay, key);
return;
}
cmsDrawMenu(pDisplay, currentTime);
if (currentTime > lastCmsHeartBeat + 500) {
// Heart beat for external CMS display device @ 500msec
// (Timeout @ 1000msec)
cmsScreenHeartBeat(&currentDisplay);
lastCmsHeartBeat = currentTime;
}
}
lastCalled = currentTime;
}
void cmsHandler(uint32_t unusedTime)
{
UNUSED(unusedTime);
if (cmsDeviceCount < 0)
return;
static uint32_t lastCalled = 0;
uint32_t now = millis();
if (now - lastCalled >= CMS_UPDATE_INTERVAL) {
cmsUpdate(&currentDisplay, now);
lastCalled = now;
}
}
void cmsInit(void)
{
cmsx_InfoInit();
}
//
// Menu tables, should eventually be all GONE!?
//
//
// IMU
//
OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1};
uint8_t tempPid[4][3];
static OSD_UINT8_t entryRollP = {&tempPid[PIDROLL][0], 10, 150, 1};
static OSD_UINT8_t entryRollI = {&tempPid[PIDROLL][1], 1, 150, 1};
static OSD_UINT8_t entryRollD = {&tempPid[PIDROLL][2], 0, 150, 1};
static OSD_UINT8_t entryPitchP = {&tempPid[PIDPITCH][0], 10, 150, 1};
static OSD_UINT8_t entryPitchI = {&tempPid[PIDPITCH][1], 1, 150, 1};
static OSD_UINT8_t entryPitchD = {&tempPid[PIDPITCH][2], 0, 150, 1};
static OSD_UINT8_t entryYawP = {&tempPid[PIDYAW][0], 10, 150, 1};
static OSD_UINT8_t entryYawI = {&tempPid[PIDYAW][1], 1, 150, 1};
static OSD_UINT8_t entryYawD = {&tempPid[PIDYAW][2], 0, 150, 1};
void cmsx_PidRead(void)
{
uint8_t i;
for (i = 0; i < 3; i++) {
tempPid[i][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i];
tempPid[i][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i];
tempPid[i][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i];
}
tempPid[3][0] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL];
tempPid[3][1] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL];
tempPid[3][2] = masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL];
}
void cmsx_PidWriteback(void)
{
uint8_t i;
for (i = 0; i < 3; i++) {
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[i] = tempPid[i][0];
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[i] = tempPid[i][1];
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[i] = tempPid[i][2];
}
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL] = tempPid[3][0];
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL] = tempPid[3][1];
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL] = tempPid[3][2];
}
OSD_Entry menuPid[] =
{
{"--- PID ---", OME_Label, NULL, NULL, 0},
{"ROLL P", OME_UINT8, NULL, &entryRollP, 0},
{"ROLL I", OME_UINT8, NULL, &entryRollI, 0},
{"ROLL D", OME_UINT8, NULL, &entryRollD, 0},
{"PITCH P", OME_UINT8, NULL, &entryPitchP, 0},
{"PITCH I", OME_UINT8, NULL, &entryPitchI, 0},
{"PITCH D", OME_UINT8, NULL, &entryPitchD, 0},
{"YAW P", OME_UINT8, NULL, &entryYawP, 0},
{"YAW I", OME_UINT8, NULL, &entryYawI, 0},
{"YAW D", OME_UINT8, NULL, &entryYawD, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
//
// Rate & Expo
//
controlRateConfig_t rateProfile;
static OSD_FLOAT_t entryRollRate = {&rateProfile.rates[0], 0, 250, 1, 10};
static OSD_FLOAT_t entryPitchRate = {&rateProfile.rates[1], 0, 250, 1, 10};
static OSD_FLOAT_t entryYawRate = {&rateProfile.rates[2], 0, 250, 1, 10};
static OSD_FLOAT_t entryRcRate = {&rateProfile.rcRate8, 0, 200, 1, 10};
static OSD_FLOAT_t entryRcYawRate = {&rateProfile.rcYawRate8, 0, 200, 1, 10};
static OSD_FLOAT_t entryRcExpo = {&rateProfile.rcExpo8, 0, 100, 1, 10};
static OSD_FLOAT_t entryRcExpoYaw = {&rateProfile.rcYawExpo8, 0, 100, 1, 10};
static OSD_FLOAT_t extryTpaEntry = {&rateProfile.dynThrPID, 0, 70, 1, 10};
static OSD_UINT16_t entryTpaBreak = {&rateProfile.tpa_breakpoint, 1100, 1800, 10};
static OSD_FLOAT_t entryPSetpoint = {&masterConfig.profile[0].pidProfile.setpointRelaxRatio, 0, 100, 1, 10};
static OSD_FLOAT_t entryDSetpoint = {&masterConfig.profile[0].pidProfile.dtermSetpointWeight, 0, 255, 1, 10};
void cmsx_RateExpoRead()
{
memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t));
}
void cmsx_RateExpoWriteback()
{
memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t));
}
OSD_Entry menuRateExpo[] =
{
{"--- RATE&EXPO ---", OME_Label, NULL, NULL, 0},
{"RC RATE", OME_FLOAT, NULL, &entryRcYawRate, 0},
{"RC YAW RATE", OME_FLOAT, NULL, &entryRcRate, 0},
{"ROLL SUPER", OME_FLOAT, NULL, &entryRollRate, 0},
{"PITCH SUPER", OME_FLOAT, NULL, &entryPitchRate, 0},
{"YAW SUPER", OME_FLOAT, NULL, &entryYawRate, 0},
{"RC EXPO", OME_FLOAT, NULL, &entryRcExpo, 0},
{"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw, 0},
{"THR PID ATT", OME_FLOAT, NULL, &extryTpaEntry, 0},
{"TPA BRKPT", OME_UINT16, NULL, &entryTpaBreak, 0},
{"D SETPT", OME_FLOAT, NULL, &entryDSetpoint, 0},
{"D SETPT TRN", OME_FLOAT, NULL, &entryPSetpoint, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
//
// RC preview
//
static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0};
static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0};
static OSD_INT16_t entryRcThr = {&rcData[THROTTLE], 1, 2500, 0};
static OSD_INT16_t entryRcYaw = {&rcData[YAW], 1, 2500, 0};
static OSD_INT16_t entryRcAux1 = {&rcData[AUX1], 1, 2500, 0};
static OSD_INT16_t entryRcAux2 = {&rcData[AUX2], 1, 2500, 0};
static OSD_INT16_t entryRcAux3 = {&rcData[AUX3], 1, 2500, 0};
static OSD_INT16_t entryRcAux4 = {&rcData[AUX4], 1, 2500, 0};
OSD_Entry menuRc[] =
{
{"--- RC PREV ---", OME_Label, NULL, NULL, 0},
{"ROLL", OME_Poll_INT16, NULL, &entryRcRoll, 0},
{"PITCH", OME_Poll_INT16, NULL, &entryRcPitch, 0},
{"THR", OME_Poll_INT16, NULL, &entryRcThr, 0},
{"YAW", OME_Poll_INT16, NULL, &entryRcYaw, 0},
{"AUX1", OME_Poll_INT16, NULL, &entryRcAux1, 0},
{"AUX2", OME_Poll_INT16, NULL, &entryRcAux2, 0},
{"AUX3", OME_Poll_INT16, NULL, &entryRcAux3, 0},
{"AUX4", OME_Poll_INT16, NULL, &entryRcAux4, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
//
// Misc
//
OSD_UINT16_t entryMinThrottle = {&masterConfig.motorConfig.minthrottle, 1020, 1300, 10};
OSD_UINT8_t entryGyroSoftLpfHz = {&masterConfig.gyro_soft_lpf_hz, 0, 255, 1};
OSD_UINT16_t entryDtermLpf = {&masterConfig.profile[0].pidProfile.dterm_lpf_hz, 0, 500, 5};
OSD_UINT16_t entryYawLpf = {&masterConfig.profile[0].pidProfile.yaw_lpf_hz, 0, 500, 5};
OSD_UINT16_t entryYawPLimit = {&masterConfig.profile[0].pidProfile.yaw_p_limit, 100, 500, 5};
OSD_UINT8_t entryVbatScale = {&masterConfig.batteryConfig.vbatscale, 1, 250, 1};
OSD_UINT8_t entryVbatMaxCell = {&masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1};
OSD_Entry menuMisc[]=
{
{"--- MISC ---", OME_Label, NULL, NULL, 0},
{"GYRO LPF", OME_UINT8, NULL, &entryGyroSoftLpfHz, 0},
{"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf, 0},
{"YAW LPF", OME_UINT16, NULL, &entryYawLpf, 0},
{"YAW P LIM", OME_UINT16, NULL, &entryYawPLimit, 0},
{"MIN THR", OME_UINT16, NULL, &entryMinThrottle, 0},
{"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale, 0},
{"VBAT CLMAX", OME_UINT8, NULL, &entryVbatMaxCell, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
OSD_Entry menuImu[] =
{
{"--- CFG. IMU ---", OME_Label, NULL, NULL, 0},
{"PID PROF", OME_UINT8, NULL, &entryPidProfile, 0},
{"PID", OME_Submenu, cmsMenuChange, &menuPid[0], 0},
{"RATE&RXPO", OME_Submenu, cmsMenuChange, &menuRateExpo[0], 0},
{"RC PREV", OME_Submenu, cmsMenuChange, &menuRc[0], 0},
{"MISC", OME_Submenu, cmsMenuChange, &menuMisc[0], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
//
// Black box
//
//
// Should goto flashfs eventually.
//
#ifdef USE_FLASHFS
void cmsx_EraseFlash(displayPort_t *pDisplay, void *ptr)
{
UNUSED(ptr);
cmsScreenClear(pDisplay);
cmsScreenWrite(pDisplay, 5, 3, "ERASING FLASH...");
cmsScreenResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
flashfsEraseCompletely();
while (!flashfsIsReady()) {
delay(100);
}
cmsScreenClear(pDisplay);
cmsScreenResync(pDisplay); // Was max7456RefreshAll(); wedges during heavy SPI?
}
#endif // USE_FLASHFS
uint8_t featureBlackbox = 0;
OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1};
OSD_Entry menuBlackbox[] =
{
{"--- BLACKBOX ---", OME_Label, NULL, NULL, 0},
{"ENABLED", OME_Bool, NULL, &featureBlackbox, 0},
{"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom, 0},
#ifdef USE_FLASHFS
{"ERASE FLASH", OME_Submenu, cmsx_EraseFlash, NULL, 0},
#endif // USE_FLASHFS
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
//
// VTX
//
#if defined(VTX) || defined(USE_RTC6705)
uint8_t featureVtx = 0, vtxBand, vtxChannel;
static const char * const vtxBandNames[] = {
"BOSCAM A",
"BOSCAM B",
"BOSCAM E",
"FATSHARK",
"RACEBAND",
};
OSD_TAB_t entryVtxBand = {&vtxBand,4,&vtxBandNames[0]};
OSD_UINT8_t entryVtxChannel = {&vtxChannel, 1, 8, 1};
#ifdef VTX
OSD_UINT8_t entryVtxMode = {&masterConfig.vtx_mode, 0, 2, 1};
OSD_UINT16_t entryVtxMhz = {&masterConfig.vtx_mhz, 5600, 5950, 1};
#endif // VTX
OSD_Entry menu_vtx[] =
{
{"--- VTX ---", OME_Label, NULL, NULL, 0},
{"ENABLED", OME_Bool, NULL, &featureVtx, 0},
#ifdef VTX
{"VTX MODE", OME_UINT8, NULL, &entryVtxMode, 0},
{"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz, 0},
#endif // VTX
{"BAND", OME_TAB, NULL, &entryVtxBand, 0},
{"CHANNEL", OME_UINT8, NULL, &entryVtxChannel, 0},
#ifdef USE_RTC6705
{"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power, 0},
#endif // USE_RTC6705
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
#endif // VTX || USE_RTC6705
//
// LED_STRIP
//
#ifdef LED_STRIP
//local variable to keep color value
uint8_t ledColor;
static const char * const LED_COLOR_NAMES[] = {
"BLACK ",
"WHITE ",
"RED ",
"ORANGE ",
"YELLOW ",
"LIME GRN",
"GREEN ",
"MINT GRN",
"CYAN ",
"LT BLUE ",
"BLUE ",
"DK VIOLT",
"MAGENTA ",
"DEEP PNK"
};
//find first led with color flag and restore color index
//after saving all leds with flags color will have color set in OSD
static void getLedColor(void)
{
for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) {
const ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex];
int fn = ledGetFunction(ledConfig);
if (fn == LED_FUNCTION_COLOR) {
ledColor = ledGetColor(ledConfig);
break;
}
}
}
//udate all leds with flag color
static long applyLedColor(displayPort_t *pDisplay, void *ptr)
{
UNUSED(ptr);
UNUSED(pDisplay); // Arrgh
for (int ledIndex = 0; ledIndex < LED_MAX_STRIP_LENGTH; ledIndex++) {
ledConfig_t *ledConfig = &masterConfig.ledConfigs[ledIndex];
if (ledGetFunction(ledConfig) == LED_FUNCTION_COLOR)
*ledConfig = DEFINE_LED(ledGetX(ledConfig), ledGetY(ledConfig), ledColor, ledGetDirection(ledConfig), ledGetFunction(ledConfig), ledGetOverlay(ledConfig), 0);
}
return 0;
}
static uint8_t featureLedstrip;
OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]};
OSD_Entry menuLedstrip[] =
{
{"--- LED STRIP ---", OME_Label, NULL, NULL, 0},
{"ENABLED", OME_Bool, NULL, &featureLedstrip, 0},
{"LED COLOR", OME_TAB, applyLedColor, &entryLed, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
#endif // LED_STRIP
#ifdef OSD
//
// OSD specific menu pages and items
// XXX Should be part of the osd.c, or new osd_cms.c.
//
OSD_UINT8_t entryAlarmRssi = {&masterConfig.osdProfile.rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&masterConfig.osdProfile.cap_alarm, 50, 30000, 50};
OSD_UINT16_t enryAlarmFlyTime = {&masterConfig.osdProfile.time_alarm, 1, 200, 1};
OSD_UINT16_t entryAlarmAltitude = {&masterConfig.osdProfile.alt_alarm, 1, 200, 1};
OSD_Entry menuAlarms[] =
{
{"--- ALARMS ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_UINT8, NULL, &entryAlarmRssi, 0},
{"MAIN BAT", OME_UINT16, NULL, &entryAlarmCapacity, 0},
{"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime, 0},
{"MAX ALT", OME_UINT16, NULL, &entryAlarmAltitude, 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
OSD_Entry menuOsdActiveElems[] =
{
{"--- ACTIV ELEM ---", OME_Label, NULL, NULL, 0},
{"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE], 0},
{"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE], 0},
{"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON], 0},
{"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS], 0},
{"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME], 0},
{"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME], 0},
{"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE], 0},
{"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME], 0},
{"THROTTLE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS], 0},
#ifdef VTX
{"VTX CHAN", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]},
#endif // VTX
{"CURRENT (A)", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW], 0},
{"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN], 0},
#ifdef GPS
{"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED], 0},
{"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS], 0},
#endif // GPS
{"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
OSD_Entry menuOsdLayout[] =
{
{"---SCREEN LAYOUT---", OME_Label, NULL, NULL, 0},
{"ACTIVE ELEM.", OME_Submenu, cmsMenuChange, &menuOsdActiveElems[0], 0},
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
#endif // OSD
//
// Info
//
static char infoGitRev[GIT_SHORT_REVISION_LENGTH];
static char infoTargetName[] = __TARGET__;
#include "msp/msp_protocol.h" // XXX for FC identification... not available elsewhere
OSD_Entry menuInfo[] = {
{ "--- INFO ---", OME_Label, NULL, NULL, 0 },
{ BETAFLIGHT_IDENTIFIER, OME_Label, NULL, NULL, 0 },
{ FC_VERSION_STRING, OME_Label, NULL, NULL, 0 },
{ infoGitRev, OME_Label, NULL, NULL, 0 },
{ infoTargetName, OME_Label, NULL, NULL, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};
void cmsx_InfoInit(void)
{
int i;
for (i = 0 ; i < GIT_SHORT_REVISION_LENGTH ; i++) {
if (shortGitRevision[i] >= 'a' && shortGitRevision[i] <= 'f')
infoGitRev[i] = shortGitRevision[i] - 'a' + 'A';
else
infoGitRev[i] = shortGitRevision[i];
}
}
OSD_Entry menuFeatures[] =
{
{"--- FEATURES ---", OME_Label, NULL, NULL, 0},
{"BLACKBOX", OME_Submenu, cmsMenuChange, &menuBlackbox[0], 0},
#if defined(VTX) || defined(USE_RTC6705)
{"VTX", OME_Submenu, cmsMenuChange, &menu_vtx[0], 0},
#endif // VTX || USE_RTC6705
#ifdef LED_STRIP
{"LED STRIP", OME_Submenu, cmsMenuChange, &menuLedstrip[0], 0},
#endif // LED_STRIP
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
};
OSD_Entry menuMain[] =
{
{"--- MAIN MENU ---", OME_Label, NULL, NULL, 0},
{"CFG&IMU", OME_Submenu, cmsMenuChange, &menuImu[0], 0},
{"FEATURES", OME_Submenu, cmsMenuChange, &menuFeatures[0], 0},
#ifdef OSD
{"SCR LAYOUT", OME_Submenu, cmsMenuChange, &menuOsdLayout[0], 0},
{"ALARMS", OME_Submenu, cmsMenuChange, &menuAlarms[0], 0},
#endif
{"INFO", OME_Submenu, cmsMenuChange, &menuInfo[0], 0},
{"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void*)1, 0},
{"EXIT", OME_OSD_Exit, cmsMenuExit, (void*)0, 0},
{NULL,OME_END, NULL, NULL, 0}
};
void cmsx_FeatureRead(void)
{
featureBlackbox = feature(FEATURE_BLACKBOX) ? 1 : 0;
#ifdef LED_STRIP
featureLedstrip = feature(FEATURE_LED_STRIP) ? 1 : 0;
getLedColor();
#endif
#if defined(VTX) || defined(USE_RTC6705)
featureVtx = feature(FEATURE_VTX) ? 1 : 0;
#endif // VTX || USE_RTC6705
#ifdef VTX
vtxBand = masterConfig.vtxBand;
vtxChannel = masterConfig.vtx_channel + 1;
#endif // VTX
#ifdef USE_RTC6705
vtxBand = masterConfig.vtx_channel / 8;
vtxChannel = masterConfig.vtx_channel % 8 + 1;
#endif // USE_RTC6705
}
void cmsx_FeatureWriteback(void)
{
if (featureBlackbox)
featureSet(FEATURE_BLACKBOX);
else
featureClear(FEATURE_BLACKBOX);
#ifdef LED_STRIP
if (featureLedstrip)
featureSet(FEATURE_LED_STRIP);
else
featureClear(FEATURE_LED_STRIP);
#endif
#if defined(VTX) || defined(USE_RTC6705)
if (featureVtx)
featureSet(FEATURE_VTX);
else
featureClear(FEATURE_VTX);
#endif // VTX || USE_RTC6705
#ifdef VTX
masterConfig.vtxBand = vtxBand;
masterConfig.vtx_channel = vtxChannel - 1;
#endif // VTX
#ifdef USE_RTC6705
masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1;
#endif // USE_RTC6705
saveConfigAndNotify();
}
#endif // CMS
Reference in a new issue View git blame Copy permalink