From c45ba5a57bc09d3822f1a074ab102b5dd6ff34b2 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Thu, 5 Jun 2025 22:02:59 +0200 Subject: [PATCH] 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 --- src/drivers/osd/msp_osd/MspV1.cpp | 79 +++++++++- src/drivers/osd/msp_osd/MspV1.hpp | 6 + src/drivers/osd/msp_osd/module.yaml | 11 ++ src/drivers/osd/msp_osd/msp_defines.h | 64 +++++++- src/drivers/osd/msp_osd/msp_osd.cpp | 189 ++++++++++++++++++++++++ src/drivers/osd/msp_osd/msp_osd.hpp | 19 ++- src/drivers/osd/msp_osd/uorb_to_msp.cpp | 36 ++++- src/drivers/osd/msp_osd/uorb_to_msp.hpp | 6 + 8 files changed, 394 insertions(+), 16 deletions(-) diff --git a/src/drivers/osd/msp_osd/MspV1.cpp b/src/drivers/osd/msp_osd/MspV1.cpp index ae13e408eb..1b5371302f 100644 --- a/src/drivers/osd/msp_osd/MspV1.cpp +++ b/src/drivers/osd/msp_osd/MspV1.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -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; +} diff --git a/src/drivers/osd/msp_osd/MspV1.hpp b/src/drivers/osd/msp_osd/MspV1.hpp index a94b21bab5..e7bdcece70 100644 --- a/src/drivers/osd/msp_osd/MspV1.hpp +++ b/src/drivers/osd/msp_osd/MspV1.hpp @@ -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}; }; diff --git a/src/drivers/osd/msp_osd/module.yaml b/src/drivers/osd/msp_osd/module.yaml index f43fd0bace..daffa81114 100644 --- a/src/drivers/osd/msp_osd/module.yaml +++ b/src/drivers/osd/msp_osd/module.yaml @@ -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 diff --git a/src/drivers/osd/msp_osd/msp_defines.h b/src/drivers/osd/msp_osd/msp_defines.h index 55eda1074c..eeb8a12810 100644 --- a/src/drivers/osd/msp_osd/msp_defines.h +++ b/src/drivers/osd/msp_osd/msp_defines.h @@ -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, diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index e3a6b3838c..9bae007a81 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -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; } diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index b39cdb60f6..7b566ca759 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -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) _param_osd_ch_height, (ParamInt) _param_osd_scroll_rate, (ParamInt) _param_osd_dwell_time, - (ParamInt) _param_osd_log_level + (ParamInt) _param_osd_log_level, + (ParamInt) _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}; }; diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 927dc3e551..a2912018f7 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -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 diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.hpp b/src/drivers/osd/msp_osd/uorb_to_msp.hpp index dfef00b4cc..ea9382cfc8 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.hpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.hpp @@ -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