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