diff --git a/Makefile b/Makefile
index 2ab2c34f09..ba216def8b 100644
--- a/Makefile
+++ b/Makefile
@@ -169,6 +169,7 @@ GROUP_4_TARGETS := \
SPRACINGF3EVO \
SPRACINGF3MINI \
SPRACINGF3NEO \
+ SPRACINGF3OSD \
SPRACINGF4EVO \
STM32F3DISCOVERY \
TINYBEEF3 \
@@ -760,6 +761,7 @@ COMMON_SRC = \
io/gps.c \
io/ledstrip.c \
io/osd.c \
+ io/osd_slave.c \
io/transponder_ir.c \
sensors/sonar.c \
sensors/barometer.c \
@@ -851,6 +853,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
io/dashboard.c \
io/displayport_max7456.c \
io/osd.c \
+ io/osd_slave.c
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/serial_escserial.c \
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index edfb2e8700..1429a2a6f8 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -27,7 +27,7 @@
#include "platform.h"
-#ifdef CMS
+#if defined(CMS) && defined(BLACKBOX)
#include "build/version.h"
diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c
index 1827655268..8a0cebc8b2 100644
--- a/src/main/cms/cms_menu_builtin.c
+++ b/src/main/cms/cms_menu_builtin.c
@@ -93,7 +93,10 @@ static CMS_Menu menuInfo = {
static OSD_Entry menuFeaturesEntries[] =
{
{"--- FEATURES ---", OME_Label, NULL, NULL, 0},
+
+#if defined(BLACKBOX)
{"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox, 0},
+#endif
#if defined(VTX) || defined(USE_RTC6705)
{"VTX", OME_Submenu, cmsMenuChange, &cmsx_menuVtx, 0},
#endif // VTX || USE_RTC6705
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index f4b4ad4577..f497cf1cab 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -112,6 +112,7 @@
#define PG_OSD_VIDEO_CONFIG 2046
#define PG_OSD_ELEMENT_CONFIG 2045
+
// 4095 is currently the highest number that can be used for a PGN due to the top 4 bits of the 16 bit value being reserved for the version when the PG is stored in an EEPROM.
#define PG_RESERVED_FOR_TESTING_1 4095
#define PG_RESERVED_FOR_TESTING_2 4094
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
old mode 100755
new mode 100644
index 032ffc1814..fddb404ac7
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -389,6 +389,8 @@ void max7456ReInit(void)
void max7456Init(const vcdProfile_t *pVcdProfile)
{
+ max7456HardwareReset();
+
#ifdef MAX7456_SPI_CS_PIN
max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
@@ -612,4 +614,25 @@ void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
max7456Lock = false;
}
+#ifdef MAX7456_NRST_PIN
+static IO_t max7456ResetPin = IO_NONE;
+#endif
+
+void max7456HardwareReset(void)
+{
+#ifdef MAX7456_NRST_PIN
+#define IO_RESET_CFG IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_2MHz, GPIO_OType_PP, GPIO_PuPd_DOWN)
+
+ max7456ResetPin = IOGetByTag(IO_TAG(MAX7456_NRST_PIN));
+ IOInit(max7456ResetPin, OWNER_OSD, 0);
+ IOConfigGPIO(max7456ResetPin, IO_RESET_CFG);
+
+
+ // RESET
+ IOLo(max7456ResetPin);
+ delay(100);
+ IOHi(max7456ResetPin);
+#endif
+}
+
#endif
diff --git a/src/main/drivers/max7456.h b/src/main/drivers/max7456.h
index 3b7eb066b9..9dbed4ccb5 100755
--- a/src/main/drivers/max7456.h
+++ b/src/main/drivers/max7456.h
@@ -35,6 +35,7 @@
extern uint16_t maxScreenSize;
struct vcdProfile_s;
+void max7456HardwareReset(void);
void max7456Init(const struct vcdProfile_s *vcdProfile);
void max7456DrawScreen(void);
void max7456WriteNvm(uint8_t char_address, const uint8_t *font_data);
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index aade7d0176..df415e302d 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1908,11 +1908,12 @@ static void cliSerialPassthrough(char *cmdline)
tok = strtok_r(NULL, " ", &saveptr);
}
+ printf("Port %d ", id);
serialPort_t *passThroughPort;
serialPortUsage_t *passThroughPortUsage = findSerialPortUsageByIdentifier(id);
if (!passThroughPortUsage || passThroughPortUsage->serialPort == NULL) {
if (!baud) {
- printf("Port %d is closed, must specify baud.\r\n", id);
+ printf("closed, specify baud.\r\n");
return;
}
if (!mode)
@@ -1922,29 +1923,28 @@ static void cliSerialPassthrough(char *cmdline)
baud, mode,
SERIAL_NOT_INVERTED);
if (!passThroughPort) {
- printf("Port %d could not be opened.\r\n", id);
+ printf("could not be opened.\r\n");
return;
}
- printf("Port %d opened, baud = %d.\r\n", id, baud);
+ printf("opened, baud = %d.\r\n", baud);
} else {
passThroughPort = passThroughPortUsage->serialPort;
// If the user supplied a mode, override the port's mode, otherwise
// leave the mode unchanged. serialPassthrough() handles one-way ports.
- printf("Port %d already open.\r\n", id);
+ printf("already open.\r\n");
if (mode && passThroughPort->mode != mode) {
- printf("Adjusting mode from %d to %d.\r\n",
+ printf("mode changed from %d to %d.\r\n",
passThroughPort->mode, mode);
serialSetMode(passThroughPort, mode);
}
// If this port has a rx callback associated we need to remove it now.
// Otherwise no data will be pushed in the serial port buffer!
if (passThroughPort->rxCallback) {
- printf("Removing rxCallback\r\n");
passThroughPort->rxCallback = 0;
}
}
- printf("Forwarding data to %d, power cycle to exit.\r\n", id);
+ printf("forwarding, power cycle to exit.\r\n");
serialPassthrough(cliPort, passThroughPort, NULL, NULL);
}
@@ -2973,11 +2973,11 @@ static void cliFeature(char *cmdline)
}
cliPrint("\r\n");
} else if (strncasecmp(cmdline, "list", len) == 0) {
- cliPrint("Available: ");
+ cliPrint("Available:");
for (uint32_t i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
- cliPrintf("%s ", featureNames[i]);
+ cliPrintf(" %s", featureNames[i]);
}
cliPrint("\r\n");
return;
@@ -3060,7 +3060,7 @@ static void cliBeeper(char *cmdline)
} else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available:");
for (uint32_t i = 0; i < beeperCount; i++)
- cliPrintf(" %s", beeperNameForTableIndex(i));
+ cliPrintf(" %s", beeperNameForTableIndex(i));
cliPrint("\r\n");
return;
} else {
@@ -3360,11 +3360,11 @@ static void cliMixer(char *cmdline)
cliPrintf("Mixer: %s\r\n", mixerNames[mixerConfig()->mixerMode - 1]);
return;
} else if (strncasecmp(cmdline, "list", len) == 0) {
- cliPrint("Available mixers: ");
+ cliPrint("Available:");
for (uint32_t i = 0; ; i++) {
if (mixerNames[i] == NULL)
break;
- cliPrintf("%s ", mixerNames[i]);
+ cliPrintf(" %s", mixerNames[i]);
}
cliPrint("\r\n");
return;
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 8c76f38683..0baea1564c 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -91,6 +91,7 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/transponder_ir.h"
#include "io/osd.h"
+#include "io/osd_slave.h"
#include "io/displayport_msp.h"
#include "io/vtx.h"
#include "io/vtx_smartaudio.h"
@@ -155,6 +156,21 @@ void processLoopback(void)
#endif
}
+
+#ifdef BUS_SWITCH_PIN
+void busSwitchInit(void)
+{
+static IO_t busSwitchResetPin = IO_NONE;
+
+ busSwitchResetPin = IOGetByTag(IO_TAG(BUS_SWITCH_PIN));
+ IOInit(busSwitchResetPin, OWNER_SYSTEM, 0);
+ IOConfigGPIO(busSwitchResetPin, IOCFG_OUT_PP);
+
+ // ENABLE
+ IOLo(busSwitchResetPin);
+}
+#endif
+
void init(void)
{
#ifdef USE_HAL_DRIVER
@@ -255,6 +271,10 @@ void init(void)
timerInit(); // timer must be initialized before any channel is allocated
+#ifdef BUS_SWITCH_PIN
+ busSwitchInit();
+#endif
+
#if defined(AVOID_UART1_FOR_PWM_PPM)
serialInit(feature(FEATURE_SOFTSERIAL),
feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM) ? SERIAL_PORT_USART1 : SERIAL_PORT_NONE);
@@ -440,11 +460,11 @@ void init(void)
cmsInit();
#endif
-#if ( defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) )
+#if (defined(OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(CMS)) || defined(USE_OSD_SLAVE))
displayPort_t *osdDisplayPort = NULL;
#endif
-#ifdef OSD
+#if defined(OSD) && !defined(USE_OSD_SLAVE)
//The OSD need to be initialised after GYRO to avoid GYRO initialisation failure on some targets
if (feature(FEATURE_OSD)) {
@@ -459,6 +479,15 @@ void init(void)
}
#endif
+#if defined(USE_OSD_SLAVE) && !defined(OSD)
+#if defined(USE_MAX7456)
+ // If there is a max7456 chip for the OSD then use it
+ osdDisplayPort = max7456DisplayPortInit(vcdProfile());
+ // osdInit will register with CMS by itself.
+ osdSlaveInit(osdDisplayPort);
+#endif
+#endif
+
#if defined(CMS) && defined(USE_MSP_DISPLAYPORT)
// If BFOSD is not active, then register MSP_DISPLAYPORT as a CMS device.
if (!osdDisplayPort)
@@ -588,6 +617,10 @@ void init(void)
latchActiveFeatures();
motorControlEnable = true;
+#ifdef USE_OSD_SLAVE
+ osdSlaveTasksInit();
+#else
fcTasksInit();
+#endif
systemState |= SYSTEM_STATE_READY;
}
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index cbcc376a4e..09d14c29dc 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -78,6 +78,7 @@
#include "io/ledstrip.h"
#include "io/motors.h"
#include "io/osd.h"
+#include "io/osd_slave.h"
#include "io/serial.h"
#include "io/serial_4way.h"
#include "io/servos.h"
@@ -113,40 +114,39 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
-// FIXME remove ;'s
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
- { BOXARM, "ARM;", 0 },
- { BOXANGLE, "ANGLE;", 1 },
- { BOXHORIZON, "HORIZON;", 2 },
- { BOXBARO, "BARO;", 3 },
- { BOXANTIGRAVITY, "ANTI GRAVITY;", 4 },
- { BOXMAG, "MAG;", 5 },
- { BOXHEADFREE, "HEADFREE;", 6 },
- { BOXHEADADJ, "HEADADJ;", 7 },
- { BOXCAMSTAB, "CAMSTAB;", 8 },
- { BOXCAMTRIG, "CAMTRIG;", 9 },
- { BOXGPSHOME, "GPS HOME;", 10 },
- { BOXGPSHOLD, "GPS HOLD;", 11 },
- { BOXPASSTHRU, "PASSTHRU;", 12 },
- { BOXBEEPERON, "BEEPER;", 13 },
- { BOXLEDMAX, "LEDMAX;", 14 },
- { BOXLEDLOW, "LEDLOW;", 15 },
- { BOXLLIGHTS, "LLIGHTS;", 16 },
- { BOXCALIB, "CALIB;", 17 },
- { BOXGOV, "GOVERNOR;", 18 },
- { BOXOSD, "OSD SW;", 19 },
- { BOXTELEMETRY, "TELEMETRY;", 20 },
- { BOXGTUNE, "GTUNE;", 21 },
- { BOXSONAR, "SONAR;", 22 },
- { BOXSERVO1, "SERVO1;", 23 },
- { BOXSERVO2, "SERVO2;", 24 },
- { BOXSERVO3, "SERVO3;", 25 },
- { BOXBLACKBOX, "BLACKBOX;", 26 },
- { BOXFAILSAFE, "FAILSAFE;", 27 },
- { BOXAIRMODE, "AIR MODE;", 28 },
- { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 29},
- { BOXFPVANGLEMIX, "FPV ANGLE MIX;", 30},
- { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s);", 31 },
+ { BOXARM, "ARM", 0 },
+ { BOXANGLE, "ANGLE", 1 },
+ { BOXHORIZON, "HORIZON", 2 },
+ { BOXBARO, "BARO", 3 },
+ { BOXANTIGRAVITY, "ANTI GRAVITY", 4 },
+ { BOXMAG, "MAG", 5 },
+ { BOXHEADFREE, "HEADFREE", 6 },
+ { BOXHEADADJ, "HEADADJ", 7 },
+ { BOXCAMSTAB, "CAMSTAB", 8 },
+ { BOXCAMTRIG, "CAMTRIG", 9 },
+ { BOXGPSHOME, "GPS HOME", 10 },
+ { BOXGPSHOLD, "GPS HOLD", 11 },
+ { BOXPASSTHRU, "PASSTHRU", 12 },
+ { BOXBEEPERON, "BEEPER", 13 },
+ { BOXLEDMAX, "LEDMAX", 14 },
+ { BOXLEDLOW, "LEDLOW", 15 },
+ { BOXLLIGHTS, "LLIGHTS", 16 },
+ { BOXCALIB, "CALIB", 17 },
+ { BOXGOV, "GOVERNOR", 18 },
+ { BOXOSD, "OSD SW", 19 },
+ { BOXTELEMETRY, "TELEMETRY", 20 },
+ { BOXGTUNE, "GTUNE", 21 },
+ { BOXSONAR, "SONAR", 22 },
+ { BOXSERVO1, "SERVO1", 23 },
+ { BOXSERVO2, "SERVO2", 24 },
+ { BOXSERVO3, "SERVO3", 25 },
+ { BOXBLACKBOX, "BLACKBOX", 26 },
+ { BOXFAILSAFE, "FAILSAFE", 27 },
+ { BOXAIRMODE, "AIR MODE", 28 },
+ { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
+ { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
+ { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@@ -275,11 +275,6 @@ const box_t *findBoxByPermanentId(uint8_t permenantId)
static void serializeBoxNamesReply(sbuf_t *dst)
{
- int flag = 1, count = 0;
-
-reset:
- // in first run of the loop, we grab total size of junk to be sent
- // then come back and actually send it
for (int i = 0; i < activeBoxIdCount; i++) {
const int activeBoxId = activeBoxIds[i];
const box_t *box = findBoxByBoxId(activeBoxId);
@@ -287,19 +282,8 @@ reset:
continue;
}
- const int len = strlen(box->boxName);
- if (flag) {
- count += len;
- } else {
- for (int j = 0; j < len; j++) {
- sbufWriteU8(dst, box->boxName[j]);
- }
- }
- }
-
- if (flag) {
- flag = 0;
- goto reset;
+ sbufWriteData(dst, box->boxName, strlen(box->boxName));
+ sbufWriteU8(dst, ';');
}
}
@@ -583,6 +567,15 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, hardwareRevision);
#else
sbufWriteU16(dst, 0); // No other build targets currently have hardware revision detection.
+#endif
+#ifdef USE_OSD_SLAVE
+ sbufWriteU8(dst, 1); // 1 == OSD
+#else
+#if defined(OSD) && defined(USE_MAX7456)
+ sbufWriteU8(dst, 2); // 2 == FC with OSD
+#else
+ sbufWriteU8(dst, 0); // 0 == FC
+#endif
#endif
break;
@@ -1112,15 +1105,36 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#endif
break;
- case MSP_OSD_CONFIG:
-#ifdef OSD
- sbufWriteU8(dst, 1); // OSD supported
- // send video system (AUTO/PAL/NTSC)
+ case MSP_OSD_CONFIG: {
+
+#define OSD_FLAGS_OSD_FEATURE (1 << 0)
+#define OSD_FLAGS_OSD_SLAVE (1 << 1)
+#define OSD_FLAGS_RESERVED_1 (1 << 2)
+#define OSD_FLAGS_RESERVED_2 (1 << 3)
+#define OSD_FLAGS_OSD_HARDWARE_MAX_7456 (1 << 4)
+
+ uint8_t osdFlags = 0;
+#if defined(OSD)
+ osdFlags |= OSD_FLAGS_OSD_FEATURE;
+#endif
+#if defined(USE_OSD_SLAVE)
+ osdFlags |= OSD_FLAGS_OSD_SLAVE;
+#endif
#ifdef USE_MAX7456
+ osdFlags |= OSD_FLAGS_OSD_HARDWARE_MAX_7456;
+#endif
+
+ sbufWriteU8(dst, osdFlags);
+
+#ifdef USE_MAX7456
+ // send video system (AUTO/PAL/NTSC)
sbufWriteU8(dst, vcdProfile()->video_system);
#else
sbufWriteU8(dst, 0);
#endif
+
+#ifdef OSD
+ // OSD specific, not applicable to OSD slaves.
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, osdConfig()->cap_alarm);
@@ -1129,11 +1143,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdConfig()->item_pos[i]);
}
-#else
- sbufWriteU8(dst, 0); // OSD not supported
#endif
break;
-
+ }
case MSP_MOTOR_3D_CONFIG:
sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
@@ -1637,7 +1649,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
#endif
-#ifdef OSD
+#if defined(OSD) || defined (USE_OSD_SLAVE)
case MSP_SET_OSD_CONFIG:
{
const uint8_t addr = sbufReadU8(src);
@@ -1648,20 +1660,27 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else
sbufReadU8(src); // Skip video system
#endif
+#if defined(OSD)
osdConfigMutable()->units = sbufReadU8(src);
osdConfigMutable()->rssi_alarm = sbufReadU8(src);
osdConfigMutable()->cap_alarm = sbufReadU16(src);
osdConfigMutable()->time_alarm = sbufReadU16(src);
osdConfigMutable()->alt_alarm = sbufReadU16(src);
+#endif
} else {
+#if defined(OSD)
// set a position setting
const uint16_t pos = sbufReadU16(src);
if (addr < OSD_ITEM_COUNT) {
osdConfigMutable()->item_pos[addr] = pos;
}
+#else
+ return MSP_RESULT_ERROR;
+#endif
}
}
break;
+
case MSP_OSD_CHAR_WRITE:
#ifdef USE_MAX7456
{
@@ -1674,14 +1693,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
max7456WriteNvm(addr, font_data);
}
#else
+ return MSP_RESULT_ERROR;
+/*
// just discard the data
sbufReadU8(src);
for (int i = 0; i < 54; i++) {
sbufReadU8(src);
}
+*/
#endif
break;
-#endif
+#endif // OSD || USE_OSD_SLAVE
+
#if defined(USE_RTC6705) || defined(VTX_COMMON)
case MSP_SET_VTX_CONFIG:
@@ -2027,6 +2050,57 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
return ret;
}
+void mspFcProcessReply(mspPacket_t *reply)
+{
+ sbuf_t *src = &reply->buf;
+ UNUSED(src); // potentially unused depending on compile options.
+
+ switch (reply->cmd) {
+ case MSP_DISPLAYPORT: {
+#ifdef USE_OSD_SLAVE
+ int subCmd = sbufReadU8(src);
+
+ switch (subCmd) {
+ case 0: // HEARTBEAT
+ //debug[0]++;
+ osdSlaveHeartbeat();
+ break;
+ case 1: // RELEASE
+ //debug[1]++;
+ break;
+ case 2: // CLEAR
+ //debug[2]++;
+ osdSlaveClearScreen();
+ break;
+ case 3: {
+ //debug[3]++;
+
+#define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
+ const uint8_t y = sbufReadU8(src); // row
+ const uint8_t x = sbufReadU8(src); // column
+ const uint8_t reserved = sbufReadU8(src);
+ UNUSED(reserved);
+
+ char buf[MSP_OSD_MAX_STRING_LENGTH + 1];
+ const int len = MIN(sbufBytesRemaining(src), MSP_OSD_MAX_STRING_LENGTH);
+ sbufReadData(src, &buf, len);
+
+ buf[len] = 0;
+
+ osdSlaveWrite(x, y, buf);
+
+ break;
+ }
+ case 4: {
+ osdSlaveDrawScreen();
+ }
+ }
+#endif
+ break;
+ }
+ }
+}
+
/*
* Return a pointer to the process command function
*/
diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h
index f25e02fc11..7992a9f15c 100644
--- a/src/main/fc/fc_msp.h
+++ b/src/main/fc/fc_msp.h
@@ -31,3 +31,4 @@ const box_t *findBoxByPermanentId(uint8_t permenantId);
void mspFcInit(void);
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
+void mspFcProcessReply(mspPacket_t *reply);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 72c57be184..3396c5d0a9 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -59,6 +59,7 @@
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/osd.h"
+#include "io/osd_slave.h"
#include "io/serial.h"
#include "io/transponder_ir.h"
#include "io/vtx_tramp.h" // Will be gone
@@ -96,6 +97,13 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
accUpdate(&accelerometerConfigMutable()->accelerometerTrims);
}
+bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) {
+ UNUSED(currentTimeUs);
+ UNUSED(currentDeltaTimeUs);
+
+ return mspSerialWaiting();
+}
+
static void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
@@ -106,7 +114,7 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
return;
}
#endif
- mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
+ mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
}
void taskBatteryAlerts(timeUs_t currentTimeUs)
@@ -208,6 +216,32 @@ void taskVtxControl(uint32_t currentTime)
}
#endif
+#ifdef USE_OSD_SLAVE
+void osdSlaveTasksInit(void)
+{
+ schedulerInit();
+ setTaskEnabled(TASK_SERIAL, true);
+
+ bool useBatteryVoltage = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
+ setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage);
+ bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
+ setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent);
+
+ bool useBatteryAlerts = (batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts);
+ setTaskEnabled(TASK_BATTERY_ALERTS, (useBatteryVoltage || useBatteryCurrent) && useBatteryAlerts);
+
+#ifdef TRANSPONDER
+ setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
+#endif
+
+ setTaskEnabled(TASK_OSD_SLAVE, true);
+
+#ifdef STACK_CHECK
+ setTaskEnabled(TASK_STACK_CHECK, true);
+#endif
+}
+#endif
+
void fcTasksInit(void)
{
schedulerInit();
@@ -281,6 +315,9 @@ void fcTasksInit(void)
#ifdef OSD
setTaskEnabled(TASK_OSD, feature(FEATURE_OSD));
#endif
+#ifdef USE_OSD_SLAVE
+ setTaskEnabled(TASK_OSD_SLAVE, true);
+#endif
#ifdef USE_BST
setTaskEnabled(TASK_BST_MASTER_PROCESS, true);
#endif
@@ -348,8 +385,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_SERIAL] = {
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
+#ifdef USE_OSD_SLAVE
+ .checkFunc = taskSerialCheck,
+ .desiredPeriod = TASK_PERIOD_HZ(100),
+ .staticPriority = TASK_PRIORITY_REALTIME,
+#else
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
.staticPriority = TASK_PRIORITY_LOW,
+#endif
},
[TASK_DISPATCH] = {
@@ -457,6 +500,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
+#ifdef USE_OSD_SLAVE
+ [TASK_OSD_SLAVE] = {
+ .taskName = "OSD_SLAVE",
+ .checkFunc = osdSlaveCheck,
+ .taskFunc = osdSlaveUpdate,
+ .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz
+ .staticPriority = TASK_PRIORITY_HIGH,
+ },
+#endif
#ifdef TELEMETRY
[TASK_TELEMETRY] = {
.taskName = "TELEMETRY",
diff --git a/src/main/fc/fc_tasks.h b/src/main/fc/fc_tasks.h
index 16db100bb6..5abc2b26d1 100644
--- a/src/main/fc/fc_tasks.h
+++ b/src/main/fc/fc_tasks.h
@@ -20,3 +20,4 @@
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
void fcTasksInit(void);
+void osdSlaveTasksInit(void);
diff --git a/src/main/io/displayport_max7456.c b/src/main/io/displayport_max7456.c
index e1a83fd67e..65f1464baf 100644
--- a/src/main/io/displayport_max7456.c
+++ b/src/main/io/displayport_max7456.c
@@ -33,6 +33,7 @@
#include "io/displayport_max7456.h"
#include "io/osd.h"
+#include "io/osd_slave.h"
displayPort_t max7456DisplayPort;
@@ -41,9 +42,12 @@ PG_REGISTER(displayPortProfile_t, displayPortProfileMax7456, PG_DISPLAY_PORT_MAX
static int grab(displayPort_t *displayPort)
{
+ // FIXME this should probably not have a dependency on the OSD or OSD slave code
UNUSED(displayPort);
+#ifdef OSD
osdResetAlarms();
refreshTimeout = 0;
+#endif
return 0;
}
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index 9c5d00fc46..533ea4fb01 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -44,7 +44,7 @@ PG_REGISTER(displayPortProfile_t, displayPortProfileMsp, PG_DISPLAY_PORT_MSP_CON
static displayPort_t mspDisplayPort;
-static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, int len)
+static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len)
{
UNUSED(displayPort);
return mspSerialPush(cmd, buf, len);
@@ -52,7 +52,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, const uint8_t *buf, i
static int heartbeat(displayPort_t *displayPort)
{
- const uint8_t subcmd[] = { 0 };
+ uint8_t subcmd[] = { 0 };
// ensure display is not released by MW OSD software
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
@@ -65,22 +65,22 @@ static int grab(displayPort_t *displayPort)
static int release(displayPort_t *displayPort)
{
- const uint8_t subcmd[] = { 1 };
+ uint8_t subcmd[] = { 1 };
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
static int clearScreen(displayPort_t *displayPort)
{
- const uint8_t subcmd[] = { 2 };
+ uint8_t subcmd[] = { 2 };
return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
static int drawScreen(displayPort_t *displayPort)
{
- UNUSED(displayPort);
- return 0;
+ uint8_t subcmd[] = { 4 };
+ return output(displayPort, MSP_DISPLAYPORT, subcmd, sizeof(subcmd));
}
static int screenSize(const displayPort_t *displayPort)
@@ -90,7 +90,7 @@ static int screenSize(const displayPort_t *displayPort)
static int write(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *string)
{
-#define MSP_OSD_MAX_STRING_LENGTH 30
+#define MSP_OSD_MAX_STRING_LENGTH 30 // FIXME move this
uint8_t buf[MSP_OSD_MAX_STRING_LENGTH + 4];
int len = strlen(string);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 4bf42721fe..0486ed4cd9 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -127,7 +127,7 @@ typedef struct statistic_s {
static statistic_t stats;
uint16_t refreshTimeout = 0;
-#define REFRESH_1S 12
+#define REFRESH_1S 12 // FIXME dependant on how frequently the task is scheduled
static uint8_t armState;
@@ -704,6 +704,7 @@ static void osdUpdateStats(void)
stats.max_altitude = baro.BaroAlt;
}
+#ifdef BLACKBOX
static void osdGetBlackboxStatusString(char * buff, uint8_t len)
{
bool storageDeviceIsWorking = false;
@@ -744,6 +745,7 @@ static void osdGetBlackboxStatusString(char * buff, uint8_t len)
snprintf(buff, len, "FAULT");
}
}
+#endif
static void osdShowStats(void)
{
@@ -785,11 +787,13 @@ static void osdShowStats(void)
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
displayWrite(osdDisplayPort, 22, top++, buff);
+#ifdef BLACKBOX
if (blackboxConfig()->device && blackboxConfig()->device != BLACKBOX_DEVICE_SERIAL) {
displayWrite(osdDisplayPort, 2, top, "BLACKBOX :");
osdGetBlackboxStatusString(buff, 10);
displayWrite(osdDisplayPort, 22, top++, buff);
}
+#endif
refreshTimeout = 60 * REFRESH_1S;
}
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index ee26fb898a..859e7a70d5 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -17,6 +17,7 @@
#pragma once
+#ifdef OSD
#include "common/time.h"
#include "config/parameter_group.h"
@@ -80,3 +81,5 @@ void osdInit(struct displayPort_s *osdDisplayPort);
void osdResetConfig(osdConfig_t *osdProfile);
void osdResetAlarms(void);
void osdUpdate(timeUs_t currentTimeUs);
+
+#endif
diff --git a/src/main/io/osd_slave.c b/src/main/io/osd_slave.c
new file mode 100644
index 0000000000..5898e4c9ec
--- /dev/null
+++ b/src/main/io/osd_slave.c
@@ -0,0 +1,153 @@
+/*
+ * 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 Dominic Clifton
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_OSD_SLAVE
+
+#include "build/debug.h"
+#include "build/version.h"
+
+#include "common/printf.h"
+#include "common/utils.h"
+
+#include "drivers/max7456_symbols.h"
+#include "drivers/display.h"
+#include "drivers/system.h"
+
+#include "io/osd_slave.h"
+
+//#define OSD_SLAVE_DEBUG
+
+static displayPort_t *osdDisplayPort;
+
+static void osdDrawLogo(int x, int y)
+{
+ char fontOffset = 160;
+ for (int row = 0; row < 4; row++) {
+ for (int column = 0; column < 24; column++) {
+ if (fontOffset != 255) // FIXME magic number
+ displayWriteChar(osdDisplayPort, x + column, y + row, fontOffset++);
+ }
+ }
+}
+
+bool displayDrawScreenQueued = false;
+bool receivingScreen = false;
+bool stalled = false;
+
+void osdSlaveDrawScreen(void)
+{
+ displayDrawScreenQueued = true;
+}
+
+static uint32_t timeoutAt = 0;
+
+void osdSlaveClearScreen(void)
+{
+ displayClearScreen(osdDisplayPort);
+ receivingScreen = true;
+}
+
+void osdSlaveWriteChar(const uint8_t x, const uint8_t y, const uint8_t c)
+{
+ displayWriteChar(osdDisplayPort, x, y, c);
+}
+
+void osdSlaveWrite(const uint8_t x, const uint8_t y, const char *s)
+{
+ displayWrite(osdDisplayPort, x, y, s);
+}
+
+void osdSlaveHeartbeat(void)
+{
+ timeoutAt = micros() + (1000 * 1000);
+ stalled = false;
+}
+
+void osdSlaveInit(displayPort_t *osdDisplayPortToUse)
+{
+ if (!osdDisplayPortToUse)
+ return;
+
+ osdDisplayPort = osdDisplayPortToUse;
+
+ displayClearScreen(osdDisplayPort);
+
+ osdDrawLogo(3, 1);
+
+ char string_buffer[30];
+ sprintf(string_buffer, "V%s", FC_VERSION_STRING);
+ displayWrite(osdDisplayPort, 20, 6, string_buffer);
+ displayWrite(osdDisplayPort, 13, 6, "OSD");
+
+ displayResync(osdDisplayPort);
+
+ displayDrawScreenQueued = true;
+}
+
+bool osdSlaveCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+{
+ UNUSED(currentTimeUs);
+ UNUSED(currentDeltaTimeUs);
+
+ if (!stalled && (cmp32(currentTimeUs, timeoutAt) > 0)) {
+ stalled = true;
+
+ displayWrite(osdDisplayPort, 8, 12, "WAITING FOR FC");
+ displayResync(osdDisplayPort);
+ }
+
+ return receivingScreen || displayDrawScreenQueued || stalled;
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void osdSlaveUpdate(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+#ifdef MAX7456_DMA_CHANNEL_TX
+ // don't touch buffers if DMA transaction is in progress
+ if (displayIsTransferInProgress(osdDisplayPort)) {
+ return;
+ }
+#endif // MAX7456_DMA_CHANNEL_TX
+
+#ifdef OSD_SLAVE_DEBUG
+ char buff[32];
+ for (int i = 0; i < 4; i ++) {
+ sprintf(buff, "%5d", debug[i]);
+ displayWrite(osdDisplayPort, i * 8, 0, buff);
+ }
+#endif
+
+ if (displayDrawScreenQueued || stalled) {
+ displayDrawScreen(osdDisplayPort);
+ displayDrawScreenQueued = false;
+ receivingScreen = false;
+ }
+}
+#endif // OSD_SLAVE
diff --git a/src/main/io/osd_slave.h b/src/main/io/osd_slave.h
new file mode 100644
index 0000000000..c7974f84dc
--- /dev/null
+++ b/src/main/io/osd_slave.h
@@ -0,0 +1,40 @@
+/*
+ * 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
+
+#ifdef USE_OSD_SLAVE
+#include "common/time.h"
+
+struct displayPort_s;
+
+// init
+void osdSlaveInit(struct displayPort_s *osdDisplayPort);
+
+// task api
+bool osdSlaveCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
+void osdSlaveUpdate(timeUs_t currentTimeUs);
+
+// msp api
+void osdSlaveHeartbeat(void);
+void osdSlaveClearScreen(void);
+void osdSlaveWriteChar(const uint8_t x, const uint8_t y, const uint8_t c);
+void osdSlaveWrite(const uint8_t x, const uint8_t y, const char *s);
+
+void osdSlaveDrawScreen(void);
+
+#endif
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index b322ee7df6..5e6a14ac58 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 (*mspProcessReplyFnPtr)(mspPacket_t *cmd);
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index 843dba0fc3..f8c8eadf18 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -59,7 +59,7 @@
#define MSP_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
-#define API_VERSION_MINOR 34 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR 35 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
#define API_VERSION_LENGTH 2
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 74f34fd95e..7a233ca876 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -23,6 +23,7 @@
#include "common/streambuf.h"
#include "common/utils.h"
+#include "build/debug.h"
#include "io/serial.h"
@@ -82,7 +83,19 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
} else if (mspPort->c_state == MSP_HEADER_START) {
mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
} else if (mspPort->c_state == MSP_HEADER_M) {
- mspPort->c_state = (c == '<') ? MSP_HEADER_ARROW : MSP_IDLE;
+ mspPort->c_state = MSP_IDLE;
+ switch(c) {
+ case '<': // COMMAND
+ mspPort->packetType = MSP_PACKET_COMMAND;
+ mspPort->c_state = MSP_HEADER_ARROW;
+ break;
+ case '>': // REPLY
+ mspPort->packetType = MSP_PACKET_REPLY;
+ mspPort->c_state = MSP_HEADER_ARROW;
+ break;
+ default:
+ break;
+ }
} else if (mspPort->c_state == MSP_HEADER_ARROW) {
if (c > MSP_PORT_INBUF_SIZE) {
mspPort->c_state = MSP_IDLE;
@@ -173,12 +186,29 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
return mspPostProcessFn;
}
+
+static void mspSerialProcessReceivedReply(mspPort_t *msp, mspProcessReplyFnPtr mspProcessReplyFn)
+{
+ mspPacket_t reply = {
+ .buf = {
+ .ptr = msp->inBuf,
+ .end = msp->inBuf + msp->dataSize,
+ },
+ .cmd = msp->cmdMSP,
+ .result = 0,
+ };
+
+ mspProcessReplyFn(&reply);
+
+ msp->c_state = MSP_IDLE;
+}
+
/*
* Process MSP commands from serial ports configured as MSP ports.
*
* Called periodically by the scheduler.
*/
-void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn)
{
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t * const mspPort = &mspPorts[portIndex];
@@ -196,7 +226,11 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
}
if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
- mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
+ if (mspPort->packetType == MSP_PACKET_COMMAND) {
+ mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
+ } else if (mspPort->packetType == MSP_PACKET_REPLY) {
+ mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
+ }
break; // process one command at a time so as not to block.
}
}
@@ -207,23 +241,31 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
}
}
+bool mspSerialWaiting(void)
+{
+ for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+ mspPort_t * const mspPort = &mspPorts[portIndex];
+ if (!mspPort->port) {
+ continue;
+ }
+
+ if (serialRxBytesWaiting(mspPort->port)) {
+ return true;
+ }
+ }
+ return false;
+}
+
void mspSerialInit(void)
{
memset(mspPorts, 0, sizeof(mspPorts));
mspSerialAllocatePorts();
}
-int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
{
- static uint8_t pushBuf[30];
int ret = 0;
- mspPacket_t push = {
- .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), },
- .cmd = cmd,
- .result = 0,
- };
-
for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
mspPort_t * const mspPort = &mspPorts[portIndex];
if (!mspPort->port) {
@@ -235,9 +277,11 @@ int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
continue;
}
- sbufWriteData(&push.buf, data, datalen);
-
- sbufSwitchToReader(&push.buf, pushBuf);
+ mspPacket_t push = {
+ .buf = { .ptr = data, .end = data + datalen, },
+ .cmd = cmd,
+ .result = 0,
+ };
ret = mspSerialEncode(mspPort, &push);
}
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index c6faf448df..e6136420cc 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -32,6 +32,11 @@ typedef enum {
MSP_COMMAND_RECEIVED
} mspState_e;
+typedef enum {
+ MSP_PACKET_COMMAND,
+ MSP_PACKET_REPLY
+} mspPacketType_e;
+
typedef enum {
MSP_EVALUATE_NON_MSP_DATA,
MSP_SKIP_NON_MSP_DATA
@@ -58,13 +63,15 @@ typedef struct mspPort_s {
uint8_t checksum;
uint8_t cmdMSP;
mspState_e c_state;
+ mspPacketType_e packetType;
uint8_t inBuf[MSP_PORT_INBUF_SIZE];
} mspPort_t;
void mspSerialInit(void);
-void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);
+bool mspSerialWaiting(void);
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn);
void mspSerialAllocatePorts(void);
void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
-int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen);
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen);
uint32_t mspSerialTxBytesFree(void);
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 96fc785689..69641c4920 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -95,6 +95,9 @@ typedef enum {
#ifdef OSD
TASK_OSD,
#endif
+#ifdef USE_OSD_SLAVE
+ TASK_OSD_SLAVE,
+#endif
#ifdef USE_BST
TASK_BST_MASTER_PROCESS,
#endif
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index aee30e440d..eb3f85c1c5 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -145,6 +145,10 @@
#define CURRENT_METER_ADC_PIN PA5
#endif
+#define OSD
+#define USE_OSD_OVER_MSP_DISPLAYPORT
+#undef USE_DASHBOARD
+
#define TRANSPONDER
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
diff --git a/src/main/target/SPRACINGF3OSD/config.c b/src/main/target/SPRACINGF3OSD/config.c
new file mode 100644
index 0000000000..52d3b5b40d
--- /dev/null
+++ b/src/main/target/SPRACINGF3OSD/config.c
@@ -0,0 +1,36 @@
+/*
+ * 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 "drivers/serial.h"
+
+#include "io/serial.h"
+
+#include "config/feature.h"
+
+#include "fc/config.h"
+
+#ifdef TARGET_CONFIG
+void targetConfiguration(void)
+{
+ serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // To connect to FC.
+}
+#endif
diff --git a/src/main/target/SPRACINGF3OSD/target.c b/src/main/target/SPRACINGF3OSD/target.c
new file mode 100644
index 0000000000..ebd888f4bb
--- /dev/null
+++ b/src/main/target/SPRACINGF3OSD/target.c
@@ -0,0 +1,36 @@
+/*
+ * 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 "drivers/io.h"
+
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+#include "drivers/dma.h"
+
+const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
+
+ DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 1 ), // DMA1_Channel2
+ DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 1 ), // DMA1_Channel4
+ DEF_TIM(TIM16, CH1, PB4, TIM_USE_MOTOR, 1 ), // DMA1_Channel3 or DMA1_Channel6 with Remap (need remap to free SPI1_TX for Flash)
+ DEF_TIM(TIM17, CH1, PB5, TIM_USE_MOTOR, 1 ), // DMA1_Channel1 or DMA1_Channel7 with Remap (need remap, ADC1 is on DMA1_Channel1)
+
+ DEF_TIM(TIM1, CH1, PA8, TIM_USE_TRANSPONDER, 0 ),
+};
+
diff --git a/src/main/target/SPRACINGF3OSD/target.h b/src/main/target/SPRACINGF3OSD/target.h
new file mode 100644
index 0000000000..6414d2f902
--- /dev/null
+++ b/src/main/target/SPRACINGF3OSD/target.h
@@ -0,0 +1,115 @@
+/*
+ * 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 TARGET_BOARD_IDENTIFIER "SOF3"
+#define TARGET_CONFIG
+
+#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
+
+#define LED0 PA15
+
+#define USE_EXTI
+
+
+#define GYRO
+#define USE_FAKE_GYRO
+
+#define ACC
+#define USE_FAKE_ACC
+
+#define REMAP_TIM16_DMA
+#define REMAP_TIM17_DMA
+
+#define USE_VCP
+#define USE_UART1
+#define USE_UART3
+#define SERIAL_PORT_COUNT 3
+
+#define UART1_TX_PIN PA9
+#define UART1_RX_PIN PA10
+
+#define BUS_SWITCH_PIN PB3 // connects and disconnects UART1 from external devices
+
+#define UART3_TX_PIN PB10
+#define UART3_RX_PIN PB11
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C_DEVICE (I2CDEV_1)
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1 // Flash Chip
+#define USE_SPI_DEVICE_2 // MAX7456
+
+#define SPI1_NSS_PIN PA4
+#define SPI1_SCK_PIN PA5
+#define SPI1_MISO_PIN PA6
+#define SPI1_MOSI_PIN PA7
+
+#define SPI2_NSS_PIN PB12
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+
+#define USE_MAX7456
+#define MAX7456_SPI_INSTANCE SPI2
+#define MAX7456_SPI_CS_PIN SPI2_NSS_PIN
+#define MAX7456_NRST_PIN PB2
+
+#define MAX7456_DMA_CHANNEL_TX DMA1_Channel5
+#define MAX7456_DMA_CHANNEL_RX DMA1_Channel4
+#define MAX7456_DMA_IRQ_HANDLER_ID DMA1_CH4_HANDLER
+
+#define BOARD_HAS_VOLTAGE_DIVIDER
+#define BOARD_HAS_CURRENT_SENSOR
+
+#define USE_ADC
+#define ADC_INSTANCE ADC1
+#define VBAT_ADC_PIN PA2
+#define CURRENT_METER_ADC_PIN PA3
+#define VOLTAGE_12V_ADC_PIN PA0
+#define VOLTAGE_5V_ADC_PIN PA1
+
+#define TRANSPONDER
+
+#define USE_OSD_SLAVE
+
+#undef OSD
+#undef GPS
+#undef BLACKBOX
+#undef TELEMETRY
+#undef USE_SERVOS
+#undef VTX_COMMON
+#undef VTX_CONTROL
+#undef VTX_SMARTAUDIO
+#undef VTX_TRAMP
+#undef USE_MSP_DISPLAYPORT
+
+
+#define DEFAULT_FEATURES (FEATURE_TRANSPONDER)
+
+// IO - assuming 303 in 64pin package, TODO
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD (BIT(2))
+#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
+
+#define USABLE_TIMER_CHANNEL_COUNT 12 // 2xPPM, 6xPWM, UART3 RX/TX, LED Strip, IR.
+#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16))
diff --git a/src/main/target/SPRACINGF3OSD/target.mk b/src/main/target/SPRACINGF3OSD/target.mk
new file mode 100644
index 0000000000..b9712bcf01
--- /dev/null
+++ b/src/main/target/SPRACINGF3OSD/target.mk
@@ -0,0 +1,6 @@
+F3_TARGETS += $(TARGET)
+FEATURES = VCP SDCARD
+
+TARGET_SRC = \
+ drivers/accgyro_fake.c \
+ drivers/max7456.c
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index 8427804efd..fde111b64c 100644
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -31,7 +31,7 @@
#define BEEPER PC15
#define BEEPER_INVERTED
-#define INVERTER_PIN_UART2 PB2
+#define INVERTER_PIN_UART2 PB2
#define USE_EXTI
#define MPU_INT_EXTI PC13
@@ -154,6 +154,8 @@
#define BOARD_HAS_VOLTAGE_DIVIDER
+#define OSD
+#define USE_OSD_OVER_MSP_DISPLAYPORT
#define LED_STRIP
#define TRANSPONDER