mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge pull request #2845 from betaflight/spracingf3osd-and-osd-slave
Add SPRACINGF3OSD target and OSD SLAVE support
This commit is contained in:
commit
0e1dc33371
29 changed files with 752 additions and 102 deletions
3
Makefile
3
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 \
|
||||
|
|
|
@ -27,7 +27,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef CMS
|
||||
#if defined(CMS) && defined(BLACKBOX)
|
||||
|
||||
#include "build/version.h"
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
23
src/main/drivers/max7456.c
Executable file → Normal file
23
src/main/drivers/max7456.c
Executable file → Normal file
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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",
|
||||
|
|
|
@ -20,3 +20,4 @@
|
|||
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
|
||||
|
||||
void fcTasksInit(void);
|
||||
void osdSlaveTasksInit(void);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
153
src/main/io/osd_slave.c
Normal file
153
src/main/io/osd_slave.c
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
Created by Dominic Clifton
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
40
src/main/io/osd_slave.h
Normal file
40
src/main/io/osd_slave.h
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
36
src/main/target/SPRACINGF3OSD/config.c
Normal file
36
src/main/target/SPRACINGF3OSD/config.c
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#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
|
36
src/main/target/SPRACINGF3OSD/target.c
Normal file
36
src/main/target/SPRACINGF3OSD/target.c
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
#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 ),
|
||||
};
|
||||
|
115
src/main/target/SPRACINGF3OSD/target.h
Normal file
115
src/main/target/SPRACINGF3OSD/target.h
Normal file
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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))
|
6
src/main/target/SPRACINGF3OSD/target.mk
Normal file
6
src/main/target/SPRACINGF3OSD/target.mk
Normal file
|
@ -0,0 +1,6 @@
|
|||
F3_TARGETS += $(TARGET)
|
||||
FEATURES = VCP SDCARD
|
||||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro_fake.c \
|
||||
drivers/max7456.c
|
|
@ -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
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue