Rebased onto current CMS dev

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"

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"
 

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);

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;
+}

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);

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",

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#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
+};

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"

src/main/io/canvas.c

@@ -0,0 +1,94 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <ctype.h>
+
+#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

src/main/io/canvas.h

@@ -0,0 +1,3 @@
+#pragma once
+void canvasInit(void);
+void canvasCmsInit(displayPort_t *);

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"
+

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;

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

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

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"

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"

src/main/io/osd.h

@@ -17,8 +17,6 @@
 
 #pragma once
 
-#include <stdint.h>
-
 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)

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 } },

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
+#ifdef CMS
-// API for BFOSD3.0
-
 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

src/main/io/vtx_smartaudio.h

@@ -27,10 +27,10 @@ uint8_t smartAudioTxMode;
 uint8_t smartAudioPitFMode;
 uint16_t smartAudioORFreq;
 
-void smartAudioConfigureOpModelByGvar(void *);
+long smartAudioConfigureOpModelByGvar(displayPort_t *, void *self);
-void smartAudioConfigurePitFModeByGvar(void *);
+long smartAudioConfigurePitFModeByGvar(displayPort_t *, void *self);
-void smartAudioConfigureBandByGvar(void *);
+long smartAudioConfigureBandByGvar(displayPort_t *, void *self);
-void smartAudioConfigureChanByGvar(void *);
+long smartAudioConfigureChanByGvar(displayPort_t *, void *self);
-void smartAudioConfigurePowerByGvar(void *);
+long smartAudioConfigurePowerByGvar(displayPort_t *, void *self);
-void smartAudioSetTxModeByGvar(void *);
+long smartAudioSetTxModeByGvar(displayPort_t *, void *self);
 #endif

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

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)
 {

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);

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
 //

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;
+}

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);

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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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)

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

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

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

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

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
 

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

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

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

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

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

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
 

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
 //

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

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"
 

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"