fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 17:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

[vtx] Add VTX driver with Tramp and SmartAudio support

Browse Source
This commit is contained in:
Niklas Hauser
2026-01-26 11:05:22 +01:00
committed by Niklas Hauser
parent fad4450d7d
commit c0c265cd1f
28 changed files with 3397 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/rc/crsf_rc/CrsfParser.cpp
+65
View File
@@ -46,6 +46,9 @@
#include "QueueBuffer.hpp"
#include "CrsfParser.hpp"
#include "Crc8.hpp"
#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT
#include <px4_platform_common/param.h>
#endif
#define CRSF_CHANNEL_VALUE_MIN 172
#define CRSF_CHANNEL_VALUE_MAX 1811
@@ -60,6 +63,7 @@ enum CRSF_PAYLOAD_SIZE {
CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX = -1,
CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22,
CRSF_PAYLOAD_SIZE_ATTITUDE = 6,
CRSF_PAYLOAD_SIZE_MSP_WRITE = -1, // -1 means variable length
CRSF_PAYLOAD_SIZE_ELRS_STATUS = -1, // unclear how large this message is
};
@@ -127,12 +131,18 @@ static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPac
static bool ProcessLinkStatistics(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
static bool ProcessLinkStatisticsTx(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
static bool ProcessElrsStatus(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT
static bool ProcessMspWrite(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet);
#endif
static const CrsfPacketDescriptor_t crsf_packet_descriptors[] = {
{CRSF_PACKET_TYPE_RC_CHANNELS_PACKED, CRSF_PAYLOAD_SIZE_RC_CHANNELS, ProcessChannelData},
{CRSF_PACKET_TYPE_LINK_STATISTICS, CRSF_PAYLOAD_SIZE_LINK_STATISTICS, ProcessLinkStatistics},
{CRSF_PACKET_TYPE_LINK_STATISTICS_TX, CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX, ProcessLinkStatisticsTx},
{CRSF_PACKET_TYPE_ELRS_STATUS, CRSF_PAYLOAD_SIZE_ELRS_STATUS, ProcessElrsStatus},
#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT
{CRSF_PACKET_TYPE_MSP_WRITE, CRSF_PAYLOAD_SIZE_MSP_WRITE, ProcessMspWrite},
#endif
};
#define CRSF_PACKET_DESCRIPTOR_COUNT (sizeof(crsf_packet_descriptors) / sizeof(CrsfPacketDescriptor_t))
@@ -259,6 +269,61 @@ static bool ProcessElrsStatus(const uint8_t *data, const uint32_t size, CrsfPack
return true;
}
#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT
static bool ProcessMspWrite(const uint8_t *data, const uint32_t size, CrsfPacket_t *const)
{
// Write the band/channel into the parameters, so it is thread-safe
// data contains the following:
// 0: CRSF v3: destination
// 1: CRSF v3: origin
// 2: CRSF v3: status
// 3: MSP: size
// 4: MSP: command
// 5: MSP: data[≤57]
// Try: crsf_rc inject 0x7C 0xC8 0xEA 0x30 0x4 0x59 0x22 0x0 0x1 0x0
int32_t map_config{};
param_get(int(px4::params::VTX_MAP_CONFIG), &map_config);
if (map_config == 0) {
// no mapping, just return
return false;
}
if (data[2] == 0x30 && data[4] == 0x59) {
const uint8_t length = data[3];
if (map_config == 1 || map_config == 2) {
// Status = bit4=new frame, bit5,6=MSPv1
// MSP command 0x59 is MSP_SET_VTX_CONFIG
uint32_t frequency = (data[6] << 8) | data[5];
if (frequency <= 0x3f) {
// first byte contains band and channel: 0b00bb'bccc
const int32_t band = (data[5] >> 3) & 0x07;
const int32_t channel = data[5] & 0x07;
param_set_no_notification(int(px4::params::VTX_BAND), &band);
param_set_no_notification(int(px4::params::VTX_CHANNEL), &channel);
frequency = 0; // Disable the frequency override
}
param_set_no_notification(int(px4::params::VTX_FREQUENCY), &frequency);
}
if (length > 2 && (map_config == 1 || map_config == 3)) {
const int32_t pit_mode = (data[8] || (data[7] == 0)) ? 1 : 0;
param_set_no_notification(int(px4::params::VTX_PIT_MODE), &pit_mode);
const int32_t power{pit_mode ? 0 : data[7] - 1};
param_set_no_notification(int(px4::params::VTX_POWER), &power);
}
}
// nothing else is implemented yet
return false;
}
#endif
static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type)
{
uint32_t i;