diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 459df49111..c58c3d96b2 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -38,6 +38,7 @@ #include "io/motors.h" #include "io/servos.h" #include "io/gps.h" +#include "io/cms.h" #include "io/osd.h" #include "io/ledstrip.h" #include "io/vtx.h" diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1db353f731..7d5272733c 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -63,6 +63,7 @@ #include "io/servos.h" #include "io/ledstrip.h" #include "io/gps.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 1432b6882b..60c2f517c6 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,6 +55,7 @@ typedef enum { FEATURE_VTX = 1 << 24, FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, + FEATURE_CANVAS = 1 << 27, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8fe284997f..3119e27bc9 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -62,9 +62,7 @@ #include "io/flashfs.h" #include "io/transponder_ir.h" #include "io/asyncfatfs/asyncfatfs.h" -#include "io/osd.h" #include "io/serial_4way.h" -#include "io/vtx.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -1780,3 +1778,17 @@ void mspFcInit(void) { initActiveBoxIds(); } + +void mspServerPush(mspPacket_t *push, uint8_t *data, int len) +{ + sbuf_t *dst = &push->buf; + + while (len--) { + sbufWriteU8(dst, *data++); + } +} + +mspPushCommandFnPtr mspFcPushInit(void) +{ + return mspServerPush; +} diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h index 53edbd8ed1..df317ab3a1 100644 --- a/src/main/fc/fc_msp.h +++ b/src/main/fc/fc_msp.h @@ -21,3 +21,5 @@ void mspFcInit(void); mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); + +mspPushCommandFnPtr mspFcPushInit(void); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 43b308ab23..b5cc16f7e5 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -40,6 +40,8 @@ #include "flight/pid.h" #include "flight/altitudehold.h" +#include "io/cms.h" + #include "io/beeper.h" #include "io/display.h" #include "io/gps.h" @@ -48,6 +50,7 @@ #include "io/serial.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" +#include "io/vtx_smartaudio.h" #include "msp/msp_serial.h" @@ -342,6 +345,10 @@ void fcTasksInit(void) #ifdef USE_BST setTaskEnabled(TASK_BST_MASTER_PROCESS, true); #endif +#ifdef CMS + // XXX Should check FEATURE + setTaskEnabled(TASK_CMS, true); +#endif #ifdef VTX_CONTROL setTaskEnabled(TASK_VTXCTRL, true); #endif @@ -505,6 +512,15 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#ifdef CMS + [TASK_CMS] = { + .taskName = "CMS", + .taskFunc = cmsHandler, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + #ifdef VTX_CONTROL [TASK_VTXCTRL] = { .taskName = "VTXCTRL", diff --git a/src/main/fc/fc_tasks.c.orig b/src/main/fc/fc_tasks.c.orig new file mode 100644 index 0000000000..46d96a8909 --- /dev/null +++ b/src/main/fc/fc_tasks.c.orig @@ -0,0 +1,534 @@ +/* + * 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 . + */ + +#include +#include +#include + +#include + +#include "common/axis.h" +#include "common/color.h" +#include "common/utils.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/compass.h" +#include "drivers/serial.h" + +#include "fc/config.h" +#include "fc/fc_msp.h" +#include "fc/fc_tasks.h" +#include "fc/mw.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/pid.h" +#include "flight/altitudehold.h" + +#include "io/cms.h" + +#include "io/beeper.h" +#include "io/display.h" +#include "io/gps.h" +#include "io/ledstrip.h" +#include "io/cms.h" +#include "io/osd.h" +#include "io/serial.h" +#include "io/serial_cli.h" +#include "io/transponder_ir.h" + +#include "msp/msp_serial.h" + +#include "rx/rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sonar.h" + +#include "scheduler/scheduler.h" + +#include "telemetry/telemetry.h" + +#include "config/feature.h" +#include "config/config_profile.h" +#include "config/config_master.h" + + +/* VBAT monitoring interval (in microseconds) - 1s*/ +#define VBATINTERVAL (6 * 3500) +/* IBat monitoring interval (in microseconds) - 6 default looptimes */ +#define IBATINTERVAL (6 * 3500) + + +static void taskUpdateAccelerometer(uint32_t currentTime) +{ + UNUSED(currentTime); + + imuUpdateAccelerometer(&masterConfig.accelerometerTrims); +} + +static void taskUpdateAttitude(uint32_t currentTime) +{ + imuUpdateAttitude(currentTime); +} + +static void taskHandleSerial(uint32_t currentTime) +{ + UNUSED(currentTime); +#ifdef USE_CLI + // in cli mode, all serial stuff goes to here. enter cli mode by sending # + if (cliMode) { + cliProcess(); + return; + } +#endif + mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand); +} + +#ifdef BEEPER +static void taskUpdateBeeper(uint32_t currentTime) +{ + beeperUpdate(currentTime); //call periodic beeper handler +} +#endif + +static void taskUpdateBattery(uint32_t currentTime) +{ +#ifdef USE_ADC + static uint32_t vbatLastServiced = 0; + if (feature(FEATURE_VBAT)) { + if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { + vbatLastServiced = currentTime; + updateBattery(); + } + } +#endif + + static uint32_t ibatLastServiced = 0; + if (feature(FEATURE_CURRENT_METER)) { + const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); + + if (ibatTimeSinceLastServiced >= IBATINTERVAL) { + ibatLastServiced = currentTime; + updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + } + } +} + +static bool taskUpdateRxCheck(uint32_t currentTime, uint32_t currentDeltaTime) +{ + UNUSED(currentDeltaTime); + return rxUpdate(currentTime); +} + +static void taskUpdateRxMain(uint32_t currentTime) +{ + processRx(currentTime); + isRXDataNew = true; + +#if !defined(BARO) && !defined(SONAR) + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); +#endif + updateLEDs(); + +#ifdef BARO + if (sensors(SENSOR_BARO)) { + updateAltHoldState(); + } +#endif + +#ifdef SONAR + if (sensors(SENSOR_SONAR)) { + updateSonarAltHoldState(); + } +#endif +} + +#ifdef GPS +static void taskProcessGPS(uint32_t currentTime) +{ + // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck + // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsUdate() can and will + // change this based on available hardware + if (feature(FEATURE_GPS)) { + gpsUpdate(currentTime); + } +} +#endif + +#ifdef MAG +static void taskUpdateCompass(uint32_t currentTime) +{ + if (sensors(SENSOR_MAG)) { + compassUpdate(currentTime, &masterConfig.magZero); + } +} +#endif + +#ifdef BARO +static void taskUpdateBaro(uint32_t currentTime) +{ + UNUSED(currentTime); + + if (sensors(SENSOR_BARO)) { + const uint32_t newDeadline = baroUpdate(); + if (newDeadline != 0) { + rescheduleTask(TASK_SELF, newDeadline); + } + } +} +#endif + +#ifdef SONAR +static void taskUpdateSonar(uint32_t currentTime) +{ + UNUSED(currentTime); + + if (sensors(SENSOR_SONAR)) { + sonarUpdate(); + } +} +#endif + +#if defined(BARO) || defined(SONAR) +static void taskCalculateAltitude(uint32_t currentTime) +{ + if (false +#if defined(BARO) + || (sensors(SENSOR_BARO) && isBaroReady()) +#endif +#if defined(SONAR) + || sensors(SENSOR_SONAR) +#endif + ) { + calculateEstimatedAltitude(currentTime); + }} +#endif + +#ifdef DISPLAY +static void taskUpdateDisplay(uint32_t currentTime) +{ + if (feature(FEATURE_DISPLAY)) { + displayUpdate(currentTime); + } +} +#endif + +#ifdef TELEMETRY +static void taskTelemetry(uint32_t currentTime) +{ + telemetryCheckState(); + + if (!cliMode && feature(FEATURE_TELEMETRY)) { + telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); + } +} +#endif + +#ifdef LED_STRIP +static void taskLedStrip(uint32_t currentTime) +{ + if (feature(FEATURE_LED_STRIP)) { + ledStripUpdate(currentTime); + } +} +#endif + +#ifdef TRANSPONDER +static void taskTransponder(uint32_t currentTime) +{ + if (feature(FEATURE_TRANSPONDER)) { + transponderUpdate(currentTime); + } +} +#endif + +#ifdef OSD +static void taskUpdateOsd(uint32_t currentTime) +{ + if (feature(FEATURE_OSD)) { + updateOsd(currentTime); + } +} +#endif + +#ifdef VTX_CONTROL +// Everything that listens to VTX devices +void taskVtxControl(uint32_t currentTime) +{ + if (ARMING_FLAG(ARMED)) + return; + +#ifdef VTX_SMARTAUDIO + smartAudioProcess(currentTime); +#endif +} +#endif + +void fcTasksInit(void) +{ + schedulerInit(); + rescheduleTask(TASK_GYROPID, gyro.targetLooptime); + setTaskEnabled(TASK_GYROPID, true); + + if (sensors(SENSOR_ACC)) { + setTaskEnabled(TASK_ACCEL, true); + rescheduleTask(TASK_ACCEL, accSamplingInterval); + } + + setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); + setTaskEnabled(TASK_SERIAL, true); + setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); + setTaskEnabled(TASK_RX, true); + +#ifdef BEEPER + setTaskEnabled(TASK_BEEPER, true); +#endif +#ifdef GPS + setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); +#endif +#ifdef MAG + setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); +#if defined(USE_SPI) && defined(USE_MAG_AK8963) + // fixme temporary solution for AK6983 via slave I2C on MPU9250 + rescheduleTask(TASK_COMPASS, 1000000 / 40); +#endif +#endif +#ifdef BARO + setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); +#endif +#ifdef SONAR + setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); +#endif +#if defined(BARO) || defined(SONAR) + setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); +#endif +#ifdef DISPLAY + setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); +#endif +#ifdef TELEMETRY + setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); + // Reschedule telemetry to 500hz for Jeti Exbus + if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) { + rescheduleTask(TASK_TELEMETRY, 2000); + } +#endif +#ifdef LED_STRIP + setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); +#endif +#ifdef TRANSPONDER + setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); +#endif +#ifdef OSD + setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); +#endif +#ifdef USE_BST + setTaskEnabled(TASK_BST_MASTER_PROCESS, true); +#endif +<<<<<<< HEAD +#ifdef VTX_CONTROL + setTaskEnabled(TASK_VTXCTRL, true); +======= +#ifdef CMS + // XXX Should check FEATURE + setTaskEnabled(TASK_CMS, true); +>>>>>>> bfdev-osd-cms-separation-poc +#endif +} + +cfTask_t cfTasks[TASK_COUNT] = { + [TASK_SYSTEM] = { + .taskName = "SYSTEM", + .taskFunc = taskSystem, + .desiredPeriod = 1000000 / 10, // run every 100 ms + .staticPriority = TASK_PRIORITY_HIGH, + }, + + [TASK_GYROPID] = { + .taskName = "PID", + .subTaskName = "GYRO", + .taskFunc = taskMainPidLoopCheck, + .desiredPeriod = TASK_GYROPID_DESIRED_PERIOD, + .staticPriority = TASK_PRIORITY_REALTIME, + }, + + [TASK_ACCEL] = { + .taskName = "ACCEL", + .taskFunc = taskUpdateAccelerometer, + .desiredPeriod = 1000000 / 1000, // every 1ms + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_ATTITUDE] = { + .taskName = "ATTITUDE", + .taskFunc = taskUpdateAttitude, + .desiredPeriod = 1000000 / 100, + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + + [TASK_RX] = { + .taskName = "RX", + .checkFunc = taskUpdateRxCheck, + .taskFunc = taskUpdateRxMain, + .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling + .staticPriority = TASK_PRIORITY_HIGH, + }, + + [TASK_SERIAL] = { + .taskName = "SERIAL", + .taskFunc = taskHandleSerial, + .desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud + .staticPriority = TASK_PRIORITY_LOW, + }, + + [TASK_BATTERY] = { + .taskName = "BATTERY", + .taskFunc = taskUpdateBattery, + .desiredPeriod = 1000000 / 50, // 50 Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, + +#ifdef BEEPER + [TASK_BEEPER] = { + .taskName = "BEEPER", + .taskFunc = taskUpdateBeeper, + .desiredPeriod = 1000000 / 100, // 100 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef GPS + [TASK_GPS] = { + .taskName = "GPS", + .taskFunc = taskProcessGPS, + .desiredPeriod = 1000000 / 10, // GPS usually don't go raster than 10Hz + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + +#ifdef MAG + [TASK_COMPASS] = { + .taskName = "COMPASS", + .taskFunc = taskUpdateCompass, + .desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef BARO + [TASK_BARO] = { + .taskName = "BARO", + .taskFunc = taskUpdateBaro, + .desiredPeriod = 1000000 / 20, + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef SONAR + [TASK_SONAR] = { + .taskName = "SONAR", + .taskFunc = taskUpdateSonar, + .desiredPeriod = 1000000 / 20, + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#if defined(BARO) || defined(SONAR) + [TASK_ALTITUDE] = { + .taskName = "ALTITUDE", + .taskFunc = taskCalculateAltitude, + .desiredPeriod = 1000000 / 40, + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef TRANSPONDER + [TASK_TRANSPONDER] = { + .taskName = "TRANSPONDER", + .taskFunc = taskTransponder, + .desiredPeriod = 1000000 / 250, // 250 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef DISPLAY + [TASK_DISPLAY] = { + .taskName = "DISPLAY", + .taskFunc = taskUpdateDisplay, + .desiredPeriod = 1000000 / 10, + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif +#ifdef OSD + [TASK_OSD] = { + .taskName = "OSD", + .taskFunc = taskUpdateOsd, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif +#ifdef TELEMETRY + [TASK_TELEMETRY] = { + .taskName = "TELEMETRY", + .taskFunc = taskTelemetry, + .desiredPeriod = 1000000 / 250, // 250 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef LED_STRIP + [TASK_LEDSTRIP] = { + .taskName = "LEDSTRIP", + .taskFunc = taskLedStrip, + .desiredPeriod = 1000000 / 100, // 100 Hz + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif + +#ifdef USE_BST + [TASK_BST_MASTER_PROCESS] = { + .taskName = "BST_MASTER_PROCESS", + .taskFunc = taskBstMasterProcess, + .desiredPeriod = 1000000 / 50, // 50 Hz + .staticPriority = TASK_PRIORITY_IDLE, + }, +#endif + +<<<<<<< HEAD +#ifdef VTX_CONTROL + [TASK_VTXCTRL] = { + .taskName = "VTXCTRL", + .taskFunc = taskVtxControl, + .desiredPeriod = 1000000 / 5, // 5Hz @200msec + .staticPriority = TASK_PRIORITY_IDLE, +======= +#ifdef CMS + [TASK_CMS] = { + .taskName = "CMS", + .taskFunc = cmsHandler, + .desiredPeriod = 1000000 / 60, // 60 Hz + .staticPriority = TASK_PRIORITY_LOW, +>>>>>>> bfdev-osd-cms-separation-poc + }, +#endif +}; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 316d5178b2..1d410a9435 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -40,6 +40,8 @@ #include "fc/rc_curves.h" #include "fc/runtime_config.h" +#include "io/cms.h" + #include "io/gps.h" #include "io/beeper.h" #include "io/motors.h" diff --git a/src/main/io/canvas.c b/src/main/io/canvas.c new file mode 100644 index 0000000000..97b12f03d9 --- /dev/null +++ b/src/main/io/canvas.c @@ -0,0 +1,94 @@ +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef CANVAS + +#include "drivers/system.h" + +#include "io/cms.h" + +#include "fc/fc_msp.h" +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +int canvasOutput(uint8_t cmd, uint8_t *buf, int len) +{ + mspSerialPush(cmd, buf, len); + + return 6 + len; +} + +int canvasBegin(void) +{ + uint8_t subcmd[] = { 0 }; + + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +int canvasHeartBeat(void) +{ + return canvasBegin(); +} + +int canvasEnd(void) +{ + uint8_t subcmd[] = { 1 }; + + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +int canvasClear(void) +{ + uint8_t subcmd[] = { 2 }; + + return canvasOutput(MSP_CANVAS, subcmd, sizeof(subcmd)); +} + +int canvasWrite(uint8_t col, uint8_t row, char *string) +{ + int len; + char buf[30 + 4]; + + if ((len = strlen(string)) >= 30) + len = 30; + + buf[0] = 3; + buf[1] = row; + buf[2] = col; + buf[3] = 0; + memcpy((char *)&buf[4], string, len); + + return canvasOutput(MSP_CANVAS, (uint8_t *)buf, len + 4); +} + +screenFnVTable_t canvasVTable = { + canvasBegin, + canvasEnd, + canvasClear, + canvasWrite, + canvasHeartBeat, + NULL, +}; + +void canvasCmsInit(displayPort_t *pPort) +{ + pPort->rows = 13; + pPort->cols = 30; + pPort->buftime = 23; // = 256/(115200/10) + pPort->bufsize = 192; // 256 * 3/4 (Be conservative) + pPort->VTable = &canvasVTable; +} + +void canvasInit() +{ + mspSerialPushInit(mspFcPushInit()); // Called once at startup to initialize push function in msp + + cmsDeviceRegister(canvasCmsInit); +} +#endif diff --git a/src/main/io/canvas.h b/src/main/io/canvas.h new file mode 100644 index 0000000000..c7f96207ca --- /dev/null +++ b/src/main/io/canvas.h @@ -0,0 +1,3 @@ +#pragma once +void canvasInit(void); +void canvasCmsInit(displayPort_t *); diff --git a/src/main/io/cms.c b/src/main/io/cms.c new file mode 100644 index 0000000000..76e01a76b4 --- /dev/null +++ b/src/main/io/cms.c @@ -0,0 +1,1445 @@ +/* + * 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 . + */ + +/* + Created by Marcin Baliniak + OSD-CMS separation by jflyper + */ + +#include +#include +#include +#include + +#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(¤tDisplay); + initfunc = cmsDeviceSelectNext(); + } + + if (!initfunc) + return; + + cmsScreenInit(¤tDisplay, initfunc); + cmsComputeBatchsize(¤tDisplay); + cmsScreenBegin(¤tDisplay); + cmsMenuChange(¤tDisplay, 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(¤tDisplay, key); + return; + } + + cmsDrawMenu(pDisplay, currentTime); + + if (currentTime > lastCmsHeartBeat + 500) { + // Heart beat for external CMS display device @ 500msec + // (Timeout @ 1000msec) + cmsScreenHeartBeat(¤tDisplay); + 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(¤tDisplay, 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 + +#ifdef VTX_SMARTAUDIO +#include "io/vtx_smartaudio.h" + +static const char * const smartAudioBandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; +OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; + +static const char * const smartAudioChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; + +static const char * const smartAudioPowerNames[] = { + "---", + " 25", + "200", + "500", + "800", +}; + +OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; + +static const char * const smartAudioTxModeNames[] = { + "------", + "PIT-OR", + "PIT-IR", + "ACTIVE", +}; + +OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; + +OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; + +static const char * const smartAudioOpModelNames[] = { + "FREE", + "PIT", +}; + +OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; + +static const char * const smartAudioPitFModeNames[] = { + "IN-RANGE", + "OUT-RANGE", +}; + +OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; + +OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; + +OSD_Entry menu_smartAudioConfig[] = { + { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL, 0 }, + { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel, 0 }, + { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode, 0 }, + { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq, 0 }, // OME_Poll_UINT16 + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +static const char * const smartAudioStatusNames[] = { + "OFFLINE", + "ONLINE V1", + "ONLINE V2", +}; + +OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; +OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; + +OSD_Entry menu_smartAudioStats[] = { + { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL, 0 }, + { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline, 0 }, + { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate, 0 }, + { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre, 0 }, + { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen, 0 }, + { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr, 0 }, + { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +OSD_Entry menu_vtxSmartAudio[] = +{ + { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL, 0 }, + { smartAudioStatusString, OME_Label, NULL, NULL, 0 }, + { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode, 0 }, + { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand, 0 }, + { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan, 0 }, + { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq, 0 }, + { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower, 0 }, + { "CONFIG", OME_Submenu, cmsMenuChange, &menu_smartAudioConfig[0], 0 }, + { "STAT", OME_Submenu, cmsMenuChange, &menu_smartAudioStats[0], 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; +#endif // VTX_SMARTAUDIO + +// +// 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 +#if defined(VTX_SMARTAUDIO) + {"VTX", OME_Submenu, cmsMenuChange, &menu_vtxSmartAudio[0], 0}, +#endif + +#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 diff --git a/src/main/io/cms.h b/src/main/io/cms.h new file mode 100644 index 0000000000..a9ab352cd3 --- /dev/null +++ b/src/main/io/cms.h @@ -0,0 +1,40 @@ +#pragma once + +typedef struct screenFnVTable_s { + int (*begin)(void); + int (*end)(void); + int (*clear)(void); + int (*write)(uint8_t, uint8_t, char *); + int (*heartbeat)(void); + void (*resync)(void); +} screenFnVTable_t; + +typedef struct displayPort_s { + uint8_t rows; + uint8_t cols; + uint16_t buftime; + uint16_t bufsize; + uint16_t batchsize; // Computed by CMS + screenFnVTable_t *VTable; + + // CMS state + bool cleared; +} displayPort_t; + +// Device management +typedef void (*cmsDeviceInitFuncPtr)(displayPort_t *); +bool cmsDeviceRegister(cmsDeviceInitFuncPtr); + +// For main.c and scheduler +void cmsInit(void); +void cmsHandler(uint32_t); + +// Required for external CMS tables + +long cmsChangeScreen(displayPort_t *, void *); +long cmsExitMenu(displayPort_t *, void *); + +#define STARTUP_HELP_TEXT1 "MENU: THR MID" +#define STARTUP_HELP_TEXT2 "+ YAW LEFT" +#define STARTUP_HELP_TEXT3 "+ PITCH UP" + diff --git a/src/main/io/cms_types.h b/src/main/io/cms_types.h new file mode 100644 index 0000000000..df68379ebf --- /dev/null +++ b/src/main/io/cms_types.h @@ -0,0 +1,97 @@ +// +// Menu element types +// XXX Upon separation, all OME would be renamed to CME_ or similar. +// + +#pragma once + +typedef long (*OSDMenuFuncPtr)(displayPort_t *, void *); + +//type of elements +typedef enum +{ + OME_Label, + OME_Back, + OME_OSD_Exit, + OME_Submenu, + OME_Bool, + OME_INT8, + OME_UINT8, + OME_UINT16, + OME_INT16, + OME_Poll_INT16, + OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's + //wlasciwosci elementow + OME_VISIBLE, + OME_TAB, + OME_END, +} OSD_MenuElement; + +typedef struct +{ + char *text; + OSD_MenuElement type; + OSDMenuFuncPtr func; + void *data; + uint8_t flags; +} OSD_Entry; + +// Bits in flags +#define PRINT_VALUE 0x01 // Value has been changed, need to redraw +#define PRINT_LABEL 0x02 // Text label should be printed + +#define IS_PRINTVALUE(p) ((p)->flags & PRINT_VALUE) +#define SET_PRINTVALUE(p) { (p)->flags |= PRINT_VALUE; } +#define CLR_PRINTVALUE(p) { (p)->flags &= ~PRINT_VALUE; } + +#define IS_PRINTLABEL(p) ((p)->flags & PRINT_LABEL) +#define SET_PRINTLABEL(p) { (p)->flags |= PRINT_LABEL; } +#define CLR_PRINTLABEL(p) { (p)->flags &= ~PRINT_LABEL; } + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; +} OSD_UINT8_t; + +typedef struct +{ + int8_t *val; + int8_t min; + int8_t max; + int8_t step; +} OSD_INT8_t; + +typedef struct +{ + int16_t *val; + int16_t min; + int16_t max; + int16_t step; +} OSD_INT16_t; + +typedef struct +{ + uint16_t *val; + uint16_t min; + uint16_t max; + uint16_t step; +} OSD_UINT16_t; + +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; + uint16_t multipler; +} OSD_FLOAT_t; + +typedef struct +{ + uint8_t *val; + uint8_t max; + const char * const *names; +} OSD_TAB_t; diff --git a/src/main/io/display.c b/src/main/io/display.c index 1d7c88199a..d8b5a8b82e 100644 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -50,6 +50,8 @@ #include "flight/imu.h" #include "flight/failsafe.h" +#include "io/cms.h" + #ifdef GPS #include "io/gps.h" #include "flight/navigation.h" @@ -577,10 +579,19 @@ void showDebugPage(void) } #endif +#ifdef OLEDCMS +static bool displayInCMS = false; +#endif + void displayUpdate(uint32_t currentTime) { static uint8_t previousArmedState = 0; +#ifdef OLEDCMS + if (displayInCMS) + return; +#endif + const bool updateNow = (int32_t)(currentTime - nextDisplayUpdateAt) >= 0L; if (!updateNow) { return; @@ -692,6 +703,10 @@ void displayInit(rxConfig_t *rxConfigToUse) resetDisplay(); delay(200); +#if defined(CMS) && defined(OLEDCMS) + cmsDeviceRegister(displayCmsInit); +#endif + rxConfig = rxConfigToUse; memset(&pageState, 0, sizeof(pageState)); @@ -729,4 +744,55 @@ void displayDisablePageCycling(void) pageState.pageFlags &= ~PAGE_STATE_FLAG_CYCLE_ENABLED; } +#ifdef OLEDCMS +#include "io/cms.h" + +int displayCmsBegin(void) +{ + displayInCMS = true; + + return 0; +} + +int displayCmsEnd(void) +{ + displayInCMS = false; + + return 0; +} + +int displayCmsClear(void) +{ + i2c_OLED_clear_display_quick(); + + return 0; +} + +int displayCmsWrite(uint8_t x, uint8_t y, char *s) +{ + i2c_OLED_set_xy(x, y); + i2c_OLED_send_string(s); + + return 0; +} + +screenFnVTable_t displayCmsVTable = { + displayCmsBegin, + displayCmsEnd, + displayCmsClear, + displayCmsWrite, + NULL, + NULL, +}; + +void displayCmsInit(displayPort_t *pPort) +{ + pPort->rows = 8; + pPort->cols = 21; + pPort->buftime = 1; + pPort->bufsize = 50000; + pPort->VTable = &displayCmsVTable; +} +#endif // OLEDCMS + #endif diff --git a/src/main/io/display.h b/src/main/io/display.h index 75abffbe3c..e2c6c2b967 100644 --- a/src/main/io/display.h +++ b/src/main/io/display.h @@ -45,3 +45,7 @@ void displayEnablePageCycling(void); void displayDisablePageCycling(void); void displayResetPageCycling(void); void displaySetNextPageChangeAt(uint32_t futureMicros); + +#ifdef CMS +void displayCmsInit(displayPort_t *); +#endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index f8e9d880f0..07d8a33f8b 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -38,6 +38,8 @@ #include "sensors/sensors.h" +#include "io/cms.h" + #include "io/serial.h" #include "io/display.h" #include "io/gps.h" diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index baeb80c957..0ed87081d2 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -62,8 +62,6 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/gps.h" -#include "io/osd.h" -#include "io/vtx.h" #include "flight/failsafe.h" #include "flight/mixer.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3a9a25b6ef..c67c07ce64 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -18,6 +18,8 @@ /* Created by Marcin Baliniak some functions based on MinimOSD + + OSD-CMS separation by jflyper */ #include @@ -27,14 +29,17 @@ #include "platform.h" -#ifdef OSD - #include "build/version.h" +#ifdef OSD + #include "common/utils.h" #include "drivers/system.h" +#include "io/cms.h" +#include "io/cms_types.h" + #include "io/flashfs.h" #include "io/osd.h" @@ -42,7 +47,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "flight/pid.h" +//#include "flight/pid.h" #include "config/config_profile.h" #include "config/config_master.h" @@ -55,1500 +60,58 @@ #include "drivers/max7456.h" #include "drivers/max7456_symbols.h" -#ifdef USE_RTC6705 -#include "drivers/vtx_soft_spi_rtc6705.h" -#endif - -#ifdef VTX_SMARTAUDIO -#include "io/vtx_smartaudio.h" -#endif - #include "common/printf.h" +#include "build/debug.h" + +// Things in both OSD and CMS + #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 - -//osd current screen - to reduce long lines ;-) -#define OSD_cfg masterConfig.osdProfile -#define curr_profile masterConfig.profile[masterConfig.current_profile_index] - -uint16_t refreshTimeout = 0; - -#define VISIBLE_FLAG 0x0800 -#define BLINK_FLAG 0x0400 bool blinkState = true; -#define OSD_POS(x,y) (x | (y << 5)) -#define OSD_X(x) (x & 0x001F) -#define OSD_Y(x) ((x >> 5) & 0x001F) -#define VISIBLE(x) (x & VISIBLE_FLAG) -#define BLINK(x) ((x & BLINK_FLAG) && blinkState) -#define BLINK_OFF(x) (x & ~BLINK_FLAG) - -extern uint8_t RSSI; // TODO: not used? +//extern uint8_t RSSI; // TODO: not used? static uint16_t flyTime = 0; uint8_t statRssi; statistic_t stats; -#define BUTTON_TIME 2 -#define BUTTON_PAUSE 5 +uint16_t refreshTimeout = 0; #define REFRESH_1S 12 -#define LEFT_MENU_COLUMN 1 -#define RIGHT_MENU_COLUMN 23 -#define MAX_MENU_ITEMS (max7456GetRowsCount() - 2) - -uint8_t osdRows; - -//type of elements -typedef enum -{ - OME_Label, - OME_Back, - OME_OSD_Exit, - OME_Submenu, - OME_Bool, - OME_INT8, - OME_UINT8, - OME_UINT16, - OME_INT16, - OME_FLOAT, //only up to 255 value and cant be 2.55 or 25.5, just for PID's - //wlasciwosci elementow - OME_VISIBLE, - OME_POS, - OME_TAB, - OME_END, -} OSD_MenuElement; - -//local variable to detect arm/disarm and show statistic etc uint8_t armState; -uint8_t featureBlackbox = 0; -uint8_t featureLedstrip = 0; -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) -uint8_t featureVtx = 0, vtxBand, vtxChannel; -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO - -// We are in menu flag -bool inMenu = false; - -typedef void (* OSDMenuFuncPtr)(void *data); +// OSD forwards void osdUpdate(uint32_t currentTime); char osdGetAltitudeSymbol(); int32_t osdGetAltitude(int32_t alt); -void osdOpenMenu(void); -void osdExitMenu(void * ptr); -void osdMenuBack(void); void osdEditElement(void *ptr); -void osdEraseFlash(void *ptr); -void osdUpdateMaxRows(void); -void osdChangeScreen(void * ptr); void osdDrawElements(void); void osdDrawSingleElement(uint8_t item); -typedef struct -{ - char *text; - OSD_MenuElement type; - OSDMenuFuncPtr func; - void *data; -} OSD_Entry; +bool osdInMenu = false; -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; -} OSD_UINT8_t; - -typedef struct -{ - int8_t *val; - int8_t min; - int8_t max; - int8_t step; -} OSD_INT8_t; - -typedef struct -{ - int16_t *val; - int16_t min; - int16_t max; - int16_t step; -} OSD_INT16_t; - -typedef struct -{ - uint16_t *val; - uint16_t min; - uint16_t max; - uint16_t step; -} OSD_UINT16_t; - -typedef struct -{ - uint8_t *val; - uint8_t min; - uint8_t max; - uint8_t step; - uint16_t multipler; -} OSD_FLOAT_t; - -typedef struct -{ - uint8_t *val; - uint8_t max; - const char * const *names; -} OSD_TAB_t; - -OSD_Entry *menuStack[10]; //tab to save menu stack -uint8_t menuStackHistory[10]; //current position in menu stack -uint8_t menuStackIdx = 0; - -OSD_Entry *currentMenu; -OSD_Entry *nextPage = NULL; - -int8_t currentMenuPos = 0; -uint8_t currentMenuIdx = 0; -uint16_t *currentElement = NULL; - -OSD_Entry menuAlarms[]; -OSD_Entry menuOsdLayout[]; -OSD_Entry menuOsdActiveElems[]; -OSD_Entry menuOsdElemsPositions[]; -OSD_Entry menuFeatures[]; -OSD_Entry menuBlackbox[]; -#ifdef LED_STRIP -OSD_Entry menuLedstrip[]; -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) -OSD_Entry menu_vtx[]; -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO -OSD_Entry menuImu[]; -OSD_Entry menuPid[]; -OSD_Entry menuRc[]; -OSD_Entry menuRateExpo[]; -OSD_Entry menuMisc[]; - -OSD_Entry menuMain[] = -{ - {"----MAIN MENU----", OME_Label, NULL, NULL}, - {"SCREEN LAYOUT", OME_Submenu, osdChangeScreen, &menuOsdLayout[0]}, - {"ALARMS", OME_Submenu, osdChangeScreen, &menuAlarms[0]}, - {"CFG. IMU", OME_Submenu, osdChangeScreen, &menuImu[0]}, - {"FEATURES", OME_Submenu, osdChangeScreen, &menuFeatures[0]}, - {"SAVE & EXIT", OME_OSD_Exit, osdExitMenu, (void*)1}, - {"EXIT", OME_OSD_Exit, osdExitMenu, (void*)0}, - {NULL,OME_END, NULL, NULL} -}; - -OSD_Entry menuFeatures[] = -{ - {"----- FEATURES -----", OME_Label, NULL, NULL}, - {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, -#ifdef LED_STRIP - {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, -#endif // LED_STRIP -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) - {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryBlackboxRateDenom = {&masterConfig.blackbox_rate_denom,1,32,1}; - -OSD_Entry menuBlackbox[] = -{ - {"--- BLACKBOX ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureBlackbox}, - {"RATE DENOM", OME_UINT8, NULL, &entryBlackboxRateDenom}, -#ifdef USE_FLASHFS - {"ERASE FLASH", OME_Submenu, osdEraseFlash, NULL}, -#endif // USE_FLASHFS - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -#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 GREEN", - " GREEN ", - " MINT GREEN", - " CYAN ", - " LIGHT BLUE", - " BLUE ", - "DARK VIOLET", - " MAGENTA ", - " DEEP PINK" -}; - -//find first led with color flag and restore color index -//after saving all leds with flags color will have color set in OSD -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 -void applyLedColor(void * ptr) -{ - UNUSED(ptr); - 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); - } -} - -OSD_TAB_t entryLed = {&ledColor, 13, &LED_COLOR_NAMES[0]}; - -OSD_Entry menuLedstrip[] = -{ - {"--- LED TRIP ---", OME_Label, NULL, NULL}, - {"ENABLED", OME_Bool, NULL, &featureLedstrip}, - {"LED COLOR", OME_TAB, applyLedColor, &entryLed}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // LED_STRIP - -#if defined(VTX) || defined(USE_RTC6705) -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}, - {"ENABLED", OME_Bool, NULL, &featureVtx}, -#ifdef VTX - {"VTX MODE", OME_UINT8, NULL, &entryVtxMode}, - {"VTX MHZ", OME_UINT16, NULL, &entryVtxMhz}, -#endif // VTX - {"BAND", OME_TAB, NULL, &entryVtxBand}, - {"CHANNEL", OME_UINT8, NULL, &entryVtxChannel}, -#ifdef USE_RTC6705 - {"LOW POWER", OME_Bool, NULL, &masterConfig.vtx_power}, -#endif // USE_RTC6705 - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; -#endif // VTX || USE_RTC6705 - -#ifdef VTX_SMARTAUDIO -static const char * const smartAudioBandNames[] = { - "--------", - "BOSCAM A", - "BOSCAM B", - "BOSCAM E", - "FATSHARK", - "RACEBAND", -}; -OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; - -static const char * const smartAudioChanNames[] = { - "-", "1", "2", "3", "4", "5", "6", "7", "8", -}; - -OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; - -static const char * const smartAudioPowerNames[] = { - "---", - " 25", - "200", - "500", - "800", -}; - -OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; - -static const char * const smartAudioTxModeNames[] = { - "------", - "PIT-OR", - "PIT-IR", - "ACTIVE", -}; - -OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; - -OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; - -static const char * const smartAudioOpModelNames[] = { - "FREE", - "PIT", -}; - -OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; - -static const char * const smartAudioPitFModeNames[] = { - "IN-RANGE", - "OUT-RANGE", -}; - -OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; - -OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; - -OSD_Entry menu_smartAudioConfig[] = { - { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL }, - { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel }, - { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode }, - { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq }, // OME_Poll_UINT16 - { "BACK", OME_Back, NULL, NULL }, - { NULL, OME_END, NULL, NULL } -}; - -static const char * const smartAudioStatusNames[] = { - "OFFLINE", - "ONLINE V1", - "ONLINE V2", -}; - -OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; -OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; -OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; - -OSD_Entry menu_smartAudioStats[] = { - { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL }, - { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline }, - { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate }, - { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre }, - { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen }, - { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr }, - { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr }, - { "BACK", OME_Back, NULL, NULL }, - { NULL, OME_END, NULL, NULL } -}; - -OSD_Entry menu_vtx[] = -{ - { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL }, - { smartAudioStatusString, OME_Label, NULL, NULL }, - { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode }, - { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, - { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan }, - { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq }, - { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower }, - { "CONFIG", OME_Submenu, osdChangeScreen, &menu_smartAudioConfig[0] }, - { "STAT", OME_Submenu, osdChangeScreen, &menu_smartAudioStats[0] }, - { "BACK", OME_Back, NULL, NULL }, - { NULL, OME_END, NULL, NULL } -}; -#endif // VTX_SMARTAUDIO - -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}, - {"GYRO LOWPASS", OME_UINT8, NULL, &entryGyroSoftLpfHz}, - {"DTERM LPF", OME_UINT16, NULL, &entryDtermLpf}, - {"YAW LPF", OME_UINT16, NULL, &entryYawLpf}, - {"YAW P LIMIT", OME_UINT16, NULL, &entryYawPLimit}, - {"MINTHROTTLE", OME_UINT16, NULL, &entryMinThrottle}, - {"VBAT SCALE", OME_UINT8, NULL, &entryVbatScale}, - {"VBAT CELL MAX", OME_UINT8, NULL, &entryVbatMaxCell}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryPidProfile = {&masterConfig.current_profile_index, 0, MAX_PROFILE_COUNT, 1}; - -OSD_Entry menuImu[] = -{ - {"-----CFG. IMU-----", OME_Label, NULL, NULL}, - {"PID", OME_Submenu, osdChangeScreen, &menuPid[0]}, - {"PID PROFILE", OME_UINT8, NULL, &entryPidProfile}, - {"RATE & RXPO", OME_Submenu, osdChangeScreen, &menuRateExpo[0]}, - {"RC PREVIEW", OME_Submenu, osdChangeScreen, &menuRc[0]}, - {"MISC", OME_Submenu, osdChangeScreen, &menuMisc[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -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}; - -OSD_Entry menuPid[] = -{ - {"------- PID -------", OME_Label, NULL, NULL}, - {"ROLL P", OME_UINT8, NULL, &entryRollP}, - {"ROLL I", OME_UINT8, NULL, &entryRollI}, - {"ROLL D", OME_UINT8, NULL, &entryRollD}, - - {"PITCH P", OME_UINT8, NULL, &entryPitchP}, - {"PITCH I", OME_UINT8, NULL, &entryPitchI}, - {"PITCH D", OME_UINT8, NULL, &entryPitchD}, - - {"YAW P", OME_UINT8, NULL, &entryYawP}, - {"YAW I", OME_UINT8, NULL, &entryYawI}, - {"YAW D", OME_UINT8, NULL, &entryYawD}, - - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -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 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}; - -OSD_Entry menuRateExpo[] = -{ - {"----RATE & EXPO----", OME_Label, NULL, NULL}, - {"ROLL RATE", OME_FLOAT, NULL, &entryRollRate}, - {"PITCH RATE", OME_FLOAT, NULL, &entryPitchRate}, - {"YAW RATE", OME_FLOAT, NULL, &entryYawRate}, - {"RC RATE", OME_FLOAT, NULL, &entryRcRate}, - {"RC EXPO", OME_FLOAT, NULL, &entryRcExpo}, - {"RC YAW EXPO", OME_FLOAT, NULL, &entryRcExpoYaw}, - {"THR. PID ATT.", OME_FLOAT, NULL, &extryTpaEntry}, - {"TPA BREAKPOINT", OME_UINT16, NULL, &entryTpaBreak}, - {"D SETPOINT", OME_FLOAT, NULL, &entryDSetpoint}, - {"D SETPOINT TRANSITION", OME_FLOAT, NULL, &entryPSetpoint}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -static OSD_INT16_t entryRcRoll = {&rcData[ROLL], 1, 2500, 0}; -static OSD_INT16_t entryRcPitch = {&rcData[PITCH], 1, 2500, 0}; -static OSD_INT16_t entryRcThrottle = {&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 PREVIEW ----", OME_Label, NULL, NULL}, - {"ROLL", OME_INT16, NULL, &entryRcRoll}, - {"PITCH", OME_INT16, NULL, &entryRcPitch}, - {"THROTTLE", OME_INT16, NULL, &entryRcThrottle}, - {"YAW", OME_INT16, NULL, &entryRcYaw}, - {"AUX1", OME_INT16, NULL, &entryRcAux1}, - {"AUX2", OME_INT16, NULL, &entryRcAux2}, - {"AUX3", OME_INT16, NULL, &entryRcAux3}, - {"AUX4", OME_INT16, NULL, &entryRcAux4}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdLayout[] = -{ - {"---SCREEN LAYOUT---", OME_Label, NULL, NULL}, - {"ACTIVE ELEM.", OME_Submenu, osdChangeScreen, &menuOsdActiveElems[0]}, - {"POSITION", OME_Submenu, osdChangeScreen, &menuOsdElemsPositions[0]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_UINT8_t entryAlarmRssi = {&OSD_cfg.rssi_alarm, 5, 90, 5}; -OSD_UINT16_t entryAlarmCapacity = {&OSD_cfg.cap_alarm, 50, 30000, 50}; -OSD_UINT16_t enryAlarmFlyTime = {&OSD_cfg.time_alarm, 1, 200, 1}; -OSD_UINT16_t entryAlarmAltitude = {&OSD_cfg.alt_alarm, 1, 200, 1}; - -OSD_Entry menuAlarms[] = -{ - {"------ ALARMS ------", OME_Label, NULL, NULL}, - {"RSSI", OME_UINT8, NULL, &entryAlarmRssi}, - {"MAIN BATT.", OME_UINT16, NULL, &entryAlarmCapacity}, - {"FLY TIME", OME_UINT16, NULL, &enryAlarmFlyTime}, - {"MAX ALTITUDE", OME_UINT16, NULL, &entryAlarmAltitude}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdElemsPositions[] = -{ - {"---POSITION---", OME_Label, NULL, NULL}, - {"RSSI", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"UPTIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#ifdef VTX - {"VTX CHAN", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_VTX_CHANNEL]}, -#endif // VTX - {"CURRENT (A)", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_CURRENT_DRAW]}, - {"USED MAH", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_POS, osdEditElement, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -OSD_Entry menuOsdActiveElems[] = -{ - {" --ACTIV ELEM.-- ", OME_Label, NULL, NULL}, - {"RSSI", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE]}, - {"MAIN BATTERY", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE]}, - {"HORIZON", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]}, - {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_HORIZON_SIDEBARS]}, - {"UPTIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ONTIME]}, - {"FLY TIME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYTIME]}, - {"FLY MODE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_FLYMODE]}, - {"NAME", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_CRAFT_NAME]}, - {"THROTTLE POS", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_THROTTLE_POS]}, -#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]}, - {"USED MAH", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN]}, -#ifdef GPS - {"GPS SPEED", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SPEED]}, - {"GPS SATS.", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_GPS_SATS]}, -#endif // GPS - {"ALTITUDE", OME_VISIBLE, NULL, &masterConfig.osdProfile.item_pos[OSD_ALTITUDE]}, - {"BACK", OME_Back, NULL, NULL}, - {NULL, OME_END, NULL, NULL} -}; - -void resetOsdConfig(osd_profile_t *osdProfile) -{ - osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; - osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); - osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); - osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); - osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); - osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); - osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); - osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); - osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); - - osdProfile->rssi_alarm = 20; - osdProfile->cap_alarm = 2200; - osdProfile->time_alarm = 10; // in minutes - osdProfile->alt_alarm = 100; // meters or feet depend on configuration - - osdProfile->video_system = 0; -} - -void osdInit(void) -{ - char x, string_buffer[30]; - - armState = ARMING_FLAG(ARMED); - - max7456Init(masterConfig.osdProfile.video_system); - - max7456ClearScreen(); - - // display logo and help - x = 160; - for (int i = 1; i < 5; i++) { - for (int j = 3; j < 27; j++) { - if (x != 255) - max7456WriteChar(j, i, x++); - } - } - - sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); - max7456Write(5, 6, string_buffer); - max7456Write(7, 7, "MENU: THRT MID"); - max7456Write(13, 8, "YAW RIGHT"); - max7456Write(13, 9, "PITCH UP"); - max7456RefreshAll(); - - refreshTimeout = 4 * REFRESH_1S; -} - -/** - * Gets the correct altitude symbol for the current unit system - */ -char osdGetAltitudeSymbol() -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return 0xF; - default: - return 0xC; - } -} - -/** - * Converts altitude based on the current unit system. - * @param alt Raw altitude (i.e. as taken from BaroAlt) - */ -int32_t osdGetAltitude(int32_t alt) -{ - switch (masterConfig.osdProfile.units) { - case OSD_UNIT_IMPERIAL: - return (alt * 328) / 100; // Convert to feet / 100 - default: - return alt; // Already in metre / 100 - } -} - -void osdUpdateAlarms(void) -{ - int32_t alt = osdGetAltitude(BaroAlt) / 100; - statRssi = rssi * 100 / 1024; - - if (statRssi < OSD_cfg.rssi_alarm) - OSD_cfg.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - - if (vbat <= (batteryWarningVoltage - 1)) - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - - if (STATE(GPS_FIX) == 0) - OSD_cfg.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - - if (flyTime / 60 >= OSD_cfg.time_alarm && ARMING_FLAG(ARMED)) - OSD_cfg.item_pos[OSD_FLYTIME] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - - if (mAhDrawn >= OSD_cfg.cap_alarm) - OSD_cfg.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; - - if (alt >= OSD_cfg.alt_alarm) - OSD_cfg.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; - else - OSD_cfg.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; -} - -void osdResetAlarms(void) -{ - OSD_cfg.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; - OSD_cfg.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; -} - -uint8_t osdHandleKey(uint8_t key) -{ - uint8_t res = BUTTON_TIME; - OSD_Entry *p; - - if (!currentMenu) - return res; - - if (key == KEY_ESC) { - osdMenuBack(); - return BUTTON_PAUSE; - } - - if (key == KEY_DOWN) { - if (currentMenuPos < currentMenuIdx) - currentMenuPos++; - else { - if (nextPage) // we have more pages - { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = 0; - } - } - - if (key == KEY_UP) { - currentMenuPos--; - - if ((currentMenu + currentMenuPos)->type == OME_Label && currentMenuPos > 0) - currentMenuPos--; - - if (currentMenuPos == -1 || (currentMenu + currentMenuPos)->type == OME_Label) { - if (nextPage) { - max7456ClearScreen(); - p = nextPage; - nextPage = currentMenu; - currentMenu = (OSD_Entry *)p; - currentMenuPos = 0; - osdUpdateMaxRows(); - } - currentMenuPos = currentMenuIdx; - } - } - - if (key == KEY_DOWN || key == KEY_UP) - return res; - - p = currentMenu + currentMenuPos; - - switch (p->type) { - case OME_POS: - if (key == KEY_RIGHT) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) // no submenu for hidden elements - break; - } - case OME_Submenu: - case OME_OSD_Exit: - if (p->func && key == KEY_RIGHT) { - p->func(p->data); - res = BUTTON_PAUSE; - } - break; - case OME_Back: - osdMenuBack(); - res = BUTTON_PAUSE; - break; - case OME_Bool: - if (p->data) { - uint8_t *val = p->data; - if (key == KEY_RIGHT) - *val = 1; - else - *val = 0; - } - break; - case OME_VISIBLE: - 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; - } - 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; - } - } - 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(p->data); - } - 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; - } - } - 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; - } - } - 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; - } - } - break; - case OME_Label: - case OME_END: - break; - } - return res; -} - -void osdUpdateMaxRows(void) -{ - OSD_Entry *ptr; - - currentMenuIdx = 0; - for (ptr = currentMenu; ptr->type != OME_END; ptr++) - currentMenuIdx++; - - if (currentMenuIdx > MAX_MENU_ITEMS) - currentMenuIdx = MAX_MENU_ITEMS; - - currentMenuIdx--; -} - -void osdMenuBack(void) -{ - uint8_t i; - - // becasue pids and rates meybe stored in profiles we need some thicks to manipulate it - // hack to save pid profile - if (currentMenu == &menuPid[0]) { - for (i = 0; i < 3; i++) { - curr_profile.pidProfile.P8[i] = tempPid[i][0]; - curr_profile.pidProfile.I8[i] = tempPid[i][1]; - curr_profile.pidProfile.D8[i] = tempPid[i][2]; - } - - curr_profile.pidProfile.P8[PIDLEVEL] = tempPid[3][0]; - curr_profile.pidProfile.I8[PIDLEVEL] = tempPid[3][1]; - curr_profile.pidProfile.D8[PIDLEVEL] = tempPid[3][2]; - } - - // hack - save rate config for current profile - if (currentMenu == &menuRateExpo[0]) - memcpy(&masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], &rateProfile, sizeof(controlRateConfig_t)); - - if (menuStackIdx) { - max7456ClearScreen(); - menuStackIdx--; - nextPage = NULL; - currentMenu = menuStack[menuStackIdx]; - currentMenuPos = menuStackHistory[menuStackIdx]; - - osdUpdateMaxRows(); - } - else - osdOpenMenu(); -} - -void simple_ftoa(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 osdDrawMenu(void) -{ - uint8_t i = 0; - OSD_Entry *p; - char buff[10]; - uint8_t top = (osdRows - currentMenuIdx) / 2 - 1; - if (!currentMenu) - return; - - if ((currentMenu + currentMenuPos)->type == OME_Label) // skip label - currentMenuPos++; - - for (p = currentMenu; p->type != OME_END; p++) { - if (currentMenuPos == i) - max7456Write(LEFT_MENU_COLUMN, i + top, " >"); - else - max7456Write(LEFT_MENU_COLUMN, i + top, " "); - max7456Write(LEFT_MENU_COLUMN + 2, i + top, p->text); - - switch (p->type) { - case OME_POS: { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - if (!(*val & VISIBLE_FLAG)) - break; - } - case OME_Submenu: - max7456Write(RIGHT_MENU_COLUMN, i + top, ">"); - break; - case OME_Bool: - if (p->data) { - if (*((uint8_t *)(p->data))) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_TAB: { - OSD_TAB_t *ptr = p->data; - max7456Write(RIGHT_MENU_COLUMN - 5, i + top, (char *)ptr->names[*ptr->val]); - break; - } - case OME_VISIBLE: - if (p->data) { - uint32_t address = (uint32_t)p->data; - uint16_t *val; - - val = (uint16_t *)address; - - if (VISIBLE(*val)) - max7456Write(RIGHT_MENU_COLUMN, i + top, "YES"); - else - max7456Write(RIGHT_MENU_COLUMN, i + top, "NO "); - } - break; - case OME_UINT8: - if (p->data) { - OSD_UINT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT8: - if (p->data) { - OSD_INT8_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_UINT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_INT16: - if (p->data) { - OSD_UINT16_t *ptr = p->data; - itoa(*ptr->val, buff, 10); - max7456Write(RIGHT_MENU_COLUMN, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN, i + top, buff); - } - break; - case OME_FLOAT: - if (p->data) { - OSD_FLOAT_t *ptr = p->data; - simple_ftoa(*ptr->val * ptr->multipler, buff); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, " "); - max7456Write(RIGHT_MENU_COLUMN - 1, i + top, buff); - } - break; - case OME_OSD_Exit: - case OME_Label: - case OME_END: - case OME_Back: - break; - } - i++; - - if (i == MAX_MENU_ITEMS) // max per page - { - nextPage = currentMenu + i; - if (nextPage->type == OME_END) - nextPage = NULL; - break; - } - } -} - -void osdResetStats(void) -{ - stats.max_current = 0; - stats.max_speed = 0; - stats.min_voltage = 500; - stats.max_current = 0; - stats.min_rssi = 99; - stats.max_altitude = 0; -} - -void osdUpdateStats(void) -{ - int16_t value; - - value = GPS_speed * 36 / 1000; - if (stats.max_speed < value) - stats.max_speed = value; - - if (stats.min_voltage > vbat) - stats.min_voltage = vbat; - - value = amperage / 100; - if (stats.max_current < value) - stats.max_current = value; - - if (stats.min_rssi > statRssi) - stats.min_rssi = statRssi; - - if (stats.max_altitude < BaroAlt) - stats.max_altitude = BaroAlt; -} - -void osdShowStats(void) -{ - uint8_t top = 2; - char buff[10]; - - max7456ClearScreen(); - max7456Write(2, top++, " --- STATS ---"); - - if (STATE(GPS_FIX)) { - max7456Write(2, top, "MAX SPEED :"); - itoa(stats.max_speed, buff, 10); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MIN BATTERY :"); - sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); - max7456Write(22, top++, buff); - - max7456Write(2, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - max7456Write(22, top++, buff); - - if (feature(FEATURE_CURRENT_METER)) { - max7456Write(2, top, "MAX CURRENT :"); - itoa(stats.max_current, buff, 10); - strcat(buff, "A"); - max7456Write(22, top++, buff); - - max7456Write(2, top, "USED MAH :"); - itoa(mAhDrawn, buff, 10); - strcat(buff, "\x07"); - max7456Write(22, top++, buff); - } - - max7456Write(2, top, "MAX ALTITUDE :"); - int32_t alt = osdGetAltitude(stats.max_altitude); - sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); - max7456Write(22, top++, buff); - - refreshTimeout = 60 * REFRESH_1S; -} - -// called when motors armed -void osdArmMotors(void) -{ - max7456ClearScreen(); - max7456Write(12, 7, "ARMED"); - refreshTimeout = REFRESH_1S / 2; - osdResetStats(); -} - -void updateOsd(uint32_t currentTime) -{ - static uint32_t counter; -#ifdef MAX7456_DMA_CHANNEL_TX - // don't touch buffers if DMA transaction is in progress - if (max7456DmaInProgres()) - return; -#endif // MAX7456_DMA_CHANNEL_TX - - // redraw values in buffer - if (counter++ % 5 == 0) - osdUpdate(currentTime); - else // rest of time redraw screen 10 chars per idle to don't lock the main idle - max7456DrawScreen(); - - // do not allow ARM if we are in menu - if (inMenu) - DISABLE_ARMING_FLAG(OK_TO_ARM); -} - -void osdUpdate(uint32_t currentTime) -{ - static uint8_t rcDelay = BUTTON_TIME; - static uint8_t lastSec = 0; - uint8_t key = 0, sec; - - // detect enter to menu - if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) - osdOpenMenu(); - - // detect arm/disarm - if (armState != ARMING_FLAG(ARMED)) { - if (ARMING_FLAG(ARMED)) - osdArmMotors(); // reset statistic etc - else - osdShowStats(); // show statistic - - armState = ARMING_FLAG(ARMED); - } - - osdUpdateStats(); - - sec = currentTime / 1000000; - - if (ARMING_FLAG(ARMED) && sec != lastSec) { - flyTime++; - lastSec = sec; - } - - if (refreshTimeout) { - if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics - refreshTimeout = 1; - refreshTimeout--; - if (!refreshTimeout) - max7456ClearScreen(); - return; - } - - blinkState = (millis() / 200) % 2; - - if (inMenu) { - if (rcDelay) { - rcDelay--; - } - 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 exit using YAW - { - key = KEY_ESC; - rcDelay = BUTTON_TIME; - } - - if (key && !currentElement) { - rcDelay = osdHandleKey(key); - return; - } - if (currentElement) // edit position of element - { - if (key) { - if (key == KEY_ESC) { - // exit - osdMenuBack(); - rcDelay = BUTTON_PAUSE; - *currentElement &= ~BLINK_FLAG; - currentElement = NULL; - return; - } - else { - uint8_t x, y; - x = OSD_X(*currentElement); - y = OSD_Y(*currentElement); - switch (key) { - case KEY_UP: - y--; - break; - case KEY_DOWN: - y++; - break; - case KEY_RIGHT: - x++; - break; - case KEY_LEFT: - x--; - break; - } - - *currentElement &= 0xFC00; - *currentElement |= OSD_POS(x, y); - max7456ClearScreen(); - } - } - osdDrawElements(); - } - else - osdDrawMenu(); - } - else { - osdUpdateAlarms(); - osdDrawElements(); - } -} - -void osdChangeScreen(void *ptr) -{ - uint8_t i; - if (ptr) { - max7456ClearScreen(); - // hack - save profile to temp - if (ptr == &menuPid[0]) { - for (i = 0; i < 3; i++) { - tempPid[i][0] = curr_profile.pidProfile.P8[i]; - tempPid[i][1] = curr_profile.pidProfile.I8[i]; - tempPid[i][2] = curr_profile.pidProfile.D8[i]; - } - tempPid[3][0] = curr_profile.pidProfile.P8[PIDLEVEL]; - tempPid[3][1] = curr_profile.pidProfile.I8[PIDLEVEL]; - tempPid[3][2] = curr_profile.pidProfile.D8[PIDLEVEL]; - } - - if (ptr == &menuRateExpo[0]) - memcpy(&rateProfile, &masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile], sizeof(controlRateConfig_t)); - - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentMenu = (OSD_Entry *)ptr; - currentMenuPos = 0; - osdUpdateMaxRows(); - } -} - -#ifdef USE_FLASHFS -void osdEraseFlash(void *ptr) -{ - UNUSED(ptr); - - max7456ClearScreen(); - max7456Write(5, 3, "ERASING FLASH..."); - max7456RefreshAll(); - - flashfsEraseCompletely(); - while (!flashfsIsReady()) { - delay(100); - } - max7456ClearScreen(); - max7456RefreshAll(); -} -#endif // USE_FLASHFS - -void osdEditElement(void *ptr) -{ - uint32_t address = (uint32_t)ptr; - - // zsave position on menu stack - menuStack[menuStackIdx] = currentMenu; - menuStackHistory[menuStackIdx] = currentMenuPos; - menuStackIdx++; - - currentElement = (uint16_t *)address; - - *currentElement |= BLINK_FLAG; - max7456ClearScreen(); -} - -void osdExitMenu(void *ptr) -{ - max7456ClearScreen(); - max7456Write(5, 3, "RESTARTING IMU..."); - max7456RefreshAll(); - stopMotors(); - stopPwmAllMotors(); - delay(200); - - if (ptr) { - // save local variables to configuration - if (featureBlackbox) - featureSet(FEATURE_BLACKBOX); - else - featureClear(FEATURE_BLACKBOX); - - if (featureLedstrip) - featureSet(FEATURE_LED_STRIP); - else - featureClear(FEATURE_LED_STRIP); -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) - if (featureVtx) - featureSet(FEATURE_VTX); - else - featureClear(FEATURE_VTX); -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO - -#ifdef VTX - masterConfig.vtxBand = vtxBand; - masterConfig.vtx_channel = vtxChannel - 1; -#endif // VTX - -#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) - masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; -#endif // USE_RTC6705 || VTX_SMARTAUDIO - - saveConfigAndNotify(); - } - - systemReset(); -} - -void osdOpenMenu(void) -{ - if (inMenu) - return; - - if (feature(FEATURE_LED_STRIP)) - featureLedstrip = 1; - - if (feature(FEATURE_BLACKBOX)) - featureBlackbox = 1; - -#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) - if (feature(FEATURE_VTX)) - featureVtx = 1; -#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO - -#ifdef VTX - vtxBand = masterConfig.vtxBand; - vtxChannel = masterConfig.vtx_channel + 1; -#endif // VTX - -#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) - vtxBand = masterConfig.vtx_channel / 8; - vtxChannel = masterConfig.vtx_channel % 8 + 1; -#endif // USE_RTC6705 - - osdRows = max7456GetRowsCount(); - inMenu = true; - refreshTimeout = 0; - max7456ClearScreen(); - currentMenu = &menuMain[0]; - osdResetAlarms(); - osdChangeScreen(currentMenu); -#ifdef LED_STRIP - getLedColor(); -#endif // LED_STRIP -} - -void osdDrawElementPositioningHelp(void) -{ - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); - max7456Write(OSD_X(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(OSD_cfg.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); -} +#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AH_SIDEBAR_WIDTH_POS 7 +#define AH_SIDEBAR_HEIGHT_POS 3 void osdDrawElements(void) { max7456ClearScreen(); +#if 0 if (currentElement) osdDrawElementPositioningHelp(); - else if (sensors(SENSOR_ACC) || inMenu) +#else + if (false) + ; +#endif + else if (sensors(SENSOR_ACC) || osdInMenu) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); osdDrawSingleElement(OSD_CROSSHAIRS); @@ -1567,26 +130,20 @@ void osdDrawElements(void) osdDrawSingleElement(OSD_ALTITUDE); #ifdef GPS - if (sensors(SENSOR_GPS) || inMenu) { + if (sensors(SENSOR_GPS) || osdInMenu) { osdDrawSingleElement(OSD_GPS_SATS); osdDrawSingleElement(OSD_GPS_SPEED); } #endif // GPS } -#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees -#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees -#define AH_SIDEBAR_WIDTH_POS 7 -#define AH_SIDEBAR_HEIGHT_POS 3 - - void osdDrawSingleElement(uint8_t item) { - if (!VISIBLE(OSD_cfg.item_pos[item]) || BLINK(OSD_cfg.item_pos[item])) + if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.item_pos[item])) return; - uint8_t elemPosX = OSD_X(OSD_cfg.item_pos[item]); - uint8_t elemPosY = OSD_Y(OSD_cfg.item_pos[item]); + uint8_t elemPosX = OSD_X(masterConfig.osdProfile.item_pos[item]); + uint8_t elemPosY = OSD_Y(masterConfig.osdProfile.item_pos[item]); char buff[32]; switch(item) { @@ -1788,4 +345,372 @@ void osdDrawSingleElement(uint8_t item) max7456Write(elemPosX, elemPosY, buff); } +void resetOsdConfig(osd_profile_t *osdProfile) +{ + osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); + osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); + osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); + osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); + osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); + osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); + osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + + osdProfile->rssi_alarm = 20; + osdProfile->cap_alarm = 2200; + osdProfile->time_alarm = 10; // in minutes + osdProfile->alt_alarm = 100; // meters or feet depend on configuration + + osdProfile->video_system = 0; +} + +void osdInit(void) +{ + char x, string_buffer[30]; + + armState = ARMING_FLAG(ARMED); + + max7456Init(masterConfig.osdProfile.video_system); + + max7456ClearScreen(); + + // display logo and help + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) { + if (x != 255) + max7456WriteChar(j, i, x++); + } + } + + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); + max7456Write(5, 6, string_buffer); + max7456Write(7, 7, STARTUP_HELP_TEXT1); + max7456Write(11, 8, STARTUP_HELP_TEXT2); + max7456Write(11, 9, STARTUP_HELP_TEXT3); + + max7456RefreshAll(); + + refreshTimeout = 4 * REFRESH_1S; + +#ifdef CMS + cmsDeviceRegister(osdCmsInit); +#endif +} + +/** + * Gets the correct altitude symbol for the current unit system + */ +char osdGetAltitudeSymbol() +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return 0xF; + default: + return 0xC; + } +} + +/** + * Converts altitude based on the current unit system. + * @param alt Raw altitude (i.e. as taken from BaroAlt) + */ +int32_t osdGetAltitude(int32_t alt) +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return (alt * 328) / 100; // Convert to feet / 100 + default: + return alt; // Already in metre / 100 + } +} + +void osdUpdateAlarms(void) +{ + int32_t alt = osdGetAltitude(BaroAlt) / 100; + statRssi = rssi * 100 / 1024; + + if (statRssi < masterConfig.osdProfile.rssi_alarm) + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + + if (vbat <= (batteryWarningVoltage - 1)) + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + + if (STATE(GPS_FIX) == 0) + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + + if (flyTime / 60 >= masterConfig.osdProfile.time_alarm && ARMING_FLAG(ARMED)) + masterConfig.osdProfile.item_pos[OSD_FLYTIME] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + + if (mAhDrawn >= masterConfig.osdProfile.cap_alarm) + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + + if (alt >= masterConfig.osdProfile.alt_alarm) + masterConfig.osdProfile.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; +} + +void osdResetAlarms(void) +{ + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; +} + +void osdResetStats(void) +{ + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 500; + stats.max_current = 0; + stats.min_rssi = 99; + stats.max_altitude = 0; +} + +void osdUpdateStats(void) +{ + int16_t value; + + value = GPS_speed * 36 / 1000; + if (stats.max_speed < value) + stats.max_speed = value; + + if (stats.min_voltage > vbat) + stats.min_voltage = vbat; + + value = amperage / 100; + if (stats.max_current < value) + stats.max_current = value; + + if (stats.min_rssi > statRssi) + stats.min_rssi = statRssi; + + if (stats.max_altitude < BaroAlt) + stats.max_altitude = BaroAlt; +} + +void osdShowStats(void) +{ + uint8_t top = 2; + char buff[10]; + + max7456ClearScreen(); + max7456Write(2, top++, " --- STATS ---"); + + if (STATE(GPS_FIX)) { + max7456Write(2, top, "MAX SPEED :"); + itoa(stats.max_speed, buff, 10); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MIN BATTERY :"); + sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); + max7456Write(22, top++, buff); + + max7456Write(2, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + max7456Write(22, top++, buff); + + if (feature(FEATURE_CURRENT_METER)) { + max7456Write(2, top, "MAX CURRENT :"); + itoa(stats.max_current, buff, 10); + strcat(buff, "A"); + max7456Write(22, top++, buff); + + max7456Write(2, top, "USED MAH :"); + itoa(mAhDrawn, buff, 10); + strcat(buff, "\x07"); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MAX ALTITUDE :"); + int32_t alt = osdGetAltitude(stats.max_altitude); + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); + max7456Write(22, top++, buff); + + refreshTimeout = 60 * REFRESH_1S; +} + +// called when motors armed +void osdArmMotors(void) +{ + max7456ClearScreen(); + max7456Write(12, 7, "ARMED"); + refreshTimeout = REFRESH_1S / 2; + osdResetStats(); +} + +void updateOsd(uint32_t currentTime) +{ + static uint32_t counter = 0; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456DmaInProgres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdUpdate(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456DrawScreen(); + + // do not allow ARM if we are in menu + if (osdInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void osdUpdate(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + +#ifdef OSD_CALLS_CMS + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } +#endif + + // detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) + osdArmMotors(); // reset statistic etc + else + osdShowStats(); // show statistic + + armState = ARMING_FLAG(ARMED); + } + + osdUpdateStats(); + + sec = currentTime / 1000000; + + if (ARMING_FLAG(ARMED) && sec != lastSec) { + flyTime++; + lastSec = sec; + } + + if (refreshTimeout) { + if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + refreshTimeout = 1; + refreshTimeout--; + if (!refreshTimeout) + max7456ClearScreen(); + return; + } + + blinkState = (millis() / 200) % 2; + + if (!osdInMenu) { + osdUpdateAlarms(); + osdDrawElements(); +#ifdef OSD_CALLS_CMS + } else { + cmsUpdate(currentTime); +#endif + } +} + +// +// OSD specific CMS functions +// + +uint8_t shiftdown; + +int osdMenuBegin(void) +{ + osdResetAlarms(); + osdInMenu = true; + refreshTimeout = 0; + + return 0; +} + +int osdMenuEnd(void) +{ + osdInMenu = false; + + return 0; +} + +int osdClearScreen(void) +{ + max7456ClearScreen(); + + return 0; +} + +int osdWrite(uint8_t x, uint8_t y, char *s) +{ + max7456Write(x, y + shiftdown, s); + + return 0; +} + +#ifdef EDIT_ELEMENT_SUPPORT +void osdEditElement(void *ptr) +{ + uint32_t address = (uint32_t)ptr; + + // zsave position on menu stack + menuStack[menuStackIdx] = currentMenu; + menuStackHistory[menuStackIdx] = currentMenuPos; + menuStackIdx++; + + currentElement = (uint16_t *)address; + + *currentElement |= BLINK_FLAG; + max7456ClearScreen(); +} + +void osdDrawElementPositioningHelp(void) +{ + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); +} +#endif + +screenFnVTable_t osdVTable = { + osdMenuBegin, + osdMenuEnd, + osdClearScreen, + osdWrite, + NULL, + max7456RefreshAll, +}; + +void osdCmsInit(displayPort_t *pPort) +{ + shiftdown = masterConfig.osdProfile.row_shiftdown; + pPort->rows = max7456GetRowsCount() - shiftdown; + pPort->cols = 30; + pPort->buftime = 1; // Very fast + pPort->bufsize = 50000; // Very large + pPort->VTable = &osdVTable; +} #endif // OSD diff --git a/src/main/io/osd.c.orig b/src/main/io/osd.c.orig new file mode 100755 index 0000000000..8622f24e19 --- /dev/null +++ b/src/main/io/osd.c.orig @@ -0,0 +1,1006 @@ +/* + * 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 . + */ + +/* + Created by Marcin Baliniak + some functions based on MinimOSD + + OSD-CMS separation by jflyper + */ + +#include +#include +#include +#include + +#include "platform.h" + +#include "build/version.h" + +#ifdef OSD + +#include "common/utils.h" + +#include "drivers/system.h" + +#include "io/cms.h" +#include "io/cms_types.h" + +#include "io/flashfs.h" +#include "io/osd.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" + +#ifdef USE_HARDWARE_REVISION_DETECTION +#include "hardware_revision.h" +#endif + +#include "drivers/max7456.h" +#include "drivers/max7456_symbols.h" + +<<<<<<< HEAD +#ifdef USE_RTC6705 +#include "drivers/vtx_soft_spi_rtc6705.h" +#endif + +#ifdef VTX_SMARTAUDIO +#include "io/vtx_smartaudio.h" +#endif + +======= +>>>>>>> bfdev-osd-cms-separation-poc +#include "common/printf.h" + +#include "build/debug.h" + +// Things in both OSD and CMS + +#define IS_HI(X) (rcData[X] > 1750) +#define IS_LO(X) (rcData[X] < 1250) +#define IS_MID(X) (rcData[X] > 1250 && rcData[X] < 1750) + +bool blinkState = true; + +//extern uint8_t RSSI; // TODO: not used? + +static uint16_t flyTime = 0; +uint8_t statRssi; + +statistic_t stats; + +uint16_t refreshTimeout = 0; +#define REFRESH_1S 12 + +uint8_t armState; +<<<<<<< HEAD +uint8_t featureBlackbox = 0; +uint8_t featureLedstrip = 0; + +#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +uint8_t featureVtx = 0, vtxBand, vtxChannel; +#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO +======= +>>>>>>> bfdev-osd-cms-separation-poc + +// OSD forwards + +void osdUpdate(uint32_t currentTime); +char osdGetAltitudeSymbol(); +int32_t osdGetAltitude(int32_t alt); +void osdEditElement(void *ptr); +void osdDrawElements(void); +void osdDrawSingleElement(uint8_t item); + +bool osdInMenu = false; + +#define AH_MAX_PITCH 200 // Specify maximum AHI pitch value displayed. Default 200 = 20.0 degrees +#define AH_MAX_ROLL 400 // Specify maximum AHI roll value displayed. Default 400 = 40.0 degrees +#define AH_SIDEBAR_WIDTH_POS 7 +#define AH_SIDEBAR_HEIGHT_POS 3 + +void osdDrawElements(void) +{ + max7456ClearScreen(); + +#if 0 + if (currentElement) + osdDrawElementPositioningHelp(); +#else + if (false) + ; +#endif + else if (sensors(SENSOR_ACC) || osdInMenu) + { + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_CROSSHAIRS); + } + + osdDrawSingleElement(OSD_MAIN_BATT_VOLTAGE); + osdDrawSingleElement(OSD_RSSI_VALUE); + osdDrawSingleElement(OSD_FLYTIME); + osdDrawSingleElement(OSD_ONTIME); + osdDrawSingleElement(OSD_FLYMODE); + osdDrawSingleElement(OSD_THROTTLE_POS); + osdDrawSingleElement(OSD_VTX_CHANNEL); + osdDrawSingleElement(OSD_CURRENT_DRAW); + osdDrawSingleElement(OSD_MAH_DRAWN); + osdDrawSingleElement(OSD_CRAFT_NAME); + osdDrawSingleElement(OSD_ALTITUDE); + +<<<<<<< HEAD +typedef struct +{ + uint8_t *val; + uint8_t min; + uint8_t max; + uint8_t step; + uint16_t multipler; +} OSD_FLOAT_t; + +typedef struct +{ + uint8_t *val; + uint8_t max; + const char * const *names; +} OSD_TAB_t; + +OSD_Entry *menuStack[10]; //tab to save menu stack +uint8_t menuStackHistory[10]; //current position in menu stack +uint8_t menuStackIdx = 0; + +OSD_Entry *currentMenu; +OSD_Entry *nextPage = NULL; + +int8_t currentMenuPos = 0; +uint8_t currentMenuIdx = 0; +uint16_t *currentElement = NULL; + +OSD_Entry menuAlarms[]; +OSD_Entry menuOsdLayout[]; +OSD_Entry menuOsdActiveElems[]; +OSD_Entry menuOsdElemsPositions[]; +OSD_Entry menuFeatures[]; +OSD_Entry menuBlackbox[]; +#ifdef LED_STRIP +OSD_Entry menuLedstrip[]; +#endif // LED_STRIP +#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) +OSD_Entry menu_vtx[]; +#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO +OSD_Entry menuImu[]; +OSD_Entry menuPid[]; +OSD_Entry menuRc[]; +OSD_Entry menuRateExpo[]; +OSD_Entry menuMisc[]; + +OSD_Entry menuMain[] = +{ + {"----MAIN MENU----", OME_Label, NULL, NULL}, + {"SCREEN LAYOUT", OME_Submenu, osdChangeScreen, &menuOsdLayout[0]}, + {"ALARMS", OME_Submenu, osdChangeScreen, &menuAlarms[0]}, + {"CFG. IMU", OME_Submenu, osdChangeScreen, &menuImu[0]}, + {"FEATURES", OME_Submenu, osdChangeScreen, &menuFeatures[0]}, + {"SAVE & EXIT", OME_OSD_Exit, osdExitMenu, (void*)1}, + {"EXIT", OME_OSD_Exit, osdExitMenu, (void*)0}, + {NULL,OME_END, NULL, NULL} +}; +======= +#ifdef GPS + if (sensors(SENSOR_GPS) || osdInMenu) { + osdDrawSingleElement(OSD_GPS_SATS); + osdDrawSingleElement(OSD_GPS_SPEED); + } +#endif // GPS +} +>>>>>>> bfdev-osd-cms-separation-poc + +void osdDrawSingleElement(uint8_t item) +{ +<<<<<<< HEAD + {"----- FEATURES -----", OME_Label, NULL, NULL}, + {"BLACKBOX", OME_Submenu, osdChangeScreen, &menuBlackbox[0]}, +#ifdef LED_STRIP + {"LED STRIP", OME_Submenu, osdChangeScreen, &menuLedstrip[0]}, +#endif // LED_STRIP +#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) + {"VTX", OME_Submenu, osdChangeScreen, &menu_vtx[0]}, +#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO + {"BACK", OME_Back, NULL, NULL}, + {NULL, OME_END, NULL, NULL} +}; +======= + if (!VISIBLE(masterConfig.osdProfile.item_pos[item]) || BLINK(masterConfig.osdProfile.item_pos[item])) + return; +>>>>>>> bfdev-osd-cms-separation-poc + + uint8_t elemPosX = OSD_X(masterConfig.osdProfile.item_pos[item]); + uint8_t elemPosY = OSD_Y(masterConfig.osdProfile.item_pos[item]); + char buff[32]; + + switch(item) { + case OSD_RSSI_VALUE: + { + uint16_t osdRssi = rssi * 100 / 1024; // change range + if (osdRssi >= 100) + osdRssi = 99; + + buff[0] = SYM_RSSI; + sprintf(buff + 1, "%d", osdRssi); + break; + } + + case OSD_MAIN_BATT_VOLTAGE: + { + buff[0] = SYM_BATT_5; + sprintf(buff + 1, "%d.%1dV", vbat / 10, vbat % 10); + break; + } + + case OSD_CURRENT_DRAW: + { + buff[0] = SYM_AMP; + sprintf(buff + 1, "%d.%02d", abs(amperage) / 100, abs(amperage) % 100); + break; + } + + case OSD_MAH_DRAWN: + { + buff[0] = SYM_MAH; + sprintf(buff + 1, "%d", mAhDrawn); + break; + } + +#ifdef GPS + case OSD_GPS_SATS: + { + buff[0] = 0x1f; + sprintf(buff + 1, "%d", GPS_numSat); + break; + } + + case OSD_GPS_SPEED: + { + sprintf(buff, "%d", GPS_speed * 36 / 1000); + break; + } +#endif // GPS + + case OSD_ALTITUDE: + { + int32_t alt = osdGetAltitude(BaroAlt); + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); + break; + } + + case OSD_ONTIME: + { + uint32_t seconds = micros() / 1000000; + buff[0] = SYM_ON_M; + sprintf(buff + 1, "%02d:%02d", seconds / 60, seconds % 60); + break; + } + + case OSD_FLYTIME: + { + buff[0] = SYM_FLY_M; + sprintf(buff + 1, "%02d:%02d", flyTime / 60, flyTime % 60); + break; + } + + case OSD_FLYMODE: + { + char *p = "ACRO"; + +<<<<<<< HEAD +#ifdef VTX_SMARTAUDIO +static const char * const smartAudioBandNames[] = { + "--------", + "BOSCAM A", + "BOSCAM B", + "BOSCAM E", + "FATSHARK", + "RACEBAND", +}; +OSD_TAB_t entrySmartAudioBand = { &smartAudioBand, 5, &smartAudioBandNames[0], NULL }; + +static const char * const smartAudioChanNames[] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8", +}; + +OSD_TAB_t entrySmartAudioChan = { &smartAudioChan, 8, &smartAudioChanNames[0], NULL }; + +static const char * const smartAudioPowerNames[] = { + "---", + " 25", + "200", + "500", + "800", +}; + +OSD_TAB_t entrySmartAudioPower = { &smartAudioPower, 4, &smartAudioPowerNames[0]}; + +static const char * const smartAudioTxModeNames[] = { + "------", + "PIT-OR", + "PIT-IR", + "ACTIVE", +}; + +OSD_TAB_t entrySmartAudioTxMode = { &smartAudioTxMode, 3, &smartAudioTxModeNames[0]}; + +OSD_UINT16_t entrySmartAudioFreq = { &smartAudioFreq, 5600, 5900, 0 }; + +static const char * const smartAudioOpModelNames[] = { + "FREE", + "PIT", +}; + +OSD_TAB_t entrySmartAudioOpModel = { &smartAudioOpModel, 1, &smartAudioOpModelNames[0] }; + +static const char * const smartAudioPitFModeNames[] = { + "IN-RANGE", + "OUT-RANGE", +}; + +OSD_TAB_t entrySmartAudioPitFMode = { &smartAudioPitFMode, 1, &smartAudioPitFModeNames[0] }; + +OSD_UINT16_t entrySmartAudioORFreq = { &smartAudioORFreq, 5600, 5900, 1 }; + +OSD_Entry menu_smartAudioConfig[] = { + { "--- SMARTAUDIO CONFIG ---", OME_Label, NULL, NULL }, + { "OP MODEL", OME_TAB, smartAudioConfigureOpModelByGvar, &entrySmartAudioOpModel }, + { "PIT FREQ", OME_TAB, smartAudioConfigurePitFModeByGvar, &entrySmartAudioPitFMode }, + { "OR FREQ", OME_UINT16, NULL, &entrySmartAudioORFreq }, // OME_Poll_UINT16 + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + +static const char * const smartAudioStatusNames[] = { + "OFFLINE", + "ONLINE V1", + "ONLINE V2", +}; + +OSD_TAB_t entrySmartAudioOnline = { &smartAudioStatus, 2, &smartAudioStatusNames[0] }; +OSD_UINT16_t entrySmartAudioBaudrate = { &smartAudioSmartbaud, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadpre = { &saerr_badpre, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatBadlen = { &saerr_badlen, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatCrcerr = { &saerr_crcerr, 0, 0, 0 }; +OSD_UINT16_t entrySmartAudioStatOooerr = { &saerr_oooresp, 0, 0, 0 }; + +OSD_Entry menu_smartAudioStats[] = { + { "--- SMARTAUDIO STATS ---", OME_Label, NULL, NULL }, + { "STATUS", OME_TAB, NULL, &entrySmartAudioOnline }, + { "BAUDRATE", OME_UINT16, NULL, &entrySmartAudioBaudrate }, + { "BADPRE", OME_UINT16, NULL, &entrySmartAudioStatBadpre }, + { "BADLEN", OME_UINT16, NULL, &entrySmartAudioStatBadlen }, + { "CRCERR", OME_UINT16, NULL, &entrySmartAudioStatCrcerr }, + { "OOOERR", OME_UINT16, NULL, &entrySmartAudioStatOooerr }, + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + +OSD_Entry menu_vtx[] = +{ + { "-- VTX SMARTAUDIO --", OME_Label, NULL, NULL }, + { smartAudioStatusString, OME_Label, NULL, NULL }, + { "TXMODE", OME_TAB, smartAudioSetTxModeByGvar, &entrySmartAudioTxMode }, + { "BAND", OME_TAB, smartAudioConfigureBandByGvar, &entrySmartAudioBand }, + { "CHAN", OME_TAB, smartAudioConfigureChanByGvar, &entrySmartAudioChan }, + { "FREQ", OME_UINT16, NULL, &entrySmartAudioFreq }, + { "POWER", OME_TAB, smartAudioConfigurePowerByGvar, &entrySmartAudioPower }, + { "CONFIG", OME_Submenu, osdChangeScreen, &menu_smartAudioConfig[0] }, + { "STAT", OME_Submenu, osdChangeScreen, &menu_smartAudioStats[0] }, + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; +#endif // VTX_SMARTAUDIO + +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}; +======= + if (isAirmodeActive()) + p = "AIR"; +>>>>>>> bfdev-osd-cms-separation-poc + + if (FLIGHT_MODE(FAILSAFE_MODE)) + p = "!FS"; + else if (FLIGHT_MODE(ANGLE_MODE)) + p = "STAB"; + else if (FLIGHT_MODE(HORIZON_MODE)) + p = "HOR"; + + max7456Write(elemPosX, elemPosY, p); + return; + } + + case OSD_CRAFT_NAME: + { + if (strlen(masterConfig.name) == 0) + strcpy(buff, "CRAFT_NAME"); + else { + for (uint8_t i = 0; i < MAX_NAME_LENGTH; i++) { + buff[i] = toupper((unsigned char)masterConfig.name[i]); + if (masterConfig.name[i] == 0) + break; + } + } + + break; + } + + case OSD_THROTTLE_POS: + { + buff[0] = SYM_THR; + buff[1] = SYM_THR1; + sprintf(buff + 2, "%d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + break; + } + +#ifdef VTX + case OSD_VTX_CHANNEL: + { + sprintf(buff, "CH:%d", current_vtx_channel % CHANNELS_PER_BAND + 1); + break; + } +#endif // VTX + + case OSD_CROSSHAIRS: + { + uint8_t *screenBuffer = max7456GetScreenBuffer(); + uint16_t position = 194; + + if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) + position += 30; + + screenBuffer[position - 1] = (SYM_AH_CENTER_LINE); + screenBuffer[position + 1] = (SYM_AH_CENTER_LINE_RIGHT); + screenBuffer[position] = (SYM_AH_CENTER); + + return; + } + + case OSD_ARTIFICIAL_HORIZON: + { + uint8_t *screenBuffer = max7456GetScreenBuffer(); + uint16_t position = 194; + + int rollAngle = attitude.values.roll; + int pitchAngle = attitude.values.pitch; + + if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) + position += 30; + + if (pitchAngle > AH_MAX_PITCH) + pitchAngle = AH_MAX_PITCH; + if (pitchAngle < -AH_MAX_PITCH) + pitchAngle = -AH_MAX_PITCH; + if (rollAngle > AH_MAX_ROLL) + rollAngle = AH_MAX_ROLL; + if (rollAngle < -AH_MAX_ROLL) + rollAngle = -AH_MAX_ROLL; + + for (uint8_t x = 0; x <= 8; x++) { + int y = (rollAngle * (4 - x)) / 64; + y -= pitchAngle / 8; + y += 41; + if (y >= 0 && y <= 81) { + uint16_t pos = position - 7 + LINE * (y / 9) + 3 - 4 * LINE + x; + screenBuffer[pos] = (SYM_AH_BAR9_0 + (y % 9)); + } + } + + osdDrawSingleElement(OSD_HORIZON_SIDEBARS); + + return; + } + + case OSD_HORIZON_SIDEBARS: + { + uint8_t *screenBuffer = max7456GetScreenBuffer(); + uint16_t position = 194; + + if (maxScreenSize == VIDEO_BUFFER_CHARS_PAL) + position += 30; + + // Draw AH sides + int8_t hudwidth = AH_SIDEBAR_WIDTH_POS; + int8_t hudheight = AH_SIDEBAR_HEIGHT_POS; + for (int8_t x = -hudheight; x <= hudheight; x++) { + screenBuffer[position - hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + screenBuffer[position + hudwidth + (x * LINE)] = (SYM_AH_DECORATION); + } + + // AH level indicators + screenBuffer[position - hudwidth + 1] = (SYM_AH_LEFT); + screenBuffer[position + hudwidth - 1] = (SYM_AH_RIGHT); + + return; + } + + default: + return; + } + + max7456Write(elemPosX, elemPosY, buff); +} + +void resetOsdConfig(osd_profile_t *osdProfile) +{ + osdProfile->item_pos[OSD_RSSI_VALUE] = OSD_POS(22, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_ONTIME] = OSD_POS(22, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYTIME] = OSD_POS(22, 12) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_FLYMODE] = OSD_POS(12, 11) | VISIBLE_FLAG; + osdProfile->item_pos[OSD_CRAFT_NAME] = OSD_POS(12, 12); + osdProfile->item_pos[OSD_THROTTLE_POS] = OSD_POS(1, 4); + osdProfile->item_pos[OSD_VTX_CHANNEL] = OSD_POS(8, 6); + osdProfile->item_pos[OSD_CURRENT_DRAW] = OSD_POS(1, 3); + osdProfile->item_pos[OSD_MAH_DRAWN] = OSD_POS(15, 3); + osdProfile->item_pos[OSD_GPS_SPEED] = OSD_POS(2, 2); + osdProfile->item_pos[OSD_GPS_SATS] = OSD_POS(2, 12); + osdProfile->item_pos[OSD_ALTITUDE] = OSD_POS(1, 5); + + osdProfile->rssi_alarm = 20; + osdProfile->cap_alarm = 2200; + osdProfile->time_alarm = 10; // in minutes + osdProfile->alt_alarm = 100; // meters or feet depend on configuration + + osdProfile->video_system = 0; +} + +void osdInit(void) +{ + char x, string_buffer[30]; + + armState = ARMING_FLAG(ARMED); + + max7456Init(masterConfig.osdProfile.video_system); + + max7456ClearScreen(); + + // display logo and help + x = 160; + for (int i = 1; i < 5; i++) { + for (int j = 3; j < 27; j++) { + if (x != 255) + max7456WriteChar(j, i, x++); + } + } + + sprintf(string_buffer, "BF VERSION: %s", FC_VERSION_STRING); + max7456Write(5, 6, string_buffer); + max7456Write(7, 7, STARTUP_HELP_TEXT1); + max7456Write(11, 8, STARTUP_HELP_TEXT2); + max7456Write(11, 9, STARTUP_HELP_TEXT3); + + max7456RefreshAll(); + + refreshTimeout = 4 * REFRESH_1S; + +#ifdef CMS + cmsDeviceRegister(osdCmsInit); +#endif +} + +/** + * Gets the correct altitude symbol for the current unit system + */ +char osdGetAltitudeSymbol() +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return 0xF; + default: + return 0xC; + } +} + +/** + * Converts altitude based on the current unit system. + * @param alt Raw altitude (i.e. as taken from BaroAlt) + */ +int32_t osdGetAltitude(int32_t alt) +{ + switch (masterConfig.osdProfile.units) { + case OSD_UNIT_IMPERIAL: + return (alt * 328) / 100; // Convert to feet / 100 + default: + return alt; // Already in metre / 100 + } +} + +void osdUpdateAlarms(void) +{ + int32_t alt = osdGetAltitude(BaroAlt) / 100; + statRssi = rssi * 100 / 1024; + + if (statRssi < masterConfig.osdProfile.rssi_alarm) + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + + if (vbat <= (batteryWarningVoltage - 1)) + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + + if (STATE(GPS_FIX) == 0) + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + + if (flyTime / 60 >= masterConfig.osdProfile.time_alarm && ARMING_FLAG(ARMED)) + masterConfig.osdProfile.item_pos[OSD_FLYTIME] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + + if (mAhDrawn >= masterConfig.osdProfile.cap_alarm) + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; + + if (alt >= masterConfig.osdProfile.alt_alarm) + masterConfig.osdProfile.item_pos[OSD_ALTITUDE] |= BLINK_FLAG; + else + masterConfig.osdProfile.item_pos[OSD_ALTITUDE] &= ~BLINK_FLAG; +} + +void osdResetAlarms(void) +{ + masterConfig.osdProfile.item_pos[OSD_RSSI_VALUE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAIN_BATT_VOLTAGE] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_GPS_SATS] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_FLYTIME] &= ~BLINK_FLAG; + masterConfig.osdProfile.item_pos[OSD_MAH_DRAWN] &= ~BLINK_FLAG; +} + +void osdResetStats(void) +{ + stats.max_current = 0; + stats.max_speed = 0; + stats.min_voltage = 500; + stats.max_current = 0; + stats.min_rssi = 99; + stats.max_altitude = 0; +} + +void osdUpdateStats(void) +{ + int16_t value; + + value = GPS_speed * 36 / 1000; + if (stats.max_speed < value) + stats.max_speed = value; + + if (stats.min_voltage > vbat) + stats.min_voltage = vbat; + + value = amperage / 100; + if (stats.max_current < value) + stats.max_current = value; + + if (stats.min_rssi > statRssi) + stats.min_rssi = statRssi; + + if (stats.max_altitude < BaroAlt) + stats.max_altitude = BaroAlt; +} + +void osdShowStats(void) +{ + uint8_t top = 2; + char buff[10]; + + max7456ClearScreen(); + max7456Write(2, top++, " --- STATS ---"); + + if (STATE(GPS_FIX)) { + max7456Write(2, top, "MAX SPEED :"); + itoa(stats.max_speed, buff, 10); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MIN BATTERY :"); + sprintf(buff, "%d.%1dV", stats.min_voltage / 10, stats.min_voltage % 10); + max7456Write(22, top++, buff); + + max7456Write(2, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + max7456Write(22, top++, buff); + + if (feature(FEATURE_CURRENT_METER)) { + max7456Write(2, top, "MAX CURRENT :"); + itoa(stats.max_current, buff, 10); + strcat(buff, "A"); + max7456Write(22, top++, buff); + + max7456Write(2, top, "USED MAH :"); + itoa(mAhDrawn, buff, 10); + strcat(buff, "\x07"); + max7456Write(22, top++, buff); + } + + max7456Write(2, top, "MAX ALTITUDE :"); + int32_t alt = osdGetAltitude(stats.max_altitude); + sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol()); + max7456Write(22, top++, buff); + + refreshTimeout = 60 * REFRESH_1S; +} + +// called when motors armed +void osdArmMotors(void) +{ + max7456ClearScreen(); + max7456Write(12, 7, "ARMED"); + refreshTimeout = REFRESH_1S / 2; + osdResetStats(); +} + +void updateOsd(uint32_t currentTime) +{ + static uint32_t counter = 0; +#ifdef MAX7456_DMA_CHANNEL_TX + // don't touch buffers if DMA transaction is in progress + if (max7456DmaInProgres()) + return; +#endif // MAX7456_DMA_CHANNEL_TX + + // redraw values in buffer + if (counter++ % 5 == 0) + osdUpdate(currentTime); + else // rest of time redraw screen 10 chars per idle to don't lock the main idle + max7456DrawScreen(); + + // do not allow ARM if we are in menu + if (osdInMenu) + DISABLE_ARMING_FLAG(OK_TO_ARM); +} + +void osdUpdate(uint32_t currentTime) +{ + static uint8_t lastSec = 0; + uint8_t sec; + +#ifdef OSD_CALLS_CMS + // detect enter to menu + if (IS_MID(THROTTLE) && IS_HI(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) { + cmsOpenMenu(); + } +#endif + + // detect arm/disarm + if (armState != ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) + osdArmMotors(); // reset statistic etc + else + osdShowStats(); // show statistic + + armState = ARMING_FLAG(ARMED); + } + + osdUpdateStats(); + + sec = currentTime / 1000000; + + if (ARMING_FLAG(ARMED) && sec != lastSec) { + flyTime++; + lastSec = sec; + } + + if (refreshTimeout) { + if (IS_HI(THROTTLE) || IS_HI(PITCH)) // hide statistics + refreshTimeout = 1; + refreshTimeout--; + if (!refreshTimeout) + max7456ClearScreen(); + return; + } + + blinkState = (millis() / 200) % 2; + + if (!osdInMenu) { + osdUpdateAlarms(); + osdDrawElements(); +#ifdef OSD_CALLS_CMS + } else { + cmsUpdate(currentTime); +#endif + } +} + +// +// OSD specific CMS functions +// + +uint8_t shiftdown; + +int osdMenuBegin(void) +{ + osdResetAlarms(); + osdInMenu = true; + refreshTimeout = 0; + + return 0; +} + +int osdMenuEnd(void) +{ + osdInMenu = false; + + return 0; +} + +int osdClearScreen(void) +{ + max7456ClearScreen(); + + return 0; +} + +int osdWrite(uint8_t x, uint8_t y, char *s) +{ + max7456Write(x, y + shiftdown, s); + + return 0; +} + +#ifdef EDIT_ELEMENT_SUPPORT +void osdEditElement(void *ptr) +{ + uint32_t address = (uint32_t)ptr; + + // zsave position on menu stack + menuStack[menuStackIdx] = currentMenu; + menuStackHistory[menuStackIdx] = currentMenuPos; + menuStackIdx++; + + currentElement = (uint16_t *)address; + + *currentElement |= BLINK_FLAG; + max7456ClearScreen(); +} + +<<<<<<< HEAD +void osdExitMenu(void *ptr) +{ + max7456ClearScreen(); + max7456Write(5, 3, "RESTARTING IMU..."); + max7456RefreshAll(); + stopMotors(); + stopPwmAllMotors(); + delay(200); + + if (ptr) { + // save local variables to configuration + if (featureBlackbox) + featureSet(FEATURE_BLACKBOX); + else + featureClear(FEATURE_BLACKBOX); + + if (featureLedstrip) + featureSet(FEATURE_LED_STRIP); + else + featureClear(FEATURE_LED_STRIP); +#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) + if (featureVtx) + featureSet(FEATURE_VTX); + else + featureClear(FEATURE_VTX); +#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO + +#ifdef VTX + masterConfig.vtxBand = vtxBand; + masterConfig.vtx_channel = vtxChannel - 1; +#endif // VTX + +#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) + masterConfig.vtx_channel = vtxBand * 8 + vtxChannel - 1; +#endif // USE_RTC6705 || VTX_SMARTAUDIO + + saveConfigAndNotify(); + } + + systemReset(); +} + +void osdOpenMenu(void) +{ + if (inMenu) + return; + + if (feature(FEATURE_LED_STRIP)) + featureLedstrip = 1; + + if (feature(FEATURE_BLACKBOX)) + featureBlackbox = 1; + +#if defined(VTX) || defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) + if (feature(FEATURE_VTX)) + featureVtx = 1; +#endif // VTX || USE_RTC6705 || VTX_SMARTAUDIO + +#ifdef VTX + vtxBand = masterConfig.vtxBand; + vtxChannel = masterConfig.vtx_channel + 1; +#endif // VTX + +#if defined(USE_RTC6705) || defined(VTX_SMARTAUDIO) + vtxBand = masterConfig.vtx_channel / 8; + vtxChannel = masterConfig.vtx_channel % 8 + 1; +#endif // USE_RTC6705 + + osdRows = max7456GetRowsCount(); + inMenu = true; + refreshTimeout = 0; + max7456ClearScreen(); + currentMenu = &menuMain[0]; + osdResetAlarms(); + osdChangeScreen(currentMenu); +#ifdef LED_STRIP + getLedColor(); +#endif // LED_STRIP +} + +======= +>>>>>>> bfdev-osd-cms-separation-poc +void osdDrawElementPositioningHelp(void) +{ + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), "--- HELP --- "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 1, "USE ROLL/PITCH"); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 2, "TO MOVE ELEM. "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 3, " "); + max7456Write(OSD_X(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]), OSD_Y(masterConfig.osdProfile.item_pos[OSD_ARTIFICIAL_HORIZON]) + 4, "YAW - EXIT "); +} +#endif + +screenFnVTable_t osdVTable = { + osdMenuBegin, + osdMenuEnd, + osdClearScreen, + osdWrite, + NULL, + max7456RefreshAll, +}; + +void osdCmsInit(displayPort_t *pPort) +{ + shiftdown = masterConfig.osdProfile.row_shiftdown; + pPort->rows = max7456GetRowsCount() - shiftdown; + pPort->cols = 30; + pPort->buftime = 1; // Very fast + pPort->bufsize = 50000; // Very large + pPort->VTable = &osdVTable; +} +#endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7b5eae891f..24cf3ad99b 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -17,8 +17,6 @@ #pragma once -#include - typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -54,6 +52,8 @@ typedef struct { uint16_t alt_alarm; uint8_t video_system; + uint8_t row_shiftdown; + osd_unit_t units; } osd_profile_t; @@ -68,3 +68,18 @@ typedef struct { void updateOsd(uint32_t currentTime); void osdInit(void); void resetOsdConfig(osd_profile_t *osdProfile); + +#ifdef CMS +void osdCmsInit(displayPort_t *); +#endif + +// Character coordinate and attributes + +#define OSD_POS(x,y) (x | (y << 5)) +#define OSD_X(x) (x & 0x001F) +#define OSD_Y(x) ((x >> 5) & 0x001F) +#define VISIBLE_FLAG 0x0800 +#define BLINK_FLAG 0x0400 +#define VISIBLE(x) (x & VISIBLE_FLAG) +#define BLINK(x) ((x & BLINK_FLAG) && blinkState) +#define BLINK_OFF(x) (x & ~BLINK_FLAG) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a3b1a7a5d7..f6d2f92665 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -70,6 +70,7 @@ uint8_t cliMode = 0; #include "io/flashfs.h" #include "io/beeper.h" #include "io/asyncfatfs/asyncfatfs.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" @@ -225,7 +226,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - " ", "VTX", "RX_SPI", "SOFTSPI", NULL + " ", "VTX", "RX_SPI", "SOFTSPI", "CANVAS", NULL }; // sync this with rxFailsafeChannelMode_e @@ -946,6 +947,7 @@ const clivalue_t valueTable[] = { #ifdef OSD { "osd_video_system", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.video_system, .config.minmax = { 0, 2 } }, + { "osd_row_shiftdown", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.row_shiftdown, .config.minmax = { 0, 1 } }, { "osd_units", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.osdProfile.units, .config.lookup = { TABLE_UNIT } }, { "osd_rssi_alarm", VAR_UINT8 | MASTER_VALUE, &masterConfig.osdProfile.rssi_alarm, .config.minmax = { 0, 100 } }, diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index 786a411d4a..b58ed5befe 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -10,6 +10,8 @@ #include "drivers/system.h" #include "drivers/serial.h" #include "io/serial.h" +#include "io/cms.h" +#include "io/cms_types.h" #include "io/vtx_smartaudio.h" #include "fc/rc_controls.h" @@ -40,7 +42,7 @@ serialPort_t *debugSerialPort = NULL; #include "build/debug.h" -#ifdef OSD +#ifdef CMS static void smartAudioUpdateStatusString(void); // Forward #endif @@ -289,7 +291,7 @@ static void saProcessResponse(uint8_t *buf, int len) // Debug smartAudioPrintSettings(); - // Export current settings for BFOSD 3.0.0 + // Export current settings for CMS smartAudioBand = (sa_chan / 8) + 1; smartAudioChan = (sa_chan % 8) + 1; @@ -635,48 +637,6 @@ bool smartAudioInit() return true; } - -void smartAudioConfigurePitFModeByGvar(void *opaque) -{ - UNUSED(opaque); - - if (smartAudioPitFMode == 0) { - saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); - } else { - saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); - } -} - -void smartAudioConfigureOpModelByGvar(void *opaque) -{ - UNUSED(opaque); - - masterConfig.vtx_smartaudio_opmodel = smartAudioOpModel; - - if (smartAudioOpModel == SA_OPMODEL_FREE) { - // VTX should power up transmitting. - // Turn off In-Range and Out-Range bits - saSetMode(0); - } else { - // VTX should power up in pit mode. - // Setup In-Range or Out-Range bits - smartAudioConfigurePitFModeByGvar(opaque); - } -} - -#if 0 -void -{ - static int turn = 0; - - if ((turn++ % 2) == 0) { - saSetMode(SA_MODE_SET_UNLOCK|SA_MODE_SET_PITMODE|SA_MODE_SET_IN_RANGE_PITMODE); - } else { - saSetMode(SA_MODE_SET_UNLOCK|SA_MODE_CLR_PITMODE); - } -} -#endif - void smartAudioProcess(uint32_t now) { static bool initialSent = false; @@ -724,9 +684,7 @@ void smartAudioProcess(uint32_t now) } } -#ifdef OSD -// API for BFOSD3.0 - +#ifdef CMS char smartAudioStatusString[31] = "- - ---- --- ---- -"; uint8_t smartAudioBand = 0; @@ -751,77 +709,87 @@ static void smartAudioUpdateStatusString(void) tfp_sprintf(&smartAudioStatusString[13], "%4s", "----"); } -void smartAudioConfigureBandByGvar(void *opaque) +long smartAudioConfigureBandByGvar(displayPort_t *pDisp, void *self) { - UNUSED(opaque); + UNUSED(pDisp); + UNUSED(self); if (sa_vers == 0) { // Bounce back; not online yet smartAudioBand = 0; - return; + return 0; } if (smartAudioBand == 0) { // Bouce back, no going back to undef state smartAudioBand = 1; - return; + return 0; } smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + + return 0; } -void smartAudioConfigureChanByGvar(void *opaque) +long smartAudioConfigureChanByGvar(displayPort_t *pDisp, void *self) { - UNUSED(opaque); + UNUSED(pDisp); + UNUSED(self); if (sa_vers == 0) { // Bounce back; not online yet smartAudioChan = 0; - return; + return 0; } if (smartAudioChan == 0) { // Bounce back; no going back to undef state smartAudioChan = 1; - return; + return 0; } smartAudioSetBandChan(smartAudioBand - 1, smartAudioChan - 1); + + return 0; } -void smartAudioConfigurePowerByGvar(void *opaque) +long smartAudioConfigurePowerByGvar(displayPort_t *pDisp, void *self) { - UNUSED(opaque); + UNUSED(pDisp); + UNUSED(self); if (sa_vers == 0) { // Bounce back; not online yet smartAudioPower = 0; - return; + return 0; } if (smartAudioPower == 0) { // Bouce back; no going back to undef state smartAudioPower = 1; - return; + return 0; } smartAudioSetPowerByIndex(smartAudioPower - 1); + + return 0; } -void smartAudioSetTxModeByGvar(void *opaque) +long smartAudioSetTxModeByGvar(displayPort_t *pDisp, void *self) { - UNUSED(opaque); + UNUSED(pDisp); + UNUSED(self); if (sa_vers != 2) { // Bounce back; not online yet or can't handle mode (V1) smartAudioTxMode = SA_TXMODE_NODEF; - return; + return 0; } if (smartAudioTxMode == 0) { // Bouce back; no going back to undef state ++smartAudioTxMode; - return; + return 0; } if (smartAudioTxMode == SA_TXMODE_ACTIVE) { @@ -839,6 +807,42 @@ void smartAudioSetTxModeByGvar(void *opaque) smartAudioTxMode = SA_TXMODE_ACTIVE; } } + + return 0; } -#endif // OSD + +long smartAudioConfigurePitFModeByGvar(displayPort_t *pDisp, void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (smartAudioPitFMode == 0) { + saSetMode(SA_MODE_SET_IN_RANGE_PITMODE); + } else { + saSetMode(SA_MODE_SET_OUT_RANGE_PITMODE); + } + + return 0; +} + +long smartAudioConfigureOpModelByGvar(displayPort_t *pDisp, void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + masterConfig.vtx_smartaudio_opmodel = smartAudioOpModel; + + if (smartAudioOpModel == SA_OPMODEL_FREE) { + // VTX should power up transmitting. + // Turn off In-Range and Out-Range bits + saSetMode(0); + } else { + // VTX should power up in pit mode. + // Setup In-Range or Out-Range bits + smartAudioConfigurePitFModeByGvar(pDisp, self); + } + + return 0; +} +#endif // CMS #endif // VTX_SMARTAUDIO diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h index b146ca1b39..10aa1e303c 100644 --- a/src/main/io/vtx_smartaudio.h +++ b/src/main/io/vtx_smartaudio.h @@ -27,10 +27,10 @@ uint8_t smartAudioTxMode; uint8_t smartAudioPitFMode; uint16_t smartAudioORFreq; -void smartAudioConfigureOpModelByGvar(void *); -void smartAudioConfigurePitFModeByGvar(void *); -void smartAudioConfigureBandByGvar(void *); -void smartAudioConfigureChanByGvar(void *); -void smartAudioConfigurePowerByGvar(void *); -void smartAudioSetTxModeByGvar(void *); +long smartAudioConfigureOpModelByGvar(displayPort_t *, void *self); +long smartAudioConfigurePitFModeByGvar(displayPort_t *, void *self); +long smartAudioConfigureBandByGvar(displayPort_t *, void *self); +long smartAudioConfigureChanByGvar(displayPort_t *, void *self); +long smartAudioConfigurePowerByGvar(displayPort_t *, void *self); +long smartAudioSetTxModeByGvar(displayPort_t *, void *self); #endif diff --git a/src/main/main.c b/src/main/main.c index 689787a51e..cf322a4c20 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -74,6 +74,8 @@ #include "rx/rx.h" #include "rx/spektrum.h" +#include "io/cms.h" + #include "io/beeper.h" #include "io/serial.h" #include "io/flashfs.h" @@ -89,6 +91,7 @@ #include "io/osd.h" #include "io/vtx.h" #include "io/vtx_smartaudio.h" +#include "io/canvas.h" #include "scheduler/scheduler.h" @@ -396,6 +399,10 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); +#ifdef CMS + cmsInit(); +#endif + #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); @@ -455,6 +462,12 @@ void init(void) mspFcInit(); mspSerialInit(); +#ifdef CANVAS + if (feature(FEATURE_CANVAS)) { + canvasInit(); + } +#endif + #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif diff --git a/src/main/main.c.orig b/src/main/main.c.orig index cb217f9568..37dda56ec0 100644 --- a/src/main/main.c.orig +++ b/src/main/main.c.orig @@ -74,6 +74,8 @@ #include "rx/rx.h" #include "rx/spektrum.h" +#include "io/cms.h" + #include "io/beeper.h" #include "io/serial.h" #include "io/flashfs.h" @@ -86,9 +88,14 @@ #include "io/asyncfatfs/asyncfatfs.h" #include "io/serial_cli.h" #include "io/transponder_ir.h" +#include "io/cms.h" #include "io/osd.h" #include "io/vtx.h" +<<<<<<< HEAD #include "io/vtx_smartaudio.h" +======= +#include "io/canvas.h" +>>>>>>> bfdev-osd-cms-separation-poc #include "scheduler/scheduler.h" @@ -396,6 +403,10 @@ void init(void) initBoardAlignment(&masterConfig.boardAlignment); +#ifdef CMS + cmsInit(); +#endif + #ifdef DISPLAY if (feature(FEATURE_DISPLAY)) { displayInit(&masterConfig.rxConfig); @@ -455,6 +466,12 @@ void init(void) mspFcInit(); mspSerialInit(); +#ifdef CANVAS + if (feature(FEATURE_CANVAS)) { + canvasInit(); + } +#endif + #ifdef USE_CLI cliInit(&masterConfig.serialConfig); #endif @@ -627,73 +644,6 @@ void processLoopback(void) { #define processLoopback() #endif -<<<<<<< HEAD -void main_init(void) -{ - init(); - - /* Setup scheduler */ - schedulerInit(); - rescheduleTask(TASK_GYROPID, gyro.targetLooptime); - setTaskEnabled(TASK_GYROPID, true); - - if (sensors(SENSOR_ACC)) { - setTaskEnabled(TASK_ACCEL, true); - rescheduleTask(TASK_ACCEL, accSamplingInterval); - } - - setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC)); - setTaskEnabled(TASK_SERIAL, true); -#ifdef BEEPER - setTaskEnabled(TASK_BEEPER, true); -#endif - setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_CURRENT_METER)); - setTaskEnabled(TASK_RX, true); -#ifdef GPS - setTaskEnabled(TASK_GPS, feature(FEATURE_GPS)); -#endif -#ifdef MAG - setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); -#if defined(USE_SPI) && defined(USE_MAG_AK8963) - // fixme temporary solution for AK6983 via slave I2C on MPU9250 - rescheduleTask(TASK_COMPASS, 1000000 / 40); -#endif -#endif -#ifdef BARO - setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); -#endif -#ifdef SONAR - setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); -#endif -#if defined(BARO) || defined(SONAR) - setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); -#endif -#ifdef DISPLAY - setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY)); -#endif -#ifdef TELEMETRY - setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); - // Reschedule telemetry to 500hz for Jeti Exbus - if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) rescheduleTask(TASK_TELEMETRY, 2000); -#endif -#ifdef LED_STRIP - setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP)); -#endif -#ifdef TRANSPONDER - setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER)); -#endif -#ifdef OSD - setTaskEnabled(TASK_OSD, feature(FEATURE_OSD)); -#endif -#ifdef USE_BST - setTaskEnabled(TASK_BST_MASTER_PROCESS, true); -#endif -#ifdef VTX_CONTROL - setTaskEnabled(TASK_VTX_CONTROL, true); -#endif -} -======= ->>>>>>> betaflight/master void main_step(void) { diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index b322ee7df6..eaca6e2868 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -35,3 +35,4 @@ typedef struct mspPacket_s { struct serialPort_s; typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn); +typedef void (*mspPushCommandFnPtr)(mspPacket_t *push, uint8_t *, int); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 8d14b4d698..a0fee2efb7 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -216,6 +216,9 @@ #define MSP_OSD_VIDEO_CONFIG 180 #define MSP_SET_OSD_VIDEO_CONFIG 181 +// External OSD canvas mode messages +#define MSP_CANVAS 182 + // // Multwii original MSP commands // diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index d3f68b125a..ef39598c96 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -211,3 +211,38 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } +mspPushCommandFnPtr mspPushCommandFn; + +void mspSerialPush(uint8_t cmd, uint8_t *data, int datalen) +{ + static uint8_t pushBuf[30]; + + mspPacket_t push = { + .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), }, + .cmd = cmd, + .result = 0, + }; + + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t * const mspPort = &mspPorts[portIndex]; + if (!mspPort->port) { + continue; + } + + // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) + if (mspPort->port->identifier == SERIAL_PORT_USB_VCP) { + continue; + } + + mspPushCommandFn(&push, data, datalen); + + sbufSwitchToReader(&push.buf, pushBuf); + + mspSerialEncode(mspPort, &push); + } +} + +void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFnToUse) +{ + mspPushCommandFn = mspPushCommandFnToUse; +} diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index f2f071464b..5d92114736 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -66,3 +66,5 @@ void mspSerialInit(void); void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn); void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); +void mspSerialPushInit(mspPushCommandFnPtr mspPushCommandFn); +void mspSerialPush(uint8_t, uint8_t *, int); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 7bb6543a02..5ec6cb9330 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -85,6 +85,9 @@ typedef enum { #ifdef USE_BST TASK_BST_MASTER_PROCESS, #endif +#ifdef CMS + TASK_CMS, +#endif #ifdef VTX_CONTROL TASK_VTXCTRL, #endif diff --git a/src/main/scheduler/scheduler.h.orig b/src/main/scheduler/scheduler.h.orig new file mode 100644 index 0000000000..e080a9eb6d --- /dev/null +++ b/src/main/scheduler/scheduler.h.orig @@ -0,0 +1,142 @@ +/* + * 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 . + */ + +#pragma once + +//#define SCHEDULER_DEBUG + +typedef enum { + TASK_PRIORITY_IDLE = 0, // Disables dynamic scheduling, task is executed only if no other task is active this cycle + TASK_PRIORITY_LOW = 1, + TASK_PRIORITY_MEDIUM = 3, + TASK_PRIORITY_HIGH = 5, + TASK_PRIORITY_REALTIME = 6, + TASK_PRIORITY_MAX = 255 +} cfTaskPriority_e; + +typedef struct { + const char * taskName; + const char * subTaskName; + bool isEnabled; + uint32_t desiredPeriod; + uint8_t staticPriority; + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; + uint32_t averageExecutionTime; + uint32_t latestDeltaTime; +} cfTaskInfo_t; + +typedef enum { + /* Actual tasks */ + TASK_SYSTEM = 0, + TASK_GYROPID, + TASK_ACCEL, + TASK_ATTITUDE, + TASK_RX, + TASK_SERIAL, + TASK_BATTERY, +#ifdef BEEPER + TASK_BEEPER, +#endif +#ifdef GPS + TASK_GPS, +#endif +#ifdef MAG + TASK_COMPASS, +#endif +#ifdef BARO + TASK_BARO, +#endif +#ifdef SONAR + TASK_SONAR, +#endif +#if defined(BARO) || defined(SONAR) + TASK_ALTITUDE, +#endif +#ifdef DISPLAY + TASK_DISPLAY, +#endif +#ifdef TELEMETRY + TASK_TELEMETRY, +#endif +#ifdef LED_STRIP + TASK_LEDSTRIP, +#endif +#ifdef TRANSPONDER + TASK_TRANSPONDER, +#endif +#ifdef OSD + TASK_OSD, +#endif +#ifdef USE_BST + TASK_BST_MASTER_PROCESS, +#endif +<<<<<<< HEAD +#ifdef VTX_CONTROL + TASK_VTXCTRL, +======= +#ifdef CMS + TASK_CMS, +>>>>>>> bfdev-osd-cms-separation-poc +#endif + + /* Count of real tasks */ + TASK_COUNT, + + /* Service task IDs */ + TASK_NONE = TASK_COUNT, + TASK_SELF +} cfTaskId_e; + +typedef struct { + /* Configuration */ + const char * taskName; + const char * subTaskName; + bool (*checkFunc)(uint32_t currentTime, uint32_t currentDeltaTime); + void (*taskFunc)(uint32_t currentTime); + uint32_t desiredPeriod; // target period of execution + const uint8_t staticPriority; // dynamicPriority grows in steps of this size, shouldn't be zero + + /* Scheduling */ + uint16_t dynamicPriority; // measurement of how old task was last executed, used to avoid task starvation + uint16_t taskAgeCycles; + uint32_t lastExecutedAt; // last time of invocation + uint32_t lastSignaledAt; // time of invocation event for event-driven tasks + + /* Statistics */ + uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval + uint32_t taskLatestDeltaTime; // +#ifndef SKIP_TASK_STATISTICS + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; // total time consumed by task since boot +#endif +} cfTask_t; + +extern cfTask_t cfTasks[TASK_COUNT]; +extern uint16_t averageSystemLoadPercent; + +void getTaskInfo(cfTaskId_e taskId, cfTaskInfo_t * taskInfo); +void rescheduleTask(cfTaskId_e taskId, uint32_t newPeriodMicros); +void setTaskEnabled(cfTaskId_e taskId, bool newEnabledState); +uint32_t getTaskDeltaTime(cfTaskId_e taskId); + +void schedulerInit(void); +void scheduler(void); + +#define LOAD_PERCENTAGE_ONE 100 + +#define isSystemOverloaded() (averageSystemLoadPercent >= LOAD_PERCENTAGE_ONE) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index aafc690657..d47158b8e6 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -1,3 +1,4 @@ +#define USE_DPRINTF /* * This file is part of Cleanflight. * @@ -91,10 +92,20 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS + +// USE I2C OLED display to run CMS +#define OLEDCMS + // OSD define info: // feature name (includes source) -> MAX_OSD, used in target.mk // include the osd code #define OSD + // include the max7456 driver #define USE_MAX7456 #define MAX7456_SPI_INSTANCE SPI1 diff --git a/src/main/target/OMNIBUS/target.mk b/src/main/target/OMNIBUS/target.mk index 7b0cf8cb8e..f16c836968 100644 --- a/src/main/target/OMNIBUS/target.mk +++ b/src/main/target/OMNIBUS/target.mk @@ -15,4 +15,6 @@ TARGET_SRC = \ io/transponder_ir.c \ drivers/max7456.c \ io/osd.c \ + io/cms.c \ + io/canvas.c \ io/vtx_smartaudio.c diff --git a/src/main/target/OMNIBUS/target.mk.orig b/src/main/target/OMNIBUS/target.mk.orig new file mode 100644 index 0000000000..3de6d6e1a0 --- /dev/null +++ b/src/main/target/OMNIBUS/target.mk.orig @@ -0,0 +1,23 @@ +F3_TARGETS += $(TARGET) +FEATURES = VCP SDCARD + +TARGET_SRC = \ + drivers/accgyro_mpu.c \ + drivers/accgyro_spi_mpu6000.c \ + drivers/barometer_bmp280.c \ + drivers/barometer_spi_bmp280.c \ + drivers/compass_ak8963.c \ + drivers/compass_ak8975.c \ + drivers/compass_hmc5883l.c \ + drivers/serial_usb_vcp.c \ + drivers/transponder_ir.c \ + drivers/transponder_ir_stm32f30x.c \ + io/transponder_ir.c \ + drivers/max7456.c \ + io/osd.c \ +<<<<<<< HEAD + io/vtx_smartaudio.c +======= + io/cms.c \ + io/canvas.c +>>>>>>> bfdev-osd-cms-separation-poc diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index d7137502d8..a1dd90dfaf 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -70,6 +70,9 @@ //#define MAX7456_DMA_CHANNEL_RX DMA1_Stream0 //#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_ST0_HANDLER +#define CMS +#define CANVAS + //#define PITOT //#define USE_PITOT_MS4525 //#define MS4525_BUS I2C_DEVICE_EXT diff --git a/src/main/target/OMNIBUSF4/target.mk b/src/main/target/OMNIBUSF4/target.mk index 18034c1332..c7cf0ad933 100644 --- a/src/main/target/OMNIBUSF4/target.mk +++ b/src/main/target/OMNIBUSF4/target.mk @@ -6,5 +6,7 @@ TARGET_SRC = \ drivers/barometer_ms5611.c \ drivers/compass_hmc5883l.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c \ + io/canvas.c diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 4d73c99355..eed70b40de 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -144,3 +144,6 @@ #define USABLE_TIMER_CHANNEL_COUNT 12 #define USED_TIMERS ( TIM_N(2) | TIM_N(3) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9) ) + +#define CMS +#define CANVAS diff --git a/src/main/target/REVO/target.mk b/src/main/target/REVO/target.mk index 2711b19dac..6f70ecbe7e 100644 --- a/src/main/target/REVO/target.mk +++ b/src/main/target/REVO/target.mk @@ -4,4 +4,6 @@ FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ drivers/barometer_ms5611.c \ - drivers/compass_hmc5883l.c + drivers/compass_hmc5883l.c \ + io/cms.c \ + io/canvas.c diff --git a/src/main/target/SIRINFPV/target.h b/src/main/target/SIRINFPV/target.h index 8062644aa9..9a9f16e331 100644 --- a/src/main/target/SIRINFPV/target.h +++ b/src/main/target/SIRINFPV/target.h @@ -139,6 +139,15 @@ #define OSD +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS + +// USE I2C OLED display to run CMS +#define OLEDCMS + #define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT #define USE_SERVOS diff --git a/src/main/target/SIRINFPV/target.mk b/src/main/target/SIRINFPV/target.mk index eac7fbb6ec..6481f94182 100644 --- a/src/main/target/SIRINFPV/target.mk +++ b/src/main/target/SIRINFPV/target.mk @@ -12,4 +12,6 @@ TARGET_SRC = \ drivers/flash_m25p16.c \ drivers/vtx_soft_spi_rtc6705.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c \ + io/canvas.c diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 207a7a2dd2..7a53ebdbcd 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -140,3 +140,11 @@ #define USABLE_TIMER_CHANNEL_COUNT 17 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(15) | TIM_N(16) | TIM_N(17) ) +// Configuratoin Menu System +#define CMS + +// Use external OSD to run CMS +#define CANVAS + +// USE I2C OLED display to run CMS +#define OLEDCMS diff --git a/src/main/target/SPRACINGF3/target.mk b/src/main/target/SPRACINGF3/target.mk index 5b3a330295..dd7702375e 100644 --- a/src/main/target/SPRACINGF3/target.mk +++ b/src/main/target/SPRACINGF3/target.mk @@ -8,5 +8,7 @@ TARGET_SRC = \ drivers/barometer_bmp085.c \ drivers/barometer_bmp280.c \ drivers/compass_ak8975.c \ - drivers/compass_hmc5883l.c + drivers/compass_hmc5883l.c \ + io/cms.c \ + io/canvas.c diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 766447a09e..0f82fef749 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -120,6 +120,8 @@ #define MAX7456_SPI_INSTANCE SPI2 #define MAX7456_SPI_CS_PIN SPI2_NSS_PIN +#define CMS + //#define USE_SDCARD //#define USE_SDCARD_SPI2 // diff --git a/src/main/target/STM32F3DISCOVERY/target.mk b/src/main/target/STM32F3DISCOVERY/target.mk index 971ddf04a2..6b718b5b0a 100644 --- a/src/main/target/STM32F3DISCOVERY/target.mk +++ b/src/main/target/STM32F3DISCOVERY/target.mk @@ -26,4 +26,5 @@ TARGET_SRC = \ drivers/compass_hmc5883l.c \ drivers/flash_m25p16.c \ drivers/max7456.c \ - io/osd.c + io/osd.c \ + io/cms.c diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 15030e1517..b4cd33fb02 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -61,8 +61,6 @@ #include "io/gps.h" #include "io/ledstrip.h" #include "io/beeper.h" -#include "io/osd.h" -#include "io/vtx.h" #include "rx/rx.h" diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 4be785d479..c54f0f3c2a 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -28,9 +28,6 @@ #include "io/motors.h" #include "io/gps.h" #include "io/serial.h" -#include "io/ledstrip.h" -#include "io/osd.h" -#include "io/vtx.h" #include "sensors/boardalignment.h" #include "sensors/sensors.h"