Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Merge pull request #11705 from phobos-/msp_vtx

Browse source 
VTX Device over MSP
This commit is contained in:
haslinghuis 2022-09-12 18:11:29 +02:00 • committed by GitHub
commit abfa5faa1e
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
31 changed files with 1036 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/build/debug.c
View file

@ -105,4 +105,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"GPS_RESCUE_HEADING",
"GPS_RESCUE_TRACKING",
"ATTITUDE",
"VTX_MSP"
};

1
src/main/build/debug.h
View file

@ -103,6 +103,7 @@ typedef enum {
DEBUG_GPS_RESCUE_HEADING,
DEBUG_GPS_RESCUE_TRACKING,
DEBUG_ATTITUDE,
DEBUG_VTX_MSP,
DEBUG_COUNT
} debugType_e;

3
src/main/cli/settings.c
View file

@ -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) },

2
src/main/cms/cms_menu_main.c
View file

@ -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

8
src/main/cms/cms_menu_vtx_common.c
View file

@ -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);

285
src/main/cms/cms_menu_vtx_msp.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>
#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

27
src/main/cms/cms_menu_vtx_msp.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "cms/cms.h"
#include "cms/cms_types.h"
extern CMS_Menu cmsx_menuVtxMsp;
void mspCmsUpdateStatusString(void);

3
src/main/drivers/vtx_common.h
View file

@ -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;

5
src/main/fc/init.c
View file

@ -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

2
src/main/fc/tasks.c
View file

@ -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

2
src/main/io/displayport_msp.c
View file

@ -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)

11
src/main/io/serial.c
View file

@ -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) {

1
src/main/io/serial.h
View file

@ -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)

377
src/main/io/vtx_msp.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
/* Created by phobos- */
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include <ctype.h>
#include <string.h>
#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

35
src/main/io/vtx_msp.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <stdint.h>
#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);

28
src/main/msp/msp.c
View file

@ -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 */;

2
src/main/msp/msp_box.c
View file

@ -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

25
src/main/msp/msp_serial.c
View file

@ -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
}

3
src/main/msp/msp_serial.h
View file

@ -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);

28
src/main/pg/msp.c Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "msp.h"
PG_REGISTER(mspConfig_t, mspConfig, PG_MSP_CONFIG, 0);

31
src/main/pg/msp.h Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#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);

3
src/main/pg/pg_ids.h
View file

@ -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)

2
src/main/sensors/current.c
View file

@ -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);
}
}

1
src/main/target/SITL/target.h
View file

@ -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

1
src/main/target/common_post.h
View file

@ -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)

1
src/main/target/common_pre.h
View file

@ -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))

5
src/main/telemetry/telemetry.c
View file

@ -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;
}

17
src/test/Makefile
View file

@ -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.

116
src/test/unit/vtx_msp_unittest.cc Normal file
View file

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
#include <stdbool.h>
#include <limits.h>
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) {}
}