diff --git a/docs/Serial.md b/docs/Serial.md index afbffe0ca2..ca039910f5 100644 --- a/docs/Serial.md +++ b/docs/Serial.md @@ -100,27 +100,30 @@ Other devices can be added starting from id 50. ### 2. Serial Port Function -| Function | Value | -| ---------------------------- | ----- | -| FUNCTION_NONE | 0 | -| FUNCTION_MSP | 1 | -| FUNCTION_GPS | 2 | -| FUNCTION_TELEMETRY_FRSKY_HUB | 4 | -| FUNCTION_TELEMETRY_HOTT | 8 | -| FUNCTION_TELEMETRY_LTM | 16 | -| FUNCTION_TELEMETRY_SMARTPORT | 32 | -| FUNCTION_RX_SERIAL | 64 | -| FUNCTION_BLACKBOX | 128 | -| FUNCTION_TELEMETRY_MAVLINK | 512 | -| FUNCTION_ESC_SENSOR | 1024 | -| FUNCTION_VTX_SMARTAUDIO | 2048 | -| FUNCTION_TELEMETRY_IBUS | 4096 | -| FUNCTION_VTX_TRAMP | 8192 | -| FUNCTION_RCDEVICE | 16384 | -| FUNCTION_LIDAR_TF | 32768 | -| FUNCTION_FRSKY_OSD | 65536 | +| Function | Value | Bit | +| ---------------------------- | ------ | --- | +| FUNCTION_NONE | 0 | 0 | +| FUNCTION_MSP | 1 | 1 << 0 | +| FUNCTION_GPS | 2 | 1 << 1 | +| FUNCTION_TELEMETRY_FRSKY_HUB | 4 | 1 << 2 | +| FUNCTION_TELEMETRY_HOTT | 8 | 1 << 3 | +| FUNCTION_TELEMETRY_LTM | 16 | 1 << 4 | +| FUNCTION_TELEMETRY_SMARTPORT | 32 | 1 << 5 | +| FUNCTION_RX_SERIAL | 64 | 1 << 6 | +| FUNCTION_BLACKBOX | 128 | 1 << 7 | +| NOT USED | 256 | 1 << 8 | +| FUNCTION_TELEMETRY_MAVLINK | 512 | 1 << 9 | +| FUNCTION_ESC_SENSOR | 1024 | 1 << 10 | +| FUNCTION_VTX_SMARTAUDIO | 2048 | 1 << 11 | +| FUNCTION_TELEMETRY_IBUS | 4096 | 1 << 12 | +| FUNCTION_VTX_TRAMP | 8192 | 1 << 13 | +| FUNCTION_RCDEVICE | 16384 | 1 << 14 | +| FUNCTION_LIDAR_TF | 32768 | 1 << 15 | +| FUNCTION_FRSKY_OSD | 65536 | 1 << 16 | +| FUNCTION_VTX_MSP | 131072 | 1 << 17 | Note: `FUNCTION_FRSKY_OSD` = `(1<<16)` requires 17 bits. +Note2: We can use up to 32 bits (1<<32) here. ### 3. MSP Baudrates diff --git a/make/source.mk b/make/source.mk index 7a0f1006e2..6eb4dbc269 100644 --- a/make/source.mk +++ b/make/source.mk @@ -200,6 +200,8 @@ COMMON_SRC = \ io/vtx_smartaudio.c \ io/vtx_tramp.c \ io/vtx_control.c \ + io/vtx_msp.c \ + cms/cms_menu_vtx_msp.c COMMON_DEVICE_SRC = \ $(CMSIS_SRC) \ @@ -359,7 +361,9 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ osd/osd.c \ osd/osd_elements.c \ osd/osd_warnings.c \ - rx/rx_bind.c + rx/rx_bind.c \ + io/vtx_msp.c \ + cms/cms_menu_vtx_msp.c # Gyro driver files that only contain initialization and configuration code - not runtime code SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 6003509186..3b5d438b64 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -105,4 +105,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "GPS_RESCUE_HEADING", "GPS_RESCUE_TRACKING", "ATTITUDE", + "VTX_MSP" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index ffe135d64a..08bca7a75b 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -103,6 +103,7 @@ typedef enum { DEBUG_GPS_RESCUE_HEADING, DEBUG_GPS_RESCUE_TRACKING, DEBUG_ATTITUDE, + DEBUG_VTX_MSP, DEBUG_COUNT } debugType_e; diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index ca4bd1dd3a..3f2de5f154 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -88,6 +88,7 @@ #include "pg/max7456.h" #include "pg/mco.h" #include "pg/motor.h" +#include "pg/msp.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/pinio.h" @@ -1635,6 +1636,8 @@ const clivalue_t valueTable[] = { { "scheduler_relax_rx", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_SCHEDULER_CONFIG, PG_ARRAY_ELEMENT_OFFSET(schedulerConfig_t, 0, rxRelaxDeterminism) }, { "scheduler_relax_osd", VAR_UINT16 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_SCHEDULER_CONFIG, PG_ARRAY_ELEMENT_OFFSET(schedulerConfig_t, 0, osdRelaxDeterminism) }, + { "serialmsp_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MSP_CONFIG, offsetof(mspConfig_t, halfDuplex) }, + // PG_TIMECONFIG #ifdef USE_RTC_TIME { "timezone_offset_minutes", VAR_INT16 | MASTER_VALUE, .config.minmax = { TIMEZONE_OFFSET_MINUTES_MIN, TIMEZONE_OFFSET_MINUTES_MAX }, PG_TIME_CONFIG, offsetof(timeConfig_t, tz_offsetMinutes) }, diff --git a/src/main/cms/cms_menu_main.c b/src/main/cms/cms_menu_main.c index 3863687177..e1ad809acb 100644 --- a/src/main/cms/cms_menu_main.c +++ b/src/main/cms/cms_menu_main.c @@ -82,7 +82,7 @@ static const OSD_Entry menuFeaturesEntries[] = {"BLACKBOX", OME_Submenu, cmsMenuChange, &cmsx_menuBlackbox}, #endif #if defined(USE_VTX_CONTROL) -#if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) {"VTX", OME_Funcall, cmsSelectVtx, NULL}, #endif #endif // VTX_CONTROL diff --git a/src/main/cms/cms_menu_vtx_common.c b/src/main/cms/cms_menu_vtx_common.c index 527c5aa8ea..b0dd4a02ce 100644 --- a/src/main/cms/cms_menu_vtx_common.c +++ b/src/main/cms/cms_menu_vtx_common.c @@ -24,7 +24,7 @@ #include "platform.h" -#if defined(USE_CMS) && defined(USE_VTX_CONTROL) && (defined(USE_VTX_TRAMP) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_RTC6705)) +#if defined(USE_CMS) && defined(USE_VTX_CONTROL) && (defined(USE_VTX_TRAMP) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_RTC6705) || defined(USE_VTX_MSP)) #include "common/printf.h" @@ -32,6 +32,7 @@ #include "cms/cms_menu_vtx_rtc6705.h" #include "cms/cms_menu_vtx_smartaudio.h" #include "cms/cms_menu_vtx_tramp.h" +#include "cms/cms_menu_vtx_msp.h" #include "cms/cms_types.h" #include "drivers/vtx_common.h" @@ -118,7 +119,12 @@ const void *cmsSelectVtx(displayPort_t *pDisplay, const void *ptr) break; #endif +#if defined(USE_VTX_MSP) + case VTXDEV_MSP: + cmsMenuChange(pDisplay, &cmsx_menuVtxMsp); + break; +#endif default: cmsMenuChange(pDisplay, &cmsx_menuVtxError); diff --git a/src/main/cms/cms_menu_vtx_msp.c b/src/main/cms/cms_menu_vtx_msp.c new file mode 100644 index 0000000000..6f1cd42b63 --- /dev/null +++ b/src/main/cms/cms_menu_vtx_msp.c @@ -0,0 +1,285 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_CMS) && defined(USE_VTX_MSP) + +#include "common/printf.h" +#include "common/utils.h" + +#include "cms/cms.h" +#include "cms/cms_types.h" + +#include "drivers/vtx_common.h" +#include "drivers/vtx_table.h" + +#include "config/config.h" + +#include "io/vtx_msp.h" +#include "io/vtx.h" + +char mspCmsStatusString[31] = "- -- ---- ----"; +// m bc ffff tppp +// 01234567890123 + +void mspCmsUpdateStatusString(void) +{ + vtxDevice_t *vtxDevice = vtxCommonDevice(); + + if (vtxTableBandCount == 0 || vtxTablePowerLevels == 0) { + strncpy(mspCmsStatusString, "PLEASE CONFIGURE VTXTABLE", sizeof(mspCmsStatusString)); + return; + } + + mspCmsStatusString[0] = '*'; + mspCmsStatusString[1] = ' '; + uint8_t band; + uint8_t chan; + if (!vtxCommonGetBandAndChannel(vtxDevice, &band, &chan) || (band == 0 && chan == 0)) { + mspCmsStatusString[2] = 'U';//user freq + mspCmsStatusString[3] = 'F'; + } else { + mspCmsStatusString[2] = vtxCommonLookupBandLetter(vtxDevice, band); + mspCmsStatusString[3] = vtxCommonLookupChannelName(vtxDevice, chan)[0]; + } + mspCmsStatusString[4] = ' '; + + uint16_t freq; + if (!vtxCommonGetFrequency(vtxDevice, &freq) || (freq == 0)) { + tfp_sprintf(&mspCmsStatusString[5], "----"); + } else { + tfp_sprintf(&mspCmsStatusString[5], "%4d", freq); + } + + uint8_t powerIndex; + uint16_t powerValue; + if (vtxCommonGetPowerIndex(vtxDevice, &powerIndex) && vtxCommonLookupPowerValue(vtxDevice, powerIndex, &powerValue)) { + tfp_sprintf(&mspCmsStatusString[9], " *%3d", powerValue); + } else { + tfp_sprintf(&mspCmsStatusString[9], " ----"); + } +} + +uint8_t mspCmsPitMode = 0; +uint8_t mspCmsBand = 1; +uint8_t mspCmsChan = 1; +uint16_t mspCmsFreqRef; + +static OSD_TAB_t mspCmsEntBand; +static OSD_TAB_t mspCmsEntChan; + +static OSD_UINT16_t mspCmsEntFreqRef = { &mspCmsFreqRef, 5600, 5900, 0 }; + +static uint8_t mspCmsPower = 1; + +static OSD_TAB_t mspCmsEntPower; + +static void mspCmsUpdateFreqRef(void) +{ + if (mspCmsBand > 0 && mspCmsChan > 0) { + mspCmsFreqRef = vtxCommonLookupFrequency(vtxCommonDevice(), mspCmsBand, mspCmsChan); + } +} + +static const void *mspCmsConfigBand(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (mspCmsBand == 0) { + // Bounce back + mspCmsBand = 1; + } else { + mspCmsUpdateFreqRef(); + } + + return NULL; +} + +static const void *mspCmsConfigChan(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (mspCmsChan == 0) { + // Bounce back + mspCmsChan = 1; + } else { + mspCmsUpdateFreqRef(); + } + + return NULL; +} + +static const void *mspCmsConfigPower(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (mspCmsPower == 0) { + // Bounce back + mspCmsPower = 1; + } + + return NULL; +} + +#define MSP_PIT_STATUS_NA (0) +#define MSP_PIT_STATUS_OFF (1) +#define MSP_PIT_STATUS_ON (2) + +static const char * const mspCmsPitModeNames[] = { + "---", "OFF", "ON " +}; + +static OSD_TAB_t mspCmsEntPitMode = { &mspCmsPitMode, 2, mspCmsPitModeNames }; + +static const void *mspCmsSetPitMode(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + if (mspCmsPitMode == MSP_PIT_STATUS_NA) { + // Bouce back + mspCmsPitMode = MSP_PIT_STATUS_OFF; + } else { + vtxCommonSetPitMode(vtxCommonDevice(), (mspCmsPitMode == MSP_PIT_STATUS_OFF) ? 0 : 1); + } + + return NULL; +} + +static const void *mspCmsCommence(displayPort_t *pDisp, const void *self) +{ + UNUSED(pDisp); + UNUSED(self); + + vtxDevice_t *device = vtxCommonDevice(); + vtxCommonSetBandAndChannel(device, mspCmsBand, mspCmsChan); + vtxCommonSetPowerByIndex(device, mspCmsPower); + + // update'vtx_' settings + vtxSettingsConfigMutable()->band = mspCmsBand; + vtxSettingsConfigMutable()->channel = mspCmsChan; + vtxSettingsConfigMutable()->power = mspCmsPower; + vtxSettingsConfigMutable()->freq = vtxCommonLookupFrequency(vtxCommonDevice(), mspCmsBand, mspCmsChan); + + saveConfigAndNotify(); + + return MENU_CHAIN_BACK; +} + +static bool mspCmsInitSettings(void) +{ + vtxDevice_t *device = vtxCommonDevice(); + unsigned vtxStatus; + + if (!device) { + return false; + } + + vtxCommonGetBandAndChannel(device, &mspCmsBand, &mspCmsChan); + + mspCmsUpdateFreqRef(); + if (vtxCommonGetStatus(device, &vtxStatus)) { + mspCmsPitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? MSP_PIT_STATUS_ON : MSP_PIT_STATUS_OFF; + } else { + mspCmsPitMode = MSP_PIT_STATUS_NA; + } + + if (!vtxCommonGetPowerIndex(vtxCommonDevice(), &mspCmsPower)) { + mspCmsPower = 1; + } + + mspCmsEntBand.val = &mspCmsBand; + mspCmsEntBand.max = vtxTableBandCount; + mspCmsEntBand.names = vtxTableBandNames; + + mspCmsEntChan.val = &mspCmsChan; + mspCmsEntChan.max = vtxTableChannelCount; + mspCmsEntChan.names = vtxTableChannelNames; + + mspCmsEntPower.val = &mspCmsPower; + mspCmsEntPower.max = vtxTablePowerLevels; + mspCmsEntPower.names = vtxTablePowerLabels; + + return true; +} + +static const void *mspCmsOnEnter(displayPort_t *pDisp) +{ + UNUSED(pDisp); + + if (!mspCmsInitSettings()) { + return MENU_CHAIN_BACK; + } + + return NULL; +} + +static const OSD_Entry mspCmsMenuCommenceEntries[] = { + { "CONFIRM", OME_Label, NULL, NULL }, + { "YES", OME_Funcall, mspCmsCommence, NULL }, + { "NO", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + +static CMS_Menu mspCmsMenuCommence = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXMSPC", + .GUARD_type = OME_MENU, +#endif + .onEnter = NULL, + .onExit = NULL, + .onDisplayUpdate = NULL, + .entries = mspCmsMenuCommenceEntries, +}; + +static const OSD_Entry mspMenuEntries[] = +{ + { "- MSP -", OME_Label, NULL, NULL }, + { "", OME_Label | DYNAMIC, NULL, mspCmsStatusString }, + { "PIT", OME_TAB, mspCmsSetPitMode, &mspCmsEntPitMode }, + { "BAND", OME_TAB, mspCmsConfigBand, &mspCmsEntBand }, + { "CHAN", OME_TAB, mspCmsConfigChan, &mspCmsEntChan }, + { "(FREQ)", OME_UINT16 | DYNAMIC, NULL, &mspCmsEntFreqRef }, + { "POWER", OME_TAB, mspCmsConfigPower, &mspCmsEntPower }, + { "SAVE", OME_Submenu, cmsMenuChange, &mspCmsMenuCommence }, + { "BACK", OME_Back, NULL, NULL }, + { NULL, OME_END, NULL, NULL } +}; + +CMS_Menu cmsx_menuVtxMsp = { +#ifdef CMS_MENU_DEBUG + .GUARD_text = "XVTXMSP", + .GUARD_type = OME_MENU, +#endif + .onEnter = mspCmsOnEnter, + .onExit = NULL, + .onDisplayUpdate = NULL, + .entries = mspMenuEntries, +}; +#endif diff --git a/src/main/cms/cms_menu_vtx_msp.h b/src/main/cms/cms_menu_vtx_msp.h new file mode 100644 index 0000000000..1c255ebf72 --- /dev/null +++ b/src/main/cms/cms_menu_vtx_msp.h @@ -0,0 +1,27 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +#include "cms/cms.h" +#include "cms/cms_types.h" +extern CMS_Menu cmsx_menuVtxMsp; + +void mspCmsUpdateStatusString(void); diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 1bd0aa7650..1564cbad99 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,7 +36,7 @@ #endif -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) #define VTX_SETTINGS_FREQCMD @@ -52,6 +52,7 @@ typedef enum { // 2 reserved VTXDEV_SMARTAUDIO = 3, VTXDEV_TRAMP = 4, + VTXDEV_MSP = 5, VTXDEV_UNKNOWN = 0xFF, } vtxDevType_e; diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 43075a6482..d06312d6e8 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -117,6 +117,7 @@ #include "io/transponder_ir.h" #include "io/vtx.h" #include "io/vtx_control.h" +#include "io/vtx_msp.h" #include "io/vtx_rtc6705.h" #include "io/vtx_smartaudio.h" #include "io/vtx_tramp.h" @@ -839,6 +840,10 @@ void init(void) vtxCommonInit(); #endif +#ifdef USE_VTX_MSP + vtxMspInit(); +#endif + #ifdef USE_VTX_SMARTAUDIO vtxSmartAudioInit(); #endif diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 0bb2fed77a..2a82c7dd7b 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -598,7 +598,7 @@ void tasksInit(void) #endif #ifdef USE_VTX_CONTROL -#if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_RTC6705) || defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c index bc6c564beb..d082a872bd 100644 --- a/src/main/io/displayport_msp.c +++ b/src/main/io/displayport_msp.c @@ -54,7 +54,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len return 0; } #endif - return mspSerialPush(displayPortProfileMsp()->displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY); + return mspSerialPush(displayPortProfileMsp()->displayPortSerial, cmd, buf, len, MSP_DIRECTION_REPLY, MSP_V1); } static int heartbeat(displayPort_t *displayPort) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 737c032ada..18ce1f41a0 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -271,9 +271,9 @@ serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e s } #ifdef USE_TELEMETRY -#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK) +#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | TELEMETRY_PORT_FUNCTIONS_MASK | FUNCTION_VTX_MSP) #else -#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX) +#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | FUNCTION_VTX_MSP) #endif bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) @@ -301,6 +301,13 @@ bool isSerialConfigValid(const serialConfig_t *serialConfigToCheck) } uint8_t bitCount = BITCOUNT(portConfig->functionMask); + +#ifdef USE_VTX_MSP + if ((portConfig->functionMask & FUNCTION_VTX_MSP) && bitCount == 1) { // VTX MSP has to be shared with RX or MSP serial + return false; + } +#endif + if (bitCount > 1) { // shared if (bitCount > 2) { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 92e7795ccb..61dd6bab46 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -50,6 +50,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 14), // 16384 FUNCTION_LIDAR_TF = (1 << 15), // 32768 FUNCTION_FRSKY_OSD = (1 << 16), // 65536 + FUNCTION_VTX_MSP = (1 << 17), // 131072 } serialPortFunction_e; #define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c new file mode 100644 index 0000000000..55402c06e6 --- /dev/null +++ b/src/main/io/vtx_msp.c @@ -0,0 +1,377 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +/* Created by phobos- */ + +#include +#include +#include +#include +#include + +#include "platform.h" + +#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) + +#include "build/debug.h" + +#include "cms/cms_menu_vtx_msp.h" +#include "common/crc.h" +#include "config/feature.h" + +#include "drivers/vtx_common.h" +#include "drivers/vtx_table.h" + +#include "fc/runtime_config.h" +#include "flight/failsafe.h" + +#include "io/serial.h" +#include "io/vtx_msp.h" +#include "io/vtx_control.h" +#include "io/vtx.h" + +#include "msp/msp_protocol.h" +#include "msp/msp_serial.h" + +#include "pg/vtx_table.h" + +#include "rx/crsf.h" +#include "rx/crsf_protocol.h" +#include "rx/rx.h" + +#include "telemetry/msp_shared.h" + +static uint16_t mspConfFreq = 0; +static uint8_t mspConfBand = 0; +static uint8_t mspConfChannel = 0; +static uint16_t mspConfPower = 0; +static uint8_t mspConfPitMode = 0; +static bool mspVtxConfigChanged = false; +static timeUs_t mspVtxLastTimeUs = 0; +static bool prevLowPowerDisarmedState = false; + +static const vtxVTable_t mspVTable; // forward +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, +}; + +STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; +static uint8_t mspVtxPortIdentifier = 255; + +#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms + +static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) +{ + return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxRuntimeState.serialrxProvider == SERIALRX_CRSF; +} + +static bool isLowPowerDisarmed(void) +{ + return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || + (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); +} + +void setMspVtxDeviceStatusReady(const int descriptor) +{ + if (mspVtxStatus != MSP_VTX_STATUS_READY && vtxTableConfig()->bands && vtxTableConfig()->channels && vtxTableConfig()->powerLevels) { + if (getMspSerialPortDescriptor(mspVtxPortIdentifier) == descriptor || getMspTelemetryDescriptor() == descriptor) { + mspVtxStatus = MSP_VTX_STATUS_READY; + } + } +} + +void prepareMspFrame(uint8_t *mspFrame) +{ + mspFrame[0] = VTXDEV_MSP; + mspFrame[1] = vtxSettingsConfig()->band; + mspFrame[2] = vtxSettingsConfig()->channel; + mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based + mspFrame[4] = mspConfPitMode; + mspFrame[5] = vtxSettingsConfig()->freq & 0xFF; + mspFrame[6] = (vtxSettingsConfig()->freq >> 8) & 0xFF; + mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; + mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm; + mspFrame[9] = vtxSettingsConfig()->pitModeFreq & 0xFF; + mspFrame[10] = (vtxSettingsConfig()->pitModeFreq >> 8) & 0xFF; +#ifdef USE_VTX_TABLE + mspFrame[11] = 1; + mspFrame[12] = vtxTableConfig()->bands; + mspFrame[13] = vtxTableConfig()->channels; + mspFrame[14] = vtxTableConfig()->powerLevels; +#else + mspFrame[11] = 0; + mspFrame[12] = 0; + mspFrame[13] = 0; + mspFrame[14] = 0; +#endif +} + +static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) +{ + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + uint8_t mspHeader[8] = {'$', 'X', '>', 0, mspCommand & 0xFF, (mspCommand >> 8) & 0xFF, mspFrameSize & 0xFF, (mspFrameSize >> 8) & 0xFF }; // MSP V2 Native header + uint8_t mspChecksum; + + mspChecksum = crc8_dvb_s2_update(0, &mspHeader[3], 5); // first 3 characters are not checksummable + mspChecksum = crc8_dvb_s2_update(mspChecksum, mspFrame, mspFrameSize); + + uint8_t fullMspFrameSize = mspFrameSize + sizeof(mspHeader) + 1; // add 1 for msp checksum + uint8_t crsfFrameSize = CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_LENGTH_TYPE_CRC + fullMspFrameSize; + + uint8_t crsfFrame[crsfFrameSize]; + + dst->ptr = crsfFrame; + dst->end = ARRAYEND(crsfFrame); + + sbufWriteU8(dst, CRSF_SYNC_BYTE); + sbufWriteU8(dst, fullMspFrameSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself) + sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type + sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER); // response destination is the receiver the vtx connection + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device + sbufWriteData(dst, mspHeader, sizeof(mspHeader)); + sbufWriteData(dst, mspFrame, mspFrameSize); + sbufWriteU8(dst, mspChecksum); + crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length + sbufSwitchToReader(dst, crsfFrame); + + crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); + crsfRxSendTelemetryData(); +} + +static uint16_t packetCounter = 0; + +static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) +{ + UNUSED(vtxDevice); + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP); + uint8_t frame[15]; + + switch (mspVtxStatus) { + case MSP_VTX_STATUS_OFFLINE: + // wait for MSP communication from the VTX +#ifdef USE_CMS + mspCmsUpdateStatusString(); +#endif + break; + case MSP_VTX_STATUS_READY: + if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { + mspVtxConfigChanged = true; + prevLowPowerDisarmedState = isLowPowerDisarmed(); + } + + // send an update if stuff has changed with 200ms period + if (mspVtxConfigChanged && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { + + prepareMspFrame(frame); + + if (isCrsfPortConfig(portConfig)) { + mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); + } else { + mspSerialPush((serialPortIdentifier_e) portConfig->identifier, MSP_VTX_CONFIG, frame, sizeof(frame), MSP_DIRECTION_REPLY, MSP_V2_NATIVE); + } + packetCounter++; + mspVtxLastTimeUs = currentTimeUs; + mspVtxConfigChanged = false; + +#ifdef USE_CMS + mspCmsUpdateStatusString(); +#endif + } + break; + default: + mspVtxStatus = MSP_VTX_STATUS_OFFLINE; + break; + } + + DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); + DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); + DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); + DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); +} + +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) +{ + UNUSED(vtxDevice); + return VTXDEV_MSP; +} + +static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) +{ + return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; +} + +static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) +{ + UNUSED(vtxDevice); + if (band != mspConfBand || channel != mspConfChannel) { + mspVtxConfigChanged = true; + } + mspConfBand = band; + mspConfChannel = channel; +} + +static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) +{ + uint16_t powerValue; + + if (vtxCommonLookupPowerValue(vtxDevice, index, &powerValue)) { + if (powerValue != mspConfPower) { + mspVtxConfigChanged = true; + } + mspConfPower = powerValue; + } +} + +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +{ + UNUSED(vtxDevice); + if (onoff != mspConfPitMode) { + mspVtxConfigChanged = true; + } + mspConfPitMode = onoff; +} + +static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) +{ + UNUSED(vtxDevice); + if (freq != mspConfFreq) { + mspVtxConfigChanged = true; + } + mspConfFreq = freq; +} + +static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pBand = mspConfBand; + *pChannel = mspConfChannel; + return true; +} + +static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + // Special case, power not set + if (mspConfPower == 0) { + *pIndex = 0; + return true; + } + + // Lookup value in table + for (uint8_t i = 0; i < vtxTablePowerLevels; i++) { + // Find value that matches current configured power level + if (mspConfPower == vtxTablePowerValues[i]) { + // Value found, return index + *pIndex = i + 1; + return true; + } + } + + // Value not found in table + return false; +} + +static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + *pFreq = vtxCommonLookupFrequency(vtxDevice, mspConfBand, mspConfChannel); + return true; +} + +static bool vtxMspGetStatus(const vtxDevice_t *vtxDevice, unsigned *status) +{ + if (!vtxMspIsReady(vtxDevice)) { + return false; + } + + // Mirror configued pit mode state rather than use current pitmode as we + // should, otherwise the logic in vtxProcessPitMode may not get us to the + // correct state if pitmode is toggled quickly + *status = (mspConfPitMode ? VTX_STATUS_PIT_MODE : 0); + + return true; +} + +static uint8_t vtxMspGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers) +{ + if (!vtxMspIsReady(vtxDevice)) { + return 0; + } + + for (uint8_t i = 0; i < vtxTablePowerLevels; i++) { + levels[i] = vtxTablePowerValues[i]; + uint16_t power = (uint16_t)powf(10.0f, levels[i] / 10.0f); + powers[i] = power; + } + + return vtxTablePowerLevels; +} + +static const vtxVTable_t mspVTable = { + .process = vtxMspProcess, + .getDeviceType = vtxMspGetDeviceType, + .isReady = vtxMspIsReady, + .setBandAndChannel = vtxMspSetBandAndChannel, + .setPowerByIndex = vtxMspSetPowerByIndex, + .setPitMode = vtxMspSetPitMode, + .setFrequency = vtxMspSetFreq, + .getBandAndChannel = vtxMspGetBandAndChannel, + .getPowerIndex = vtxMspGetPowerIndex, + .getFrequency = vtxMspGetFreq, + .getStatus = vtxMspGetStatus, + .getPowerLevels = vtxMspGetPowerLevels, + .serializeCustomDeviceStatus = NULL, +}; + +bool vtxMspInit() +{ + // don't bother setting up this device if we don't have MSP vtx enabled + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_MSP); + if (!portConfig) { + return false; + } + + mspVtxPortIdentifier = portConfig->identifier; + + // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. +#if defined(USE_VTX_COMMON) + vtxCommonSetDevice(&vtxMsp); +#endif + + vtxInit(); + + return true; +} + +#endif diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h new file mode 100644 index 0000000000..c769b181ed --- /dev/null +++ b/src/main/io/vtx_msp.h @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +#include + +#include "build/build_config.h" + +typedef enum { + // Offline - device hasn't responded yet + MSP_VTX_STATUS_OFFLINE = 0, + MSP_VTX_STATUS_READY, +} mspVtxStatus_e; + +bool vtxMspInit(void); +void setMspVtxDeviceStatusReady(const int descriptor); +void prepareMspFrame(uint8_t *mspFrame); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 9123192aa5..9a4de85310 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -105,6 +105,7 @@ #include "io/usb_msc.h" #include "io/vtx_control.h" #include "io/vtx.h" +#include "io/vtx_msp.h" #include "msp/msp_box.h" #include "msp/msp_protocol.h" @@ -1053,10 +1054,14 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce return true; } -static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) +static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *dst) { bool unsupportedCommand = false; +#if !defined(USE_VTX_COMMON) || !defined(USE_VTX_MSP) + UNUSED(srcDesc); +#endif + switch (cmdMSP) { case MSP_STATUS_EX: case MSP_STATUS: @@ -2030,7 +2035,9 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); #endif - +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; #endif @@ -2308,6 +2315,9 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ } else { return MSP_RESULT_ERROR; } +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; @@ -2324,6 +2334,9 @@ static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_ } else { return MSP_RESULT_ERROR; } +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; #endif // USE_VTX_TABLE @@ -3271,6 +3284,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbufReadU8(src); #endif } +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; #endif @@ -3318,6 +3334,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, } else { return MSP_RESULT_ERROR; } +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; @@ -3342,6 +3361,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, } else { return MSP_RESULT_ERROR; } +#ifdef USE_VTX_MSP + setMspVtxDeviceStatusReady(srcDesc); +#endif } break; #endif @@ -4057,7 +4079,7 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa if (mspCommonProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { ret = MSP_RESULT_ACK; - } else if (mspProcessOutCommand(cmdMSP, dst)) { + } else if (mspProcessOutCommand(srcDesc, cmdMSP, dst)) { ret = MSP_RESULT_ACK; } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) { /* ret */; diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index 165558675b..88cce296db 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -290,7 +290,7 @@ void initActiveBoxIds(void) BME(BOXCAMERA3); #endif -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) BME(BOXVTXPITMODE); BME(BOXVTXCONTROLDISABLE); #endif diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 86e82d9ea3..de754c850a 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -40,6 +40,8 @@ #include "msp_serial.h" +#include "pg/msp.h" + static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, bool sharedWithTelemetry) @@ -63,7 +65,13 @@ void mspSerialAllocatePorts(void) continue; } - serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED); + portOptions_e options = SERIAL_NOT_INVERTED; + + if (mspConfig()->halfDuplex) { + options |= SERIAL_BIDIR; + } + + serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, options); if (serialPort) { bool sharedWithTelemetry = isSerialPortShared(portConfig, FUNCTION_MSP, TELEMETRY_PORT_FUNCTIONS_MASK); resetMspPort(mspPort, serialPort, sharedWithTelemetry); @@ -86,6 +94,17 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) } } +mspDescriptor_t getMspSerialPortDescriptor(const uint8_t portIdentifier) +{ + for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { + mspPort_t *candidateMspPort = &mspPorts[portIndex]; + if (candidateMspPort->port->identifier == portIdentifier) { + return candidateMspPort->descriptor; + } + } + return -1; +} + #if defined(USE_TELEMETRY) void mspSerialReleaseSharedTelemetryPorts(void) { for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { @@ -554,7 +573,7 @@ void mspSerialInit(void) mspSerialAllocatePorts(); } -int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction) +int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction, mspVersion_e mspVersion) { int ret = 0; @@ -577,7 +596,7 @@ int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int d .direction = direction, }; - ret = mspSerialEncode(mspPort, &push, MSP_V1); + ret = mspSerialEncode(mspPort, &push, mspVersion); } return ret; // return the number of bytes written } diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 48964d4c60..466b25befd 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -120,5 +120,6 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm void mspSerialAllocatePorts(void); void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); void mspSerialReleaseSharedTelemetryPorts(void); -int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction); +mspDescriptor_t getMspSerialPortDescriptor(const uint8_t portIdentifier); +int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction, mspVersion_e mspVersion); uint32_t mspSerialTxBytesFree(void); diff --git a/src/main/pg/msp.c b/src/main/pg/msp.c new file mode 100644 index 0000000000..0af6870104 --- /dev/null +++ b/src/main/pg/msp.c @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#include "platform.h" + +#include "pg/pg.h" +#include "pg/pg_ids.h" + +#include "msp.h" + +PG_REGISTER(mspConfig_t, mspConfig, PG_MSP_CONFIG, 0); diff --git a/src/main/pg/msp.h b/src/main/pg/msp.h new file mode 100644 index 0000000000..7bff150020 --- /dev/null +++ b/src/main/pg/msp.h @@ -0,0 +1,31 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/io_types.h" + +#include "pg/pg.h" + +typedef struct mspConfig_s { + uint8_t halfDuplex; // allow msp to operate in half duplex mode +} mspConfig_t; + +PG_DECLARE(mspConfig_t, mspConfig); diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h index 5e3562e09c..6162b8ff5e 100644 --- a/src/main/pg/pg_ids.h +++ b/src/main/pg/pg_ids.h @@ -153,7 +153,8 @@ #define PG_DYN_NOTCH_CONFIG 554 #define PG_RX_EXPRESSLRS_SPI_CONFIG 555 #define PG_SCHEDULER_CONFIG 556 -#define PG_BETAFLIGHT_END 556 +#define PG_MSP_CONFIG 557 +#define PG_BETAFLIGHT_END 557 // OSD configuration (subject to change) diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index c472864e10..a71a2bab7a 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -279,7 +279,7 @@ void currentMeterMSPRefresh(timeUs_t currentTimeUs) if (cmp32(currentTimeUs, streamRequestAt) > 0) { streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz - mspSerialPush(SERIAL_PORT_ALL, MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST); + mspSerialPush(SERIAL_PORT_ALL, MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST, MSP_V1); } } diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index e6f835b8de..5bff495522 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -124,6 +124,7 @@ #undef USE_VTX_CONTROL #undef USE_VTX_SMARTAUDIO #undef USE_VTX_TRAMP +#undef USE_VTX_MSP #undef USE_CAMERA_CONTROL #undef USE_BRUSHED_ESC_AUTODETECT #undef USE_GPS_RESCUE diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 4590695afa..44c690e134 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -165,6 +165,7 @@ #undef USE_VTX_TRAMP #undef USE_VTX_SMARTAUDIO #undef USE_VTX_TABLE +#undef USE_VTX_MSP #endif #if defined(USE_RX_FRSKY_SPI_D) || defined(USE_RX_FRSKY_SPI_X) || defined(USE_RX_REDPINE_SPI) diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 97505b7810..c3808c098d 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -288,6 +288,7 @@ extern uint8_t _dmaram_end__; #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP +#define USE_VTX_MSP #endif #if ((TARGET_FLASH_SIZE > 256 && !defined(FEATURE_CUT_LEVEL)) || (FEATURE_CUT_LEVEL < 10)) diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 2e907bfaff..3868d29746 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -159,6 +159,11 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const Seri // IBUS serial RX & telemetry return true; } +#endif +#if defined(USE_MSP_OVER_TELEMETRY) && defined(USE_VTX_MSP) + if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP) { + return true; + } #endif return false; } diff --git a/src/test/Makefile b/src/test/Makefile index ec024bce4b..6497ec8a6f 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -462,6 +462,23 @@ rx_spi_expresslrs_telemetry_unittest_DEFINES := \ USE_GPS= \ USE_MSP_OVER_TELEMETRY= \ +vtx_msp_unittest_SRC := \ + $(USER_DIR)/common/crc.c \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/fc/runtime_config.c \ + $(USER_DIR)/drivers/vtx_common.c \ + $(USER_DIR)/drivers/vtx_table.c \ + $(USER_DIR)/pg/vtx_table.c \ + $(USER_DIR)/io/vtx_control.c \ + $(USER_DIR)/io/vtx.c \ + $(USER_DIR)/io/vtx_msp.c \ + +vtx_msp_unittest_DEFINES := \ + USE_VTX_COMMON= \ + USE_VTX_CONTROL= \ + USE_VTX_TABLE= \ + USE_VTX_MSP= + # Please tweak the following variable definitions as needed by your # project, except GTEST_HEADERS, which you can use in your own targets # but shouldn't modify. diff --git a/src/test/unit/vtx_msp_unittest.cc b/src/test/unit/vtx_msp_unittest.cc new file mode 100644 index 0000000000..99a5816419 --- /dev/null +++ b/src/test/unit/vtx_msp_unittest.cc @@ -0,0 +1,116 @@ +/* + * 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 + +extern "C" { + #include "platform.h" + + #include "io/vtx.h" + #include "io/vtx_msp.h" + #include "pg/vtx_table.h" + + static uint8_t mspFrame[15]; + extern mspVtxStatus_e mspVtxStatus; +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +//make clean test_vtx_msp_unittest +TEST(VtxMspUnitTest, TestVtxMspPacket) +{ + vtxSettingsConfigMutable()->band = 1; + vtxSettingsConfigMutable()->channel = 2; + vtxSettingsConfigMutable()->power = 3; + vtxSettingsConfigMutable()->freq = 5800; + vtxSettingsConfigMutable()->pitModeFreq = 5300; + vtxSettingsConfigMutable()->lowPowerDisarm = VTX_LOW_POWER_DISARM_ALWAYS; + + vtxTableConfigMutable()->bands = 4; + vtxTableConfigMutable()->channels = 5; + vtxTableConfigMutable()->powerLevels = 6; + + uint8_t expectedFrame[15] = {5, 1, 2, 1, 0, 168, 22, 0, 1, 180, 20, 1, 4, 5, 6}; + + prepareMspFrame(mspFrame); + + for (int i = 0; i < 15; i++) { + EXPECT_EQ(expectedFrame[i], mspFrame[i]); + } +} + +TEST(VtxMspUnitTest, TestVtxMspReady) +{ + mspVtxStatus = MSP_VTX_STATUS_OFFLINE; + vtxTableConfigMutable()->bands = 0; + vtxTableConfigMutable()->channels = 0; + vtxTableConfigMutable()->powerLevels = 0; + + setMspVtxDeviceStatusReady(0); + + EXPECT_EQ(MSP_VTX_STATUS_OFFLINE, mspVtxStatus); + + vtxTableConfigMutable()->bands = 1; + vtxTableConfigMutable()->channels = 1; + vtxTableConfigMutable()->powerLevels = 1; + + setMspVtxDeviceStatusReady(1); // testing wrong descriptor + + EXPECT_EQ(MSP_VTX_STATUS_OFFLINE, mspVtxStatus); + + setMspVtxDeviceStatusReady(0); + + EXPECT_EQ(MSP_VTX_STATUS_READY, mspVtxStatus); +} + +// STUBS + +extern "C" { + + #include "build/debug.h" + #include "fc/rc_modes.h" + #include "rx/rx.h" + #include "io/serial.h" + #include "msp/msp.h" + + uint8_t debugMode = 0; + int16_t debug[DEBUG16_VALUE_COUNT]; + + uint8_t cliMode = 0; + rxRuntimeState_t rxRuntimeState = rxRuntimeState_t(); + static serialPortConfig_t *findSerialPortConfig_stub_retval; + + bool IS_RC_MODE_ACTIVE(boxId_e) {return false;} + void beeperConfirmationBeeps(uint8_t) {} + void saveConfigAndNotify(void) {} + void crsfRxSendTelemetryData(void) {} + void crsfRxWriteTelemetryData(const void *, int) {} + bool failsafeIsActive(void) {return false;} + const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return findSerialPortConfig_stub_retval;} + bool isRangeActive(uint8_t , const channelRange_t *) {return true;} + int mspSerialPush(serialPortIdentifier_e, uint8_t, uint8_t *, int, mspDirection_e, mspVersion_e) {return 0;} + void tfp_sprintf(char *, char*, ...) {} + + mspDescriptor_t getMspSerialPortDescriptor(const uint8_t ) {return 0;} + mspDescriptor_t getMspTelemetryDescriptor(void) {return 0;} + + void mspCmsUpdateStatusString(void) {} +}