Added VTX device status to MSP
This commit is contained in:
parent
32373e3e05
commit
9db5b502b9
|
@ -228,4 +228,71 @@ bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t
|
|||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static void vtxCommonSerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
|
||||
{
|
||||
const bool customDeviceStatusAvailable = vtxDevice && vtxDevice->vTable->serializeCustomDeviceStatus;
|
||||
|
||||
if (customDeviceStatusAvailable) {
|
||||
vtxDevice->vTable->serializeCustomDeviceStatus(vtxDevice, dst);
|
||||
} else {
|
||||
sbufWriteU8(dst, 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void vtxCommonSerializePowerLevels(const vtxDevice_t *vtxDevice, sbuf_t *dst)
|
||||
{
|
||||
uint16_t levels[VTX_TABLE_MAX_POWER_LEVELS];
|
||||
uint16_t powers[VTX_TABLE_MAX_POWER_LEVELS];
|
||||
|
||||
const uint8_t powerLevelCount = vtxCommonGetVTXPowerLevels(vtxDevice, levels, powers);
|
||||
|
||||
sbufWriteU8(dst, powerLevelCount);
|
||||
|
||||
for (int i = 0; i < powerLevelCount; i++) {
|
||||
sbufWriteU16(dst, levels[i]);
|
||||
sbufWriteU16(dst, powers[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
|
||||
{
|
||||
if (vtxDevice) {
|
||||
const vtxDevType_e vtxType = vtxCommonGetDeviceType(vtxDevice);
|
||||
const bool deviceReady = vtxCommonDeviceIsReady(vtxDevice);
|
||||
|
||||
uint8_t band = 0;
|
||||
uint8_t channel = 0;
|
||||
const bool bandAndChannelAvailable = vtxCommonGetBandAndChannel(vtxDevice, &band, &channel);
|
||||
|
||||
uint8_t powerIndex = 0;
|
||||
const bool powerIndexAvailable = vtxCommonGetPowerIndex(vtxDevice, &powerIndex);
|
||||
|
||||
uint16_t frequency = 0;
|
||||
const bool frequencyAvailable = vtxCommonGetFrequency(vtxDevice, &frequency);
|
||||
|
||||
unsigned vtxStatus = 0; // pitmode and/or locked
|
||||
const bool vtxStatusAvailable = vtxCommonGetStatus(vtxDevice, &vtxStatus);
|
||||
|
||||
sbufWriteU8(dst, vtxType);
|
||||
sbufWriteU8(dst, deviceReady);
|
||||
|
||||
sbufWriteU8(dst, bandAndChannelAvailable);
|
||||
sbufWriteU8(dst, band);
|
||||
sbufWriteU8(dst, channel);
|
||||
|
||||
sbufWriteU8(dst, powerIndexAvailable);
|
||||
sbufWriteU8(dst, powerIndex);
|
||||
|
||||
sbufWriteU8(dst, frequencyAvailable);
|
||||
sbufWriteU16(dst, frequency);
|
||||
|
||||
sbufWriteU8(dst, vtxStatusAvailable);
|
||||
sbufWriteU32(dst, vtxStatus);
|
||||
|
||||
vtxCommonSerializePowerLevels(vtxDevice, dst);
|
||||
vtxCommonSerializeCustomDeviceStatus(vtxDevice, dst);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
#include "common/time.h"
|
||||
#include "common/streambuf.h"
|
||||
|
||||
#define VTX_SETTINGS_MAX_FREQUENCY_MHZ 5999 //max freq (in MHz) for 'vtx_freq' setting
|
||||
|
||||
|
@ -112,6 +113,8 @@ typedef struct vtxVTable_s {
|
|||
bool (*getFrequency)(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
|
||||
bool (*getStatus)(const vtxDevice_t *vtxDevice, unsigned *status);
|
||||
uint8_t (*getPowerLevels)(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers);
|
||||
|
||||
void (*serializeCustomDeviceStatus)(const vtxDevice_t *vtxDevice, sbuf_t *dst);
|
||||
} vtxVTable_t;
|
||||
|
||||
// 3.1.0
|
||||
|
@ -127,15 +130,19 @@ vtxDevice_t *vtxCommonDevice(void);
|
|||
void vtxCommonProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs);
|
||||
vtxDevType_e vtxCommonGetDeviceType(const vtxDevice_t *vtxDevice);
|
||||
bool vtxCommonDeviceIsReady(const vtxDevice_t *vtxDevice);
|
||||
|
||||
void vtxCommonSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel);
|
||||
void vtxCommonSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t level);
|
||||
void vtxCommonSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff);
|
||||
void vtxCommonSetFrequency(vtxDevice_t *vtxDevice, uint16_t freq);
|
||||
|
||||
bool vtxCommonGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel);
|
||||
bool vtxCommonGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex);
|
||||
bool vtxCommonGetFrequency(const vtxDevice_t *vtxDevice, uint16_t *pFreq);
|
||||
bool vtxCommonGetStatus(const vtxDevice_t *vtxDevice, unsigned *status);
|
||||
uint8_t vtxCommonGetVTXPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *levels, uint16_t *powers);
|
||||
// end of VTable functions
|
||||
|
||||
const char *vtxCommonLookupBandName(const vtxDevice_t *vtxDevice, int band);
|
||||
char vtxCommonLookupBandLetter(const vtxDevice_t *vtxDevice, int band);
|
||||
char vtxCommonGetBandLetter(const vtxDevice_t *vtxDevice, int band);
|
||||
|
@ -144,3 +151,4 @@ uint16_t vtxCommonLookupFrequency(const vtxDevice_t *vtxDevice, int band, int ch
|
|||
void vtxCommonLookupBandChan(const vtxDevice_t *vtxDevice, uint16_t freq, uint8_t *pBand, uint8_t *pChannel);
|
||||
const char *vtxCommonLookupPowerName(const vtxDevice_t *vtxDevice, int index);
|
||||
bool vtxCommonLookupPowerValue(const vtxDevice_t *vtxDevice, int index, uint16_t *pPowerValue);
|
||||
void vtxCommonSerializeDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst);
|
||||
|
|
|
@ -245,7 +245,9 @@ static vtxVTable_t rtc6705VTable = {
|
|||
.getFrequency = vtxRTC6705GetFreq,
|
||||
.getStatus = vtxRTC6705GetStatus,
|
||||
.getPowerLevels = vtxRTC6705GetPowerLevels,
|
||||
.serializeCustomDeviceStatus = NULL,
|
||||
};
|
||||
|
||||
#endif // VTX_COMMON
|
||||
|
||||
#endif // VTX_RTC6705
|
||||
|
|
|
@ -1047,6 +1047,20 @@ static uint8_t vtxSAGetPowerLevels(const vtxDevice_t *vtxDevice, uint16_t *level
|
|||
return saSupportedNumPowerLevels;
|
||||
}
|
||||
|
||||
#define VTX_CUSTOM_DEVICE_STATUS_SIZE 5
|
||||
|
||||
static void vtxSASerializeCustomDeviceStatus(const vtxDevice_t *vtxDevice, sbuf_t *dst)
|
||||
{
|
||||
UNUSED(vtxDevice);
|
||||
sbufWriteU8(dst, VTX_CUSTOM_DEVICE_STATUS_SIZE);
|
||||
sbufWriteU8(dst, saDevice.version);
|
||||
sbufWriteU8(dst, saDevice.mode);
|
||||
sbufWriteU16(dst, saDevice.orfreq); // pit frequency
|
||||
sbufWriteU8(dst, saDevice.willBootIntoPitMode);
|
||||
}
|
||||
|
||||
#undef VTX_CUSTOM_DEVICE_STATUS_SIZE
|
||||
|
||||
static const vtxVTable_t saVTable = {
|
||||
.process = vtxSAProcess,
|
||||
.getDeviceType = vtxSAGetDeviceType,
|
||||
|
@ -1060,6 +1074,7 @@ static const vtxVTable_t saVTable = {
|
|||
.getFrequency = vtxSAGetFreq,
|
||||
.getStatus = vtxSAGetStatus,
|
||||
.getPowerLevels = vtxSAGetPowerLevels,
|
||||
.serializeCustomDeviceStatus = vtxSASerializeCustomDeviceStatus,
|
||||
};
|
||||
#endif // VTX_COMMON
|
||||
|
||||
|
|
|
@ -643,6 +643,7 @@ static const vtxVTable_t trampVTable = {
|
|||
.getFrequency = vtxTrampGetFreq,
|
||||
.getStatus = vtxTrampGetStatus,
|
||||
.getPowerLevels = vtxTrampGetPowerLevels,
|
||||
.serializeCustomDeviceStatus = NULL,
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
|
@ -1201,6 +1201,15 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
}
|
||||
break;
|
||||
|
||||
#ifdef USE_VTX_COMMON
|
||||
case MSP2_GET_VTX_DEVICE_STATUS:
|
||||
{
|
||||
const vtxDevice_t *vtxDevice = vtxCommonDevice();
|
||||
vtxCommonSerializeDeviceStatus(vtxDevice, dst);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
case MSP_RC:
|
||||
for (int i = 0; i < rxRuntimeState.channelCount; i++) {
|
||||
sbufWriteU16(dst, rcData[i]);
|
||||
|
|
|
@ -22,4 +22,4 @@
|
|||
#define MSP2_MOTOR_OUTPUT_REORDERING 0x3001
|
||||
#define MSP2_SET_MOTOR_OUTPUT_REORDERING 0x3002
|
||||
#define MSP2_SEND_DSHOT_COMMAND 0x3003
|
||||
|
||||
#define MSP2_GET_VTX_DEVICE_STATUS 0x3004
|
||||
|
|
Loading…
Reference in New Issue