msp_osd: Add VTX config, stick commands and arming status

- Adds MSPv1 rx parsing to fetch VTX config
 - Allows to inspect and change VTX channel through CLI
 - Forward MSP_RC for stick commands osd
 - Forward MSP_STATUS for arming status for PIT and LP mode
This commit is contained in:
Peter van der Perk 2025-06-05 22:02:59 +02:00 committed by Daniel Agar
parent 437b74836c
commit c45ba5a57b
8 changed files with 394 additions and 16 deletions

View File

@ -39,6 +39,7 @@
#include <float.h>
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@ -62,7 +63,7 @@ struct msp_message_descriptor_t {
uint8_t message_size;
};
#define MSP_DESCRIPTOR_COUNT 11
#define MSP_DESCRIPTOR_COUNT 12
const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
{MSP_OSD_CONFIG, true, sizeof(msp_osd_config_t)},
{MSP_NAME, true, sizeof(msp_name_t)},
@ -75,10 +76,9 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
{MSP_COMP_GPS, true, sizeof(msp_comp_gps_t)},
{MSP_ESC_SENSOR_DATA, true, sizeof(msp_esc_sensor_data_dji_t)},
{MSP_MOTOR_TELEMETRY, true, sizeof(msp_motor_telemetry_t)},
{MSP_RC, true, sizeof(msp_rc_t)},
};
#define MSP_FRAME_START_SIZE 5
#define MSP_CRC_SIZE 1
bool MspV1::Send(const uint8_t message_id, const void *payload)
{
uint32_t payload_size = 0;
@ -149,3 +149,76 @@ bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload
int packet_size = MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
return write(_fd, packet, packet_size) == packet_size;
}
int MspV1::Receive(uint8_t *payload, uint8_t *message_id)
{
uint8_t payload_size;
uint8_t crc;
uint8_t calc_crc;
int ret;
while (!has_header) {
int bytes_available = 0;
if (ioctl(_fd, FIONREAD, &bytes_available) < 0) {
return -EIO;
}
if (bytes_available < 5) {
return -EWOULDBLOCK;
}
while (bytes_available > 4) {
if ((ret = read(_fd, header, 1)) != 1) {
return ret;
}
bytes_available--;
if (header[0] == '$') {
break;
}
}
if (header[0] != '$') {
return -EWOULDBLOCK;
}
if ((ret = read(_fd, &header[1], 4)) != 4) {
return ret;
}
if (header[0] == '$' && header[1] == 'M' && header[2] == '<') {
has_header = true;
}
}
payload_size = header[3];
*message_id = header[4];
if ((ret = read(_fd, payload, payload_size + MSP_CRC_SIZE)) != payload_size + MSP_CRC_SIZE) {
if (ret != -EWOULDBLOCK) {
has_header = false;
}
return ret;
}
has_header = false;
crc = payload[payload_size];
calc_crc = payload_size ^ header[4];
for (int i = 0; i < payload_size; i++) {
calc_crc ^= payload[i];
}
if (calc_crc != crc) {
return -EINVAL;
}
return payload_size;
}

View File

@ -33,6 +33,9 @@
#pragma once
#define MSP_FRAME_START_SIZE 5
#define MSP_CRC_SIZE 1
class MspV1
{
public:
@ -40,7 +43,10 @@ public:
int GetMessageSize(int message_type);
bool Send(const uint8_t message_id, const void *payload);
bool Send(const uint8_t message_id, const void *payload, uint32_t payload_size);
int Receive(uint8_t *payload, uint8_t *message_id);
private:
int _fd{-1};
uint8_t header[MSP_FRAME_START_SIZE + MSP_CRC_SIZE];
bool has_header{false};
};

View File

@ -97,3 +97,14 @@ parameters:
min: 100
max: 10000
default: 500
# RC Stick input
OSD_RC_STICK:
description:
short: OSD RC Stick commands
long: |
Forward RC stick input to VTX when disarmed
type: int32
min: 0
max: 1
default: 1

View File

@ -48,6 +48,8 @@
#define MSP_ARMING_CONFIG 61
#define MSP_RX_MAP 64 // get channel map (also returns number of channels total)
#define MSP_LOOP_TIME 73 // FC cycle time i.e looptime parameter
#define MSP_GET_VTX_CONFIG 88
#define MSP_SET_VTX_CONFIG 89
#define MSP_STATUS 101
#define MSP_RAW_IMU 102
#define MSP_SERVO 103
@ -75,10 +77,12 @@
#define MSP_SET_PID 202 // set P I D coeff
// commands
#define MSP_SET_HEAD 211 // define a new heading hold direction
#define MSP_SET_RAW_RC 200 // 8 rc chan
#define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed
#define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags)
#define MSP_SET_HEAD 211 // define a new heading hold direction
#define MSP_SET_RAW_RC 200 // 8 rc chan
#define MSP_SET_RAW_GPS 201 // fix, numsat, lat, lon, alt, speed
#define MSP_SET_WP 209 // sets a given WP (WP#, lat, lon, alt, flags)
#define MSP_SET_VTXTABLE_BAND 227
#define MSP_SET_VTXTABLE_POWERLEVEL 228
// bits of getActiveModes() return value
#define MSP_MODE_ARM 0
@ -895,6 +899,58 @@ struct msp_status_BF_t {
uint8_t extra_flags;
} __attribute__((packed));
struct msp_set_vtx_config_t {
uint16_t new_freq; // if setting frequency then full uint16 is the frequency in MHz (ie. 5800)
//if setting band channel than band is high 8 bits and channel is low 8 bits
uint8_t power_level;
uint8_t pit_mode; // 0 = off, 1 = on
uint8_t low_power_disarm;
uint16_t pit_freq;
uint8_t user_band;
uint8_t user_channel;
uint16_t user_freq; // in MHz, 0 if using band & channel
uint8_t band_count;
uint8_t channel_count;
uint8_t power_count;
uint8_t clear_vtxtable; // Bool
} __attribute__((packed));
struct msp_get_vtx_config_t {
uint8_t vtx_type;
uint8_t band;
uint8_t channel;
uint8_t power_index;
uint8_t pit_mode; // 0 = off, 1 = on
uint16_t freq; // in MHz, 0 if using band & channel
uint8_t device_ready;
uint8_t low_power_disarm;
} __attribute__((packed));
struct msp_set_vtxtable_powerlevel_t {
uint8_t index;
uint16_t power_value;
uint8_t power_label_length;
uint8_t power_label_name[3];
} __attribute__((packed));
#define VTX_TABLE_BAND_NAME_LENGTH 8
#define VTXDEV_MSP 5
//29 bytes
struct msp_set_vtxtable_band_t {
uint8_t band;
uint8_t band_name_length;
uint8_t band_label_name[VTX_TABLE_BAND_NAME_LENGTH];
uint8_t band_letter;
uint8_t is_factory_band;
uint8_t channel_count;
uint16_t frequency[8];
} __attribute__((packed));
////ArduPlane
enum arduPlaneModes_e {
MANUAL = 0,

View File

@ -47,6 +47,7 @@
#include <unistd.h>
#include <termios.h>
#include <string.h>
#include <ctype.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/log.h>
@ -239,6 +240,7 @@ void MspOsd::SendConfig()
_msp.Send(MSP_OSD_CONFIG, &msp_osd_config);
}
// extract it to MSPOSD_BF_Run() and MSPOSD_DJIFPV_Run() for compatibility?
void MspOsd::Run()
{
@ -280,6 +282,23 @@ void MspOsd::Run()
_is_initialized = true;
}
if (change_channel) {
msp_get_vtx_config_t vtx_set_config{0};
vtx_set_config.low_power_disarm = vtx_config.low_power_disarm;
vtx_set_config.pit_mode = vtx_config.pit_mode;
vtx_set_config.vtx_type = VTXDEV_MSP;
vtx_set_config.band = vtx_config.user_band;
vtx_set_config.channel = vtx_config.user_channel;
this->Send(MSP_GET_VTX_CONFIG, &vtx_set_config, sizeof(msp_get_vtx_config_t));
change_channel = false;
}
this->Receive();
if (!has_vtx_config) {
this->Send(MSP_GET_VTX_CONFIG, nullptr, 0);
}
// avoid premature pessimization; if skip processing if we're effectively disabled
if (_param_osd_symbols.get() == 0) {
return;
@ -420,6 +439,32 @@ void MspOsd::Run()
{
}
// MSP_RC
{
if (_param_osd_rc_stick.get() == 1) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
input_rc_s input_rc{};
_input_rc_sub.copy(&input_rc);
const auto msg = msp_osd::construct_MSP_RC(input_rc);
this->Send(MSP_RC, &msg, sizeof(msp_rc_t));
}
}
}
// MSP_STATUS
{
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
const auto msg = msp_osd::construct_MSP_STATUS(vehicle_status);
this->Send(MSP_STATUS, &msg, sizeof(msp_status_t));
}
subcmd = MSP_DP_DRAW_SCREEN;
this->Send(MSP_CMD_DISPLAYPORT, &subcmd, 1);
}
@ -443,6 +488,58 @@ void MspOsd::Send(const unsigned int message_type, const void *payload, int32_t
}
}
void MspOsd::Receive()
{
uint8_t packet[255];
uint8_t message_id;
int ret;
while ((ret = _msp.Receive(packet, &message_id)) != -EWOULDBLOCK) {
if (ret >= 0) {
switch (message_id) {
case MSP_SET_VTX_CONFIG: {
if (ret == 0xF) {
memcpy((void *)&vtx_config, packet, sizeof(vtx_config));
has_vtx_config = true;
}
break;
}
case MSP_SET_VTXTABLE_BAND: {
msp_set_vtxtable_band_t *band_info = (msp_set_vtxtable_band_t *)&packet[0];
// Only supported fixed name lenght and < 8 channels for now
if (band_info->band <= BAND_COUNT && band_info->band_name_length == 8 && band_info->channel_count <= 8) {
memcpy((void *)&vtx_bands[band_info->band - 1], packet, sizeof(msp_set_vtxtable_band_t));
if (has_vtx_config && band_info->band == vtx_config.band_count) {
has_vtx_bands = true;
}
}
break;
}
case MSP_SET_VTXTABLE_POWERLEVEL: {
if ((packet[0] - 1) < POWER_LEVEL_COUNT) {
memcpy((void *)&power_levels[packet[0] - 1], packet, sizeof(msp_set_vtxtable_powerlevel_t));
has_power_config = true;
}
break;
}
default:
break;
}
}
}
}
void MspOsd::parameters_update()
{
// update our display rate and dwell time
@ -523,11 +620,102 @@ int MspOsd::print_status()
_display.get(msg, hrt_absolute_time());
PX4_INFO("Current message: \n\t%s", msg);
if (has_vtx_config) {
PX4_INFO("=== VTX Configuration ===");
if (has_vtx_bands) {
PX4_INFO("Channel: %c%u", vtx_bands[vtx_config.user_band - 1].band_letter, vtx_config.user_channel);
} else {
PX4_INFO("Band: %u", vtx_config.user_band);
PX4_INFO("Channel: %u", vtx_config.user_channel);
}
PX4_INFO("Frequency: %u MHz", vtx_config.user_freq);
if (has_power_config && (vtx_config.power_level - 1) < POWER_LEVEL_COUNT) {
PX4_INFO("Transmit power: %.*s mW", power_levels[vtx_config.power_level - 1].power_label_length,
power_levels[vtx_config.power_level - 1].power_label_name);
} else {
PX4_INFO("Power Level: %u/%u", vtx_config.power_level, vtx_config.power_count);
}
PX4_INFO("PIT Mode: %s", vtx_config.pit_mode ? "On" : "Off");
const char *disarm_modes[] = {
"Off",
"Always",
"Until First Arm"
};
if (vtx_config.low_power_disarm < 3) {
PX4_INFO("Low Power Disarm: %s", disarm_modes[vtx_config.low_power_disarm]);
} else {
PX4_INFO("Low Power Disarm: Unknown (%u)", vtx_config.low_power_disarm);
}
PX4_INFO("PIT Frequency: %u MHz", vtx_config.pit_freq);
} else {
PX4_INFO("No VTX Configuration available, can't do channel switching");
}
return 0;
}
int MspOsd::set_channel(char *new_channel)
{
char band_letter = toupper(new_channel[0]);
if (!has_vtx_bands) {
return -2;
}
for (int i = 0; i < BAND_COUNT; i++) {
if (vtx_bands[i].band != 0) {
if (band_letter == toupper(vtx_bands[i].band_letter)) {
int channel = atoi(&new_channel[1]);
if (channel > 0 && channel <= vtx_config.channel_count && vtx_bands[i].frequency[channel - 1] != 0) {
vtx_config.user_band = vtx_bands[i].band;
vtx_config.user_channel = channel;
vtx_config.user_freq = vtx_bands[i].frequency[channel - 1];
change_channel = true;
return 0;
}
}
}
}
return -1;
}
int MspOsd::custom_command(int argc, char *argv[])
{
if (argc > 0 && strcmp("channel", argv[0]) == 0) {
if (argc == 1) {
PX4_INFO("Please provide a channel");
} else if (is_running() && _object.load()) {
MspOsd *object = _object.load();
int ret = object->set_channel(argv[1]);
if (ret == -1) {
PX4_INFO("Channel not found");
} else if (ret == -2) {
PX4_INFO("No VTX Channel table available");
}
} else {
PX4_INFO("not running");
}
}
return 0;
}
@ -553,6 +741,7 @@ $ msp_osd
PRINT_MODULE_USAGE_NAME("msp_osd", "driver");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
PRINT_MODULE_USAGE_COMMAND_DESCR("channel", "Change VTX channel");
return 0;
}

View File

@ -64,6 +64,9 @@ using namespace time_literals;
// location to "hide" unused display elements
#define LOCATION_HIDDEN 234;
#define POWER_LEVEL_COUNT 5
#define BAND_COUNT 7
struct PerformanceData {
bool initialization_problems{false};
long unsigned int successful_sends{0};
@ -118,6 +121,8 @@ public:
/** @see ModuleBase::print_status() */
int print_status() override;
int set_channel(char *new_channel);
private:
void Run() override;
@ -125,6 +130,9 @@ private:
void Send(const unsigned int message_type, const void *payload);
void Send(const unsigned int message_type, const void *payload, int32_t payload_size);
// receive vtx data
void Receive();
// send full configuration to MSP (triggers the actual update)
void SendConfig();
void SendTelemetry();
@ -166,10 +174,19 @@ private:
(ParamInt<px4::params::OSD_CH_HEIGHT>) _param_osd_ch_height,
(ParamInt<px4::params::OSD_SCROLL_RATE>) _param_osd_scroll_rate,
(ParamInt<px4::params::OSD_DWELL_TIME>) _param_osd_dwell_time,
(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level
(ParamInt<px4::params::OSD_LOG_LEVEL>) _param_osd_log_level,
(ParamInt<px4::params::OSD_RC_STICK>) _param_osd_rc_stick
)
// metadata
char _device[64] {};
PerformanceData _performance_data{};
msp_set_vtx_config_t vtx_config;
msp_set_vtxtable_powerlevel_t power_levels[POWER_LEVEL_COUNT];
msp_set_vtxtable_band_t vtx_bands[BAND_COUNT] {};
bool has_vtx_config {false};
bool has_power_config {false};
bool has_vtx_bands {false};
bool change_channel {false};
};

View File

@ -214,14 +214,8 @@ msp_rendor_rssi_t construct_rendor_RSSI(const input_rc_s &input_rc)
rssi.screenYPosition = 0x02;
rssi.screenXPosition = 0x02;
int len = snprintf(&rssi.str[0], sizeof(rssi.str) - 1, "%d", input_rc.link_quality);
if (len >= 3) {
rssi.str[3] = '%';
} else {
rssi.str[len] = '%';
}
snprintf(&rssi.str[0], sizeof(rssi.str), "%3d", input_rc.link_quality);
rssi.str[3] = '%';
return rssi;
}
@ -569,4 +563,30 @@ msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA()
return esc_sensor_data;
}
msp_rc_t construct_MSP_RC(const input_rc_s &input_rc)
{
// initialize result
msp_rc_t rc;
rc.channelValue[0] = input_rc.values[0]; // roll
rc.channelValue[1] = input_rc.values[1]; // pitch
rc.channelValue[2] = input_rc.values[3]; // yaw
rc.channelValue[3] = input_rc.values[2]; // Throttle
return rc;
}
msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status)
{
// initialize result
msp_status_t status{0};
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
status.flightModeFlags |= (1 << MSP_MODE_ARM);
}
return status;
}
} // namespace msp_osd

View File

@ -126,4 +126,10 @@ msp_rendor_distanceToHome_t construct_rendor_distanceToHome(const home_position_
// construct an MSP_ESC_SENSOR_DATA struct
msp_esc_sensor_data_dji_t construct_ESC_SENSOR_DATA();
// construct an MSP_RC struct
msp_rc_t construct_MSP_RC(const input_rc_s &input_rc);
// construct an MSP_STATUS struct
msp_status_t construct_MSP_STATUS(const vehicle_status_s &vehicle_status);
} // namespace msp_osd