diff --git a/Makefile b/Makefile
index 2ab2c34f09..8350e13854 100644
--- a/Makefile
+++ b/Makefile
@@ -760,6 +760,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 +852,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/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/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 ddbcb2b4b7..67f7ea802b 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"
@@ -2010,6 +2011,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..1dcb60b40c 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,11 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
return;
}
#endif
- mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
+#ifdef USE_OSD_SLAVE
+ mspSerialProcess(MSP_SKIP_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
+#else
+ mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
+#endif
}
void taskBatteryAlerts(timeUs_t currentTimeUs)
@@ -208,6 +220,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 +319,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 +389,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 +504,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..b8b1a72847 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -79,8 +79,8 @@ static int clearScreen(displayPort_t *displayPort)
static int drawScreen(displayPort_t *displayPort)
{
- UNUSED(displayPort);
- return 0;
+ const 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_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_serial.c b/src/main/msp/msp_serial.c
index 74f34fd95e..c1f2ece980 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,6 +241,21 @@ 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));
@@ -215,7 +264,7 @@ void mspSerialInit(void)
int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
{
- static uint8_t pushBuf[30];
+ static uint8_t pushBuf[34];
int ret = 0;
mspPacket_t push = {
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index c6faf448df..a1decd84b5 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,12 +63,14 @@ 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);
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