Configuration Menu System support for external OSD

A quick hack to support config menu on external OSD, consisiting of:

- CMS-OSD partial separation (CMS and OSD reside in a same file: osd.c)
- MSP message injection (take it as server-push in client-server model)

src/main/fc/fc_tasks.c

@@ -47,6 +47,7 @@
 #include "io/serial_msp.h"
 #include "io/serial_cli.h"
 #include "io/transponder_ir.h"
+#include "io/cms.h"
 
 #include "rx/rx.h"
 
@@ -331,6 +332,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
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -490,4 +495,13 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+
+#ifdef CMS
+    [TASK_CMS] = {
+        .taskName = "CMS",
+        .taskFunc = cmsHandler,
+        .desiredPeriod = 1000000 / 60,          // 60 Hz
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
 };

src/main/io/cms.h

@@ -0,0 +1 @@
+void cmsHandler(uint32_t);

src/main/io/serial_msp.c

@@ -84,6 +84,34 @@ void mspSerialInit(void)
     mspSerialAllocatePorts();
 }
 
+#ifdef USE_DPRINTF
+#include "common/printf.h"
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
+extern serialPort_t *debugSerialPort;
+#define dprintf(x) if (debugSerialPort) printf x
+#else
+#define dprintf(x)
+#endif
+
+void mspSerialPush(int cmd, uint8_t *data, int buflen)
+{
+    for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t * const mspPort = &mspPorts[portIndex];
+        if (!mspPort->port) {
+            continue;
+        }
+
+        // Big enough for a OSD line
+        uint8_t buf[sizeof(bufWriter_t) + 30];
+
+        writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
+
+        mspServerPush(mspPort, cmd, data, buflen);
+
+        bufWriterFlush(writer);
+    }
+}
+
 /*
  * Process MSP commands from serial ports configured as MSP ports.
  *

src/main/io/serial_msp.h

@@ -52,3 +52,4 @@ void mspSerialInit(void);
 void mspSerialProcess(void);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
+void mspSerialPush(int, uint8_t *, int);

src/main/main.c

@@ -126,6 +126,14 @@ extern uint8_t motorControlEnable;
 serialPort_t *loopbackPort;
 #endif
 
+#ifdef USE_DPRINTF
+#include "common/printf.h"
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
+serialPort_t *debugSerialPort = NULL;
+#define dprintf(x) if (debugSerialPort) printf x
+#else
+#define dprintf(x)
+#endif
 
 typedef enum {
     SYSTEM_STATE_INITIALISING   = 0,
@@ -244,6 +252,16 @@ void init(void)
     serialInit(&masterConfig.serialConfig, feature(FEATURE_SOFTSERIAL), SERIAL_PORT_NONE);
 #endif
 
+#ifdef USE_DPRINTF
+    // Setup debugSerialPort
+
+    debugSerialPort = openSerialPort(DPRINTF_SERIAL_PORT, FUNCTION_NONE, NULL, 115200, MODE_RXTX, 0);
+    if (debugSerialPort) {
+        setPrintfSerialPort(debugSerialPort);
+        dprintf(("debugSerialPort: OK\r\n"));
+    }
+#endif
+
     mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
 #ifdef USE_SERVOS
     servoMixerInit(masterConfig.customServoMixer);

src/main/msp/msp.h

@@ -23,3 +23,5 @@ typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process f
 void mspInit(void);
 bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c);
 mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort);
+
+void mspServerPush(mspPort_t *, int, 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_server_fc.c

@@ -100,6 +100,15 @@
 #include "config/config_master.h"
 #include "config/feature.h"
 
+#ifdef USE_DPRINTF
+#include "common/printf.h"
+#define DPRINTF_SERIAL_PORT SERIAL_PORT_USART3
+extern serialPort_t *debugSerialPort;
+#define dprintf(x) if (debugSerialPort) printf x
+#else
+#define dprintf(x)
+#endif
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -1968,3 +1977,17 @@ bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c)
     }
     return true;
 }
+
+void mspServerPush(mspPort_t *mspPort, int cmd, uint8_t *data, int len)
+{
+    currentPort = mspPort;
+    mspPort->cmdMSP = cmd;
+
+    headSerialReply(len);
+
+    while (len--) {
+        serialize8(*data++);
+    }
+
+    tailSerialReply();
+}

src/main/scheduler/scheduler.h

@@ -85,6 +85,9 @@ typedef enum {
 #ifdef USE_BST
     TASK_BST_MASTER_PROCESS,
 #endif
+#ifdef CMS
+    TASK_CMS,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,

src/main/target/OMNIBUS/target.h

@@ -1,3 +1,4 @@
+#define USE_DPRINTF
 /*
  * This file is part of Cleanflight.
  *
@@ -89,12 +90,19 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+// Configuratoin Menu System
+#define CMS
+
+// Use external OSD to run CMS
+#define CANVAS
+
 // OSD define info:
 //   feature name (includes source) -> MAX_OSD, used in target.mk
 // include the osd code
-#define OSD
+//#define OSD
+
 // include the max7456 driver
-#define USE_MAX7456
+//#define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI1
 #define MAX7456_SPI_CS_PIN      PB1
 #define MAX7456_SPI_CLK         (SPI_CLOCK_STANDARD*2)

src/main/target/SPRACINGF3/target.h

@@ -129,3 +129,8 @@
 #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

src/main/target/SPRACINGF3/target.mk

@@ -8,5 +8,6 @@ TARGET_SRC = \
             drivers/barometer_bmp085.c \
             drivers/barometer_bmp280.c \
             drivers/compass_ak8975.c \
-            drivers/compass_hmc5883l.c
+            drivers/compass_hmc5883l.c \
+            io/osd.c