diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 88c6be83a9..3c6cfb13f9 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -123,3 +123,9 @@ if(CONFIG_MODULES_TEMPERATURE_COMPENSATION) rc.thermal_cal ) endif() + +if(CONFIG_DRIVERS_VTXTABLE) + px4_add_romfs_files( + rc.vtxtable + ) +endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.vtxtable b/ROMFS/px4fmu_common/init.d/rc.vtxtable new file mode 100644 index 0000000000..70792b4caf --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtxtable @@ -0,0 +1,8 @@ +#!/bin/sh +# +# VTX table loading script. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +vtxtable load diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1337a777c8..57e77cd269 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -612,6 +612,16 @@ else fi unset RC_LOGGING + # + # Start the VTX services. + # + set RC_VTXTABLE ${R}etc/init.d/rc.vtxtable + if [ -f ${RC_VTXTABLE} ] + then + . ${RC_VTXTABLE} + fi + unset RC_VTXTABLE + # # Set additional parameters and env variables for selected AUTOSTART. # diff --git a/boards/auterion/fmu-v6s/default.px4board b/boards/auterion/fmu-v6s/default.px4board index 8c0e52cbed..2a4cbbe8a4 100644 --- a/boards/auterion/fmu-v6s/default.px4board +++ b/boards/auterion/fmu-v6s/default.px4board @@ -15,6 +15,8 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_PPS_CAPTURE=y +CONFIG_DRIVERS_VTX=y +CONFIG_VTX_CRSF_MSP_SUPPORT=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_BMI088_ACCELEROMETER_INT2=y CONFIG_COMMON_LIGHT=y @@ -92,3 +94,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_WQ_TTY_STACKSIZE=2500 diff --git a/boards/auterion/fmu-v6s/nuttx-config/nsh/defconfig b/boards/auterion/fmu-v6s/nuttx-config/nsh/defconfig index 2fa27f6758..0ab86df4dd 100644 --- a/boards/auterion/fmu-v6s/nuttx-config/nsh/defconfig +++ b/boards/auterion/fmu-v6s/nuttx-config/nsh/defconfig @@ -160,7 +160,7 @@ CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MAXARGUMENTS=24 CONFIG_NSH_NESTDEPTH=8 CONFIG_NSH_QUOTE=y CONFIG_NSH_ROMFSETC=y diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index 5cd8646f75..afd55db247 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -350,6 +350,7 @@ - [FrSky Telemetry](peripherals/frsky_telemetry.md) - [TBS Crossfire (CRSF) Telemetry](telemetry/crsf_telemetry.md) - [Satellite Comms (Iridium/RockBlock)](advanced_features/satcom_roadblock.md) + - [Analog Video Transmitters](vtx/index.md) - [Power Systems](power_systems/index.md) - [Battery Estimation Tuning](config/battery.md) diff --git a/docs/en/vtx/index.md b/docs/en/vtx/index.md new file mode 100644 index 0000000000..b94dd8fdb8 --- /dev/null +++ b/docs/en/vtx/index.md @@ -0,0 +1,287 @@ +# Analog Video Transmitters + +Analog Video Transmitters (VTX) can be controlled by PX4 via a half-duplex UART connection implementing the SmartAudio v1, v2, and v2.1 and Tramp protocols. + +The protocols allow writing and reading: + +- device status. +- transmission frequency in MHz or via band and channel index. +- transmission power in dBm or mW. +- operation modes. + +VTX settings are controlled by parameters and optionally via RC AUX channels or CRSF MSP commands. +The driver stores frequency and power tables that map band/channel indices to actual transmission values. +Configuration is device-specific and set up using the command line interface. + +## Getting Started + +Connect the SmartAudio or Tramp pin of the VTX to the TX pin of a free serial port on the flight controller. +Then set the following parameters: + +- `VTX_SER_CFG`: Select the serial port used for VTX communication. +- `VTX_DEVICE`: Selects the VTX device (generic SmartAudio/Tramp or a specific device). + +Note that since the VTX communication is half-duplex, you can, for example, use the single-pin Radio Controller port for the VTX and use a full duplex TELEM port for CRSF communication. + +You should now be able to see the VTX device in the driver status: + +``` +nsh> vtx status +INFO [vtx] UART device: /dev/ttyS4 +INFO [vtx] VTX table "UNINITIALIZED": +INFO [vtx] Power levels: +INFO [vtx] RC mapping: Disabled +INFO [vtx] Parameters: +INFO [vtx] band: 1 +INFO [vtx] channel: 1 +INFO [vtx] frequency: 0 MHz +INFO [vtx] power level: 1 +INFO [vtx] power: 0 = 0 mW +INFO [vtx] pit mode: off +INFO [vtx] SmartAudio v2: +INFO [vtx] band: 1 +INFO [vtx] channel: 1 +INFO [vtx] frequency: 6110 MHz +INFO [vtx] power level: 1 +INFO [vtx] power: 0 mW +INFO [vtx] pit mode: on +INFO [vtx] lock: unlocked +``` + +:::warning +Without a configured power table, power mappings are unknown and default to 0 mW. +Some VTX devices enter pit mode when power is set to 0, regardless of the `VTX_PIT_MODE` parameter. +::: + +## VTX Table Configuration + +The VTX table stores frequency and power mappings for your specific device. + +The manufacturer usually provides this information in the form of a JSON file that can be translated into a [Betaflight CLI command set](https://www.betaflight.com/docs/development/VTX#vtx-table) that this driver implements for compatibility. + +### Power Level Configuration + +``` +# Set table name ≤16 characters +vtxtable name "Peak THOR T67" + +# Set the power values that are sent to the VTX for each power level index +# Note: SmartAudio v1 and v2 use index values! +vtxtable powervalues 0 1 2 3 4 +# Note: SmartAudio v2.1 uses dBm values instead! +# vtxtable powervalues 14 23 27 30 35 +# Note: Tramp uses mW values instead! +# vtxtable powervalues 25 200 500 1000 3000 + +# Set the corresponding power labels for each power level index ≤4 characters. +# These are used for status reporting. +vtxtable powerlabels 25 200 500 1W 3W + +# Set number of power levels +vtxtable powerlevels 5 + +# Save configuration +vtxtable save +``` + +This will create a VTX table with 5 power levels. + +```nsh> vtxtable status +INFO [vtxtable] VTX table "Peak THOR T67": +INFO [vtxtable] Power levels: +INFO [vtxtable] 1: 0 = 25 +INFO [vtxtable] 2: 1 = 200 +INFO [vtxtable] 3: 2 = 500 +INFO [vtxtable] 4: 3 = 1W +INFO [vtxtable] 5: 4 = 3W +``` + +### Frequency Table Configuration + +``` +# Set the name of each band and the frequencies of each channel +vtxtable band 1 BAND_A A FACTORY 6110 6130 6150 6170 6190 6210 6230 6250 +vtxtable band 2 BAND_B B FACTORY 6270 6290 6310 6330 6350 6370 6390 6410 +vtxtable band 3 BAND_E E FACTORY 6430 6450 6470 6490 6510 6530 6550 6570 +vtxtable band 4 BAND_F F FACTORY 6590 6610 6630 6650 6670 6690 6710 6730 +vtxtable band 5 BAND_R R FACTORY 6750 6770 6790 6810 6830 6850 6870 6890 +vtxtable band 6 BAND_P P FACTORY 6910 6930 6950 6970 6990 7010 7030 7050 +vtxtable band 7 BAND_H H FACTORY 7070 7090 7110 7130 7150 7170 7190 7210 +vtxtable band 8 BAND_U U FACTORY 6115 6265 6425 6585 6745 6905 7065 7185 + +# Set number of bands and channels +vtxtable bands 8 +vtxtable channels 8 + +# Save configuration +vtxtable save +``` + +This will create a VTX table with 8 bands and 8 channels. +Note that FACTORY sends the band and channel indexes to the VTX device and they use their internal frequency mapping. In this mode the frequency is just for indication purposes. +In contrast, CUSTOM would send the actual frequency values to the VTX device, but not all devices support this mode. +Setting a frequency to zero will skip setting it. + +``` +nsh> vtxtable status +INFO [vtxtable] VTX table 8x8: Peak THOR T67 +INFO [vtxtable] A: BAND_A = 6110 6130 6150 6170 6190 6210 6230 6250 +INFO [vtxtable] B: BAND_B = 6270 6290 6310 6330 6350 6370 6390 6410 +INFO [vtxtable] E: BAND_E = 6430 6450 6470 6490 6510 6530 6550 6570 +INFO [vtxtable] F: BAND_F = 6590 6610 6630 6650 6670 6690 6710 6730 +INFO [vtxtable] R: BAND_R = 6750 6770 6790 6810 6830 6850 6870 6890 +INFO [vtxtable] P: BAND_P = 6910 6930 6950 6970 6990 7010 7030 7050 +INFO [vtxtable] H: BAND_H = 7070 7090 7110 7130 7150 7170 7190 7210 +INFO [vtxtable] U: BAND_U = 6115 6265 6425 6585 6745 6905 7065 7185 +``` + +### Table Constraints + +Maximum table dimensions: + +- ≤24 bands each with ≤16 channels and ≤32GHz frequency values. +- ≤16 power levels. +- ≤16 characters table name. +- ≤12 characters band name and 1 character band letter. +- ≤4 characters power label length (to support "2.5W"). + +## AUX Channel Mapping + +The AUX mapping feature allows you to control VTX settings using RC AUX channels. +Each mapping entry defines an AUX channel range that triggers a specific VTX configuration. + +To enable AUX mapping, set `VTX_MAP_CONFIG` to one of the following values: + +- `0`: Disabled +- `1`: Disabled (reserved for CRSF MSP integration) +- `2`: Map AUX channels to power level control only +- `3`: Map AUX channels to band and channel control only +- `4`: Map AUX channels to all settings (power, band, and channel) + +### Configuring AUX Map Entries + +Use the following command format to add mapping entries: + +``` +vtxtable +``` + +Parameters: + +- `index`: Map entry index (0-159) +- `aux_channel`: AUX channel number (3-19, where AUX1=3) +- `band`: Target band (1-24, or 0 to leave unchanged) +- `channel`: Target channel (1-16, or 0 to leave unchanged) +- `power`: Power level (1-16, 0 to leave unchanged, or -1 for pit mode) +- `start_pwm`: Start of PWM range in microseconds (typically 900-2100) +- `end_pwm`: End of PWM range in microseconds (typically 900-2100) + +:::info +AUX channel numbering starts from 3 (AUX1=channel 3) to account for the first four RC channels 0-3 used for flight control. +::: + +Example configuration for a 6-position dial controlling band/channel on AUX4 (channel 7): + +``` +vtx 0 7 7 1 0 900 1025 +vtx 1 7 7 2 0 1025 1100 +vtx 2 7 7 4 0 1100 1175 +vtx 3 7 7 6 0 1175 1225 +vtx 4 7 7 8 0 1225 1300 +vtx 5 7 3 8 0 1300 2100 +``` + +Example configuration for power control on AUX3 (channel 6): + +``` +vtxtable 16 6 0 0 -1 900 1250 +vtxtable 17 6 0 0 1 1250 1525 +vtxtable 18 6 0 0 2 1525 1650 +vtxtable 19 6 0 0 3 1650 1875 +vtxtable 20 6 0 0 4 1875 2010 +``` + +Save the configuration with: + +``` +vtxtable save +``` + +The map status can be verified with `vtxtable status`. + +## CRSF MSP Integration + +When using a CRSF receiver with MSP support, you can control VTX settings directly from your transmitter using MSP commands sent over the CRSF link. +This feature must be enabled at compile time with the `VTX_CRSF_MSP_SUPPORT` Kconfig option. + +To enable CRSF MSP control, set `VTX_MAP_CONFIG` to one of: + +- `1`: MSP controls both frequency (band/channel) and power +- `2`: MSP controls frequency (band/channel) only, AUX controls power +- `3`: MSP controls power only, AUX controls band/channel + +When MSP integration is active, the driver responds to `MSP_SET_VTX_CONFIG` (0x59) commands. +The transmitter can send band, channel, frequency, power level, and pit mode settings via MSP, which are automatically mapped to the corresponding PX4 parameters. + +:::tip +The MSP integration allows seamless VTX control from transmitters that support VTX configuration via Lua scripts or built-in VTX menus without requiring additional hardware switches. +::: + +## Build Configuration + +Both the VTX driver and VTX table support are configured via Kconfig options. + +Key configuration options: + +- `VTX_CRSF_MSP_SUPPORT`: Enables CRSF MSP command support (default: disabled) +- `VTXTABLE_CONFIG_FILE`: File path for persistent configuration (default: `/fs/microsd/vtx_config`) +- `VTXTABLE_AUX_MAP`: Enables AUX channel mapping (default: disabled) + +## Parameter Reference + +### VTX Settings Parameters + +- `VTX_BAND` (0-23): Frequency band selection (Band 1-24 in UI) +- `VTX_CHANNEL` (0-15): Channel within band (Channel 1-16 in UI) +- `VTX_FREQUENCY` (0-32000): Direct frequency in MHz (overrides band/channel when non-zero) +- `VTX_POWER` (0-15): Power level (Level 1-16 in UI, as configured in table) +- `VTX_PIT_MODE` (boolean): Pit mode for reduced power (default: disabled) + +### Configuration Parameters + +- `VTX_SER_CFG`: Serial port assignment for VTX communication +- `VTX_MAP_CONFIG`: Controls how VTX settings are mapped: + - Without `VTX_CRSF_MSP_SUPPORT`: + - `0`: Disabled + - `1`: Disabled + - `2`: AUX controls power only + - `3`: AUX controls band/channel only + - `4`: AUX controls both power and band/channel + - With `VTX_CRSF_MSP_SUPPORT`: + - `0`: Disabled + - `1`: MSP controls both frequency and power + - `2`: MSP controls frequency, AUX controls power + - `3`: MSP controls power, AUX controls band/channel + - `4`: Not used with MSP support +- `VTX_DEVICE`: Device-specific configuration (see below) + +## Device-Specific Configuration + +The `VTX_DEVICE` parameter allows device-specific workarounds. +It encodes both the protocol type and device variant: + +- Low byte (bits 0-7): Protocol selection + - `0`: SmartAudio (default) + - `1`: Tramp +- High byte (bits 8-15): Device-specific variant + - `0`: Generic device + - `20`: Peak THOR T67 + - `40`: Rush Max Solo + +### Known Device Workarounds + +**Peak THOR T67** (`VTX_DEVICE` = 5120): +This device incorrectly reports pit mode status but otherwise functions normally. +The driver applies a workaround to override the reported status with the actual configured state. + +For generic devices, use `VTX_DEVICE` = 0 (SmartAudio) or `VTX_DEVICE` = 1 (Tramp). diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5d1fb157b6..dbdaf79b7f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -240,6 +240,7 @@ set(msg_files VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg VelocityLimits.msg + Vtx.msg WheelEncoders.msg YawEstimatorStatus.msg versioned/ActuatorMotors.msg diff --git a/msg/Vtx.msg b/msg/Vtx.msg new file mode 100644 index 0000000000..b62a490636 --- /dev/null +++ b/msg/Vtx.msg @@ -0,0 +1,32 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 BAND_NAME_LENGTH = 12 +uint8 POWER_LABEL_LENGTH = 4 + +uint8 PROTOCOL_NONE = 0 # No protocol is detected, usually an error +uint8 PROTOCOL_SMART_AUDIO_V1 = 10 +uint8 PROTOCOL_SMART_AUDIO_V2 = 20 +uint8 PROTOCOL_SMART_AUDIO_V2_1 = 21 +uint8 PROTOCOL_TRAMP = 100 +uint8 protocol + +uint8 DEVICE_UNKNOWN = 0 +uint8 DEVICE_PEAK_THOR_T67 = 20 +uint8 DEVICE_RUSH_MAX_SOLO = 40 +uint8 device + +uint8 MODE_NORMAL = 0 +uint8 MODE_PIT = 1 +uint8 mode + +# Band and Channel are 0-indexed! But the user expects a 1-indexed display! +int8 band # Band number (0-23), negative values indicate frequency mode +int8 channel # Channel number (0-15), negative values indicate frequency mode +uint16 frequency # Frequency in MHz, zero indicates unknown + +uint8 band_letter # Band letter as ASCII +uint8[12] band_name # Band name in ASCII without null termination + +# Also 0-indexed, but the user expects a 1-indexed display! +int8 power_level # Current power level (0-15), negative values indicate unknown +uint8[4] power_label # Current power label in ASCII without null termination diff --git a/msg/VtxAuxMap.msg b/msg/VtxAuxMap.msg new file mode 100644 index 0000000000..0633cbb8c8 --- /dev/null +++ b/msg/VtxAuxMap.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +# See https://betaflight.com/docs/wiki/guides/current/VTX-CLI-Settings#change-vtx-power-level-using-aux-channel + +uint8 MAX_LENGTH = 160 + +uint8 length +int8[160] aux_channel +int8[160] band +int8[160] channel +int8[160] power_level +int16[160] start_range +int16[160] end_range diff --git a/msg/VtxTable.msg b/msg/VtxTable.msg new file mode 100644 index 0000000000..1ec914e707 --- /dev/null +++ b/msg/VtxTable.msg @@ -0,0 +1,35 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 NAME_LENGTH = 16 +uint8 BAND_NAME_LENGTH = 12 +uint8 POWER_LABEL_LENGTH = 4 +uint8 CHANNELS = 16 +uint8 BANDS = 24 +uint8 POWER_LEVELS = 16 + +# Configuration name in ASCII without null termination +uint8[16] name + +uint8 bands +uint8 channels +# Frequency is in MHz +# Invalid frequencies are set to 0 +uint16[384] frequency # 24*16 = 384 + +# Band names in ASCII without null termination +uint8[288] band_name # 24*12 = 288 + +# Band letters in ASCII +uint8[24] letter + +# Band attributes +uint8 ATTRIBUTE_FACTORY = 0 +uint8 ATTRIBUTE_CUSTOM = 1 +uint8[24] attribute + +uint8 power_levels +# Positive values are the exact value the VTX protocol expects to set the power level +# Negative values are alternative values depending on the VTX protocol and device +int16[16] power_value +# The power labels as ASCII without null termination +uint8[64] power_label # 4*16 = 64 diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp index a56fa0e686..98e2a2938d 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.cpp +++ b/src/drivers/rc/crsf_rc/CrsfParser.cpp @@ -46,6 +46,9 @@ #include "QueueBuffer.hpp" #include "CrsfParser.hpp" #include "Crc8.hpp" +#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT +#include +#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; diff --git a/src/drivers/vtx/CMakeLists.txt b/src/drivers/vtx/CMakeLists.txt new file mode 100644 index 0000000000..a82953f579 --- /dev/null +++ b/src/drivers/vtx/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__vtx + MAIN vtx + COMPILE_FLAGS + SRCS + VTX.cpp + smart_audio.cpp + tramp.cpp + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/vtx/Kconfig b/src/drivers/vtx/Kconfig new file mode 100644 index 0000000000..e4d7ac73aa --- /dev/null +++ b/src/drivers/vtx/Kconfig @@ -0,0 +1,18 @@ +menuconfig DRIVERS_VTX + bool "VTX Driver" + default n + select DRIVERS_VTXTABLE + select VTXTABLE_AUX_MAP + ---help--- + Control VTX devices via serial interface. + +if DRIVERS_VTX + + config VTX_CRSF_MSP_SUPPORT + bool "Support using CRSF MSP commands to configure VTX" + depends on DRIVERS_RC_CRSF_RC + default n + ---help--- + Set VTX frequency and power via CRSF MSP messages. + +endif # DRIVERS_VTX diff --git a/src/drivers/vtx/VTX.cpp b/src/drivers/vtx/VTX.cpp new file mode 100644 index 0000000000..790e813694 --- /dev/null +++ b/src/drivers/vtx/VTX.cpp @@ -0,0 +1,482 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VTX.hpp" + +#include +#include +#include +#include "smart_audio.h" +#include "tramp.h" + +using namespace time_literals; + +VTX::VTX(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), + _perf_cycle(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), + _perf_error(perf_alloc(PC_COUNT, MODULE_NAME": errors")), + _perf_get_settings(perf_alloc(PC_COUNT, MODULE_NAME": errors get settings")), + _perf_set_power(perf_alloc(PC_COUNT, MODULE_NAME": errors set power")), + _perf_set_frequency(perf_alloc(PC_COUNT, MODULE_NAME": errors set frequency")), + _perf_set_pit_mode(perf_alloc(PC_COUNT, MODULE_NAME": errors set pit mode")) +{ + if (device) { + strlcpy(_serial_path, device, sizeof(_serial_path)); + } +} + +VTX::~VTX() +{ + delete _protocol; + perf_free(_perf_cycle); + perf_free(_perf_error); + perf_free(_perf_get_settings); + perf_free(_perf_set_power); + perf_free(_perf_set_frequency); + perf_free(_perf_set_pit_mode); +} + +int VTX::init() +{ + _param_vtx_device.update(); + _device = (_param_vtx_device.get() >> 8); + const uint8_t protocol = (_param_vtx_device.get() & 0xff); + + if (_protocol == nullptr) { + if (protocol == vtx_s::PROTOCOL_TRAMP) { + _protocol = new vtx::Tramp(); + + } else { + _protocol = new vtx::SmartAudio(); + } + } + + if (_protocol == nullptr) { + PX4_ERR("Protocol alloc failed"); + return PX4_ERROR; + } + + if (_protocol->init(_serial_path) != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +void VTX::Run() +{ + static constexpr auto _INTERVAL{50_ms}; + + if (should_exit()) { + exit_and_cleanup(); + return; + } + + perf_begin(_perf_cycle); + + switch (_state) { + case STATE_INIT: + if (init() == PX4_OK) { + _state = STATE_UPDATE; + _pending = STATE_SET_ALL; + + } else { + perf_count(_perf_error); + } + + break; + + case STATE_GET_SETTINGS: + if (_protocol->get_settings() == PX4_OK) { + if (!_comms_ok) { + // previous get settings failed, send all settings again + _pending |= STATE_SET_ALL; + } + + _pending &= ~STATE_GET_SETTINGS; + _comms_ok = true; + + } else { + perf_count(_perf_get_settings); + _comms_ok = false; + } + + _update_counter = 1_s / _INTERVAL; + _state = STATE_UPDATE; + break; + + case STATE_SET_FREQUENCY: + if ((_frequency ? _protocol->set_frequency(_frequency) : _protocol->set_channel(_band, _channel)) == PX4_OK) { + _pending &= ~STATE_SET_FREQUENCY; + _pending |= STATE_GET_SETTINGS; + + } else { + perf_count(_perf_set_frequency); + } + + _state = STATE_UPDATE; + break; + + case STATE_SET_POWER: + if (_protocol->set_power(_power) == PX4_OK) { + _pending &= ~STATE_SET_POWER; + _pending |= STATE_GET_SETTINGS; + + } else { + perf_count(_perf_set_power); + } + + _state = STATE_UPDATE; + break; + + case STATE_SET_PIT_MODE: + if (_protocol->set_pit_mode(_pit_mode) == PX4_OK) { + _pending &= ~STATE_SET_PIT_MODE; + _pending |= STATE_GET_SETTINGS; + + } else { + perf_count(_perf_set_pit_mode); + } + + _state = STATE_UPDATE; + break; + + default: + // Poll parameters and update settings if needed + int new_band{_band}, new_channel{_channel}, new_frequency{_frequency}, new_power{_power}, new_pit_mode{_pit_mode}; + update_params(&new_band, &new_channel, &new_frequency, &new_power, &new_pit_mode); + + const vtx::Config::ChangeType current_change = vtxtable().get_change(); + + if (_last_config_change != current_change) { + _last_config_change = current_change; + _pending |= STATE_SET_ALL; + } + + if (_band != new_band || _channel != new_channel) { + _band = new_band; + _channel = new_channel; + _pending |= STATE_SET_FREQUENCY; + } + + if (_frequency != new_frequency) { + _frequency = new_frequency; + _pending |= STATE_SET_FREQUENCY; + } + + if (_power != new_power) { + _power = new_power; + _pending |= STATE_SET_POWER; + } + + if (_pit_mode != new_pit_mode) { + _pit_mode = new_pit_mode; + // Some VTX set power to 0 in pit mode, so also set power again + _pending |= STATE_SET_PIT_MODE | STATE_SET_POWER; + } + + if (_update_counter <= 0) { + _pending |= STATE_GET_SETTINGS; + + } else { + _update_counter--; + } + + // Round robin schedule the next pending flag test + uint8_t current{0}; + + while (_pending && !current) { + current = uint8_t(_pending & _schedule); + _schedule <<= 1; + + if (!(_schedule & STATE_MASK)) { + _schedule = STATE_GET_SETTINGS; + break; + } + } + + // Convert pending flags to state + if (current & STATE_GET_SETTINGS) { + _state = STATE_GET_SETTINGS; + + } else if (current & STATE_SET_FREQUENCY) { + _state = STATE_SET_FREQUENCY; + + } else if (current & STATE_SET_POWER) { + _state = STATE_SET_POWER; + + } else if (current & STATE_SET_PIT_MODE) { + _state = STATE_SET_PIT_MODE; + } + + handle_uorb(); + break; + } + + ScheduleDelayed(_INTERVAL); + perf_end(_perf_cycle); +} + +void VTX::update_params(int *new_band, int *new_channel, int *new_frequency, int *new_power, int *new_pit_mode) +{ + _param_vtx_band.update(); + _param_vtx_channel.update(); + _param_vtx_power.update(); + _param_vtx_frequency.update(); + _param_vtx_pit_mode.update(); + _param_vtx_map_config.update(); + const int map_config = _param_vtx_map_config.get(); + + // Set transmit channel based on either parameter or manual auxiliary channel input + if (map_config > 1) { + input_rc_s input_rc{}; + + if (_input_rc_sub.update(&input_rc) && !input_rc.rc_lost) { + int8_t band{0}, channel{0}, power{0}; + vtxtable().map_lookup(input_rc.values, input_rc.channel_count, &band, &channel, &power); + + if (map_config > 2) { + if (band > 0) { _param_vtx_band.commit_no_notification(band - 1); } + + if (channel > 0) { _param_vtx_channel.commit_no_notification(channel - 1); } + } + + if (map_config == 2 || map_config == 4) { + if (power == -1) { + _param_vtx_power.commit_no_notification(0); + _param_vtx_pit_mode.commit_no_notification(true); + + } else if (power > 0) { + _param_vtx_power.commit_no_notification(power - 1); + _param_vtx_pit_mode.commit_no_notification(false); + } + } + } + } + + *new_band = _param_vtx_band.get() % 24; + *new_channel = _param_vtx_channel.get() & 0xF; + *new_power = _param_vtx_power.get() & 0xF; + *new_frequency = _param_vtx_frequency.get(); + *new_pit_mode = _param_vtx_pit_mode.get(); +} + +void VTX::handle_uorb() +{ + vtx_s msg{}; + msg.band = _band; + msg.channel = _channel; + msg.power_level = _power; + msg.device = _device; + + if (!_comms_ok || !_protocol->copy_to(&msg)) { + msg.protocol = vtx_s::PROTOCOL_NONE; + msg.frequency = (_frequency ? _frequency : vtxtable().frequency(_band, _channel)); + msg.mode = _pit_mode ? vtx_s::MODE_PIT : vtx_s::MODE_NORMAL; + } + + // If frequency is set, overwrite band and channel to -1 + if (_frequency) { + msg.band = -1; + msg.channel = -1; + msg.band_letter = 'f'; + strncpy((char *)msg.band_name, "FREQUENCY", sizeof(msg.band_name)); + + } else { + msg.band_letter = vtxtable().band_letter(msg.band); + strncpy((char *)msg.band_name, vtxtable().band_name(msg.band), sizeof(msg.band_name)); + } + + strncpy((char *)msg.power_label, vtxtable().power_label(msg.power_level), sizeof(msg.power_label)); + + // Workarounds for specific devices + if (_device == vtx_s::DEVICE_PEAK_THOR_T67) { + // This device always reports pit mode, but still works fine + msg.frequency = vtxtable().frequency(_band, _channel); + msg.mode = _pit_mode ? vtx_s::MODE_PIT : vtx_s::MODE_NORMAL; + } + + // Only publish if something changed + if (memcmp(&msg, &_vtx_msg, sizeof(msg)) != 0) { + _vtx_msg = msg; + msg.timestamp = hrt_absolute_time(); + _vtx_pub.publish(msg); + } +} + +int VTX::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "start")) { + if (is_running()) { + return print_usage("already running"); + } + + int ret = VTX::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + if (!is_running()) { + return print_usage("not running"); + } + + // Check if the first argument is a number and delegate to the VtxTable + char *endptr{}; + strtol(argv[0], &endptr, 10); + + if (endptr != argv[0]) { + extern int vtxtable_custom_command(int argc, char *argv[]); + return vtxtable_custom_command(argc, argv); + } + + return print_usage("unknown command"); +} + +int VTX::task_spawn(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + break; + + default: + PX4_WARN("unrecognized flag"); + return -1; + break; + } + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + auto *const instance = new VTX(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); + + } else { + PX4_INFO("valid device required"); + } + } + + return PX4_ERROR; +} + +int VTX::print_status() +{ + if (_serial_path[0]) { + PX4_INFO("UART device: %s", _serial_path); + } + +#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT + const char *const config_map[] {"Disabled", "MSP", "MSP=Band/Channel, AUX=Power", "MSP=Power, AUX=Band/Channel", "AUX"}; +#else + const char *const config_map[] {"Disabled", "Disabled", "Power", "Band/Channel", "Power/Band/Channel"}; +#endif + const char *const config_str = config_map[_param_vtx_map_config.get() <= 4 ? _param_vtx_map_config.get() : 0]; + PX4_INFO("RC mapping: %s", config_str); + + PX4_INFO("Parameters:"); + PX4_INFO(" band: %d", (_frequency ? -1 : (_band + 1))); + PX4_INFO(" channel: %d", (_frequency ? -1 : (_channel + 1))); + PX4_INFO(" frequency: %u MHz", (_frequency ? _frequency : vtxtable().frequency(_band, _channel))); + PX4_INFO(" power level: %u", _power + 1); + PX4_INFO(" power: %hi = %s", vtxtable().power_value(_power), vtxtable().power_label(_power)); + PX4_INFO(" pit mode: %s", _pit_mode ? "on" : "off"); + + if (!(_comms_ok && _protocol && _protocol->print_settings())) { + PX4_ERR("%s device not found", _param_vtx_device.get() == 1 ? "Tramp" : "SmartAudio"); + } + + perf_print_counter(_perf_cycle); + perf_print_counter(_perf_error); + perf_print_counter(_perf_get_settings); + perf_print_counter(_perf_set_power); + perf_print_counter(_perf_set_frequency); + perf_print_counter(_perf_set_pit_mode); + + return 0; +} + +int VTX::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module communicates with a VTX camera via serial port. It can be used to +configure the camera settings and to control the camera's video transmission. +Supported protocols are: +- SmartAudio v1, v2.0, v2.1 +- Tramp + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vtx", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "VTX device", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("", "Sets an entry in the mapping table: "); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int vtx_main(int argc, char *argv[]) +{ + return VTX::main(argc, argv); +} diff --git a/src/drivers/vtx/VTX.hpp b/src/drivers/vtx/VTX.hpp new file mode 100644 index 0000000000..90996ab47a --- /dev/null +++ b/src/drivers/vtx/VTX.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "protocol.h" +#include "../vtxtable/VtxTable.hpp" + +/** + * @author Niklas Hauser + */ +class VTX : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VTX(const char *device); + virtual ~VTX(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + int init(); + +private: + void Run() override; + + void update_params(int *new_band, int *new_channel, int *new_frequency, int *new_power, int *new_pit_mode); + void handle_uorb(); + + char _serial_path[20] {}; + vtx::Protocol *_protocol{nullptr}; + + DEFINE_PARAMETERS( + (ParamInt) _param_vtx_band, + (ParamInt) _param_vtx_channel, + (ParamInt) _param_vtx_frequency, + (ParamInt) _param_vtx_power, + (ParamBool) _param_vtx_pit_mode, + (ParamInt) _param_vtx_map_config, + (ParamInt) _param_vtx_device + ); + + perf_counter_t _perf_cycle; + perf_counter_t _perf_error; + perf_counter_t _perf_get_settings; + perf_counter_t _perf_set_power; + perf_counter_t _perf_set_frequency; + perf_counter_t _perf_set_pit_mode; + + uORB::PublicationMulti _vtx_pub{ORB_ID(vtx)}; + uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; + + vtx_s _vtx_msg{}; + + int8_t _band{0}; + int8_t _channel{0}; + int16_t _frequency{0}; + int16_t _power{0}; + uint8_t _device{0}; + bool _pit_mode{}; + + enum : uint8_t { + STATE_INIT = 0, + STATE_GET_SETTINGS = 0b0001, + STATE_SET_PIT_MODE = 0b0010, + STATE_SET_FREQUENCY = 0b0100, + STATE_SET_POWER = 0b1000, + STATE_UPDATE = 0b1'0000, + STATE_SET_ALL = STATE_SET_FREQUENCY | STATE_SET_POWER | STATE_SET_PIT_MODE, + STATE_MASK = 0b1111, + } _state{STATE_INIT}; + uint8_t _pending{}; + uint8_t _schedule{STATE_GET_SETTINGS}; + int8_t _update_counter{}; + + bool _comms_ok{}; + vtx::Config::ChangeType _last_config_change{}; +}; diff --git a/src/drivers/vtx/module.yaml b/src/drivers/vtx/module.yaml new file mode 100644 index 0000000000..a972b26f99 --- /dev/null +++ b/src/drivers/vtx/module.yaml @@ -0,0 +1,133 @@ +module_name: VTX SmartAudio +serial_config: + - command: vtx start -d ${SERIAL_DEV} + port_config_param: + name: VTX_SER_CFG + group: VTX + label: VTX Serial Port + +parameters: + - group: VTX + definitions: + VTX_BAND: + description: + short: VTX band + long: VTX table band 1-24 + type: enum + values: + 0: Band 1 + 1: Band 2 + 2: Band 3 + 3: Band 4 + 4: Band 5 + 5: Band 6 + 6: Band 7 + 7: Band 8 + 8: Band 9 + 9: Band 10 + 10: Band 11 + 11: Band 12 + 12: Band 13 + 13: Band 14 + 14: Band 15 + 15: Band 16 + 16: Band 17 + 17: Band 18 + 18: Band 19 + 19: Band 20 + 20: Band 21 + 21: Band 22 + 22: Band 23 + 23: Band 24 + default: 0 + VTX_CHANNEL: + description: + short: VTX channel + long: VTX table channel 1-16 + type: enum + values: + 0: Channel 1 + 1: Channel 2 + 2: Channel 3 + 3: Channel 4 + 4: Channel 5 + 5: Channel 6 + 6: Channel 7 + 7: Channel 8 + 8: Channel 9 + 9: Channel 10 + 10: Channel 11 + 11: Channel 12 + 12: Channel 13 + 13: Channel 14 + 14: Channel 15 + 15: Channel 16 + default: 0 + VTX_FREQUENCY: + description: + short: VTX frequency in MHz + long: | + If the VTX frequency is set, it will overwrite the band and channel + settings. Set to 0 to use band and channel settings. + type: int32 + min: 0 + max: 32000 + default: 0 + VTX_POWER: + description: + short: VTX power level + long: VTX transmission power level 1-16 + type: enum + values: + 0: Level 1 + 1: Level 2 + 2: Level 3 + 3: Level 4 + 4: Level 5 + 5: Level 6 + 6: Level 7 + 7: Level 8 + 8: Level 9 + 9: Level 10 + 10: Level 11 + 11: Level 12 + 12: Level 13 + 13: Level 14 + 14: Level 15 + 15: Level 16 + default: 0 + VTX_PIT_MODE: + description: + short: VTX pit mode + long: VTX pit mode reduces power to the minimum + type: boolean + default: false + VTX_MAP_CONFIG: + description: + short: VTX mapping configuration + long: | + Configure how VTX settings are controlled. Options include using + MSP commands, AUX channels or a combination of both. To use MSP + commands, you must use a CRSF receiver with MSP support enabled in + the PX4 build. + type: enum + values: + 0: Disabled + 1: MSP for Power, Band and Channel + 2: MSP for Band and Channel, AUX for Power + 3: MSP for Power, AUX for Band and Channel + 4: AUX for Power, Band and Channel + default: 0 + VTX_DEVICE: + description: + short: VTX device + long: Specific VTX device useful for workarounds and optimizations + type: enum + # Note: keep values in sync with msg/Vtx.msg: lower 8-bit is protocol, next 8-bit is device + values: + 0: SmartAudio v1, v2, v2.1 Protocol + 100: Tramp Protocol + 5120: Peak THOR T67 + 10240: Rush MAX SOLO + default: 0 + reboot_required: true diff --git a/src/drivers/vtx/protocol.h b/src/drivers/vtx/protocol.h new file mode 100644 index 0000000000..cc73e7ab8d --- /dev/null +++ b/src/drivers/vtx/protocol.h @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include +#include +#include +#include +#include "../vtxtable/VtxTable.hpp" + +namespace vtx +{ + +/** + * Parser class for working with Tramp protocol. + * @author Niklas Hauser + */ +class Protocol +{ +public: + /// @brief Constructor + /// @param baudrate The baud rate for the serial connection + constexpr Protocol(uint16_t baudrate) : + _baudrate(baudrate) + {} + virtual inline ~Protocol() + { + if (_serial) { + _serial->close(); + delete _serial; + } + } + + inline int init(const char *device) + { + if (_serial == nullptr) { + _serial = new device::Serial(device, _baudrate); + + } else { + _serial->close(); + } + + if (_serial == nullptr) { + PX4_ERR("Serial alloc failed"); + return PX4_ERROR; + } + +#ifdef CONFIG_BOARD_SERIAL_RC + + // All RC serial ports are RX by default, so we need to swap RX and TX + // to get the TX pin on the RX pin. + if (strcmp(device, CONFIG_BOARD_SERIAL_RC) == 0 && !board_rc_swap_rxtx(device)) { + _serial->setSwapRxTxMode(); + } + +#endif + _serial->setStopbits(device::SerialConfig::StopBits::Two); + _serial->setSingleWireMode(); + + if (!_serial->open()) { + PX4_ERR("Serial open failed"); + delete _serial; + _serial = nullptr; + return PX4_ERROR; + } + + if (reset() != 0) { + return PX4_ERROR; + } + + return PX4_OK; + } + + /// @brief Reset the VTX + /// @return 0 on success, negative error code on failure + virtual int reset() { return 0; } + + /// @brief Get the current settings from the VTX + /// @return 0 on success, negative error code on failure + virtual int get_settings() = 0; + + /// @brief Set the power + /// @param power_level The power level to set, negative values set dBm or mW depending on the implementation + /// @return 0 on success, negative error code on failure + virtual int set_power(int16_t power_level) = 0; + + /// @brief Set the frequency + /// @param frequency_MHz The frequency to set, negative values set pit frequency + /// @return 0 on success, negative error code on failure + virtual int set_frequency(int16_t frequency_MHz) = 0; + + /// @brief Set the frequency based on band and channel + /// @param band The band (0-7) + /// @param channel The channel (0-7) + /// @return 0 on success, negative error code on failure + virtual int set_channel(uint8_t band, uint8_t channel) + { + return set_frequency(vtxtable().frequency(band, channel)); + } + + /// @brief Set the pit mode + /// @param onoff true to enable pit mode, false to disable + /// @return 0 on success, negative error code on failure + virtual int set_pit_mode(bool onoff) = 0; + + /// @brief Print the current settings + /// @return true if device is connected and settings are valid + virtual bool print_settings() = 0; + + /// @brief Copy the current settings to a uORB message + /// @param msg The message to copy the settings to + /// @return true if data is valid and has been copied + virtual bool copy_to(vtx_s *msg) = 0; + +protected: + /// @brief Parse incoming bytes + /// @param value The incoming byte + /// @return Number of remaining bytes to read, 0 if a full message has been received, negative on error + virtual int rx_parser(uint8_t value) = 0; + + inline int rx_msg() + { + using namespace time_literals; + memset(_rx_buf, 0, sizeof(_rx_buf)); + _read_state = 0; + + const hrt_abstime start_time_us = hrt_absolute_time(); + int remaining{3}; + + while (true) { + const int new_bytes = _serial->readAtLeast(_serial_buf, sizeof(_serial_buf), remaining, 20); + + for (int i = 0; i < new_bytes; i++) { + remaining = rx_parser(_serial_buf[i]); + + if (remaining <= 0) { return remaining; } + } + + if (hrt_elapsed_time(&start_time_us) > 200_ms) { + return -ETIMEDOUT; + } + } + + return -1; + } + + const uint16_t _baudrate{}; + device::Serial *_serial{}; + uint8_t _tx_buf[32] {}; + uint8_t _rx_buf[32] {}; + uint8_t _serial_buf[32] {}; + uint8_t _read_state{}; +}; + +} // namespace vtx diff --git a/src/drivers/vtx/smart_audio.cpp b/src/drivers/vtx/smart_audio.cpp new file mode 100644 index 0000000000..724beff0aa --- /dev/null +++ b/src/drivers/vtx/smart_audio.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "smart_audio.h" +#include +#include +#include +#include +#include + +namespace vtx +{ + +using namespace time_literals; + +int SmartAudio::get_settings() +{ + const uint8_t buf[] = {COMMAND_GET_SETTINGS, 0}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + Settings s; + memcpy(&s, _rx_buf + 2, sizeof(Settings)); + // Fix frequency endianess + s.frequency = __builtin_bswap16(s.frequency); + const uint8_t version = _rx_buf[0] >> 3; + + if (version == 0) { // v1 + s.number_of_power_levels = 4; + s.power_levels[0] = 7; + s.power_levels[1] = 16; + s.power_levels[2] = 25; + s.power_levels[3] = 40; + + } else if (version == 1) { // v2 + s.number_of_power_levels = 8; + + } else if (version == 2) { // v2.1 + if (s.number_of_power_levels > 7) { + s.number_of_power_levels = 7; + } + + for (int i = 0; i < s.number_of_power_levels; i++) { + s.power_levels[i] = s.power_levels[i + 1]; + } + } + + s.version = version; + + // copy the settings to storage + _settings = s; + + return PX4_OK; +} + +bool SmartAudio::print_settings() +{ + if (_settings.version > 2) { return false; } + + PX4_INFO("SmartAudio v%s%s:", ((const char *[]) {"1", "2", "2.1"})[_settings.version], + (_settings.mode & SmartAudio::MODE_SET_FREQUENCY) ? "+fr" : "+ch"); + PX4_INFO(" band: %u", ((_settings.channel >> 3) & 0b111) + 1); + PX4_INFO(" channel: %u", (_settings.channel & 0b111) + 1); + PX4_INFO(" frequency: %u MHz", _settings.frequency); + PX4_INFO(" power level: %u", _settings.current_power_level + 1); + const char *power_label = vtxtable().power_label(_settings.current_power_level); + + if (_settings.version == 2) { + PX4_INFO(" power: %u dBm = %s", _settings.current_power_dBm, power_label); + + } else { + PX4_INFO(" power: %s", power_label); + } + + PX4_INFO(" pit mode: %s", (_settings.mode & (SmartAudio::MODE_IN_RANGE_PIT_MODE | + SmartAudio::MODE_OUT_RANGE_PIT_MODE)) ? "on" : "off"); + PX4_INFO(" lock: %slocked", (_settings.mode & SmartAudio::MODE_UNLOCKED) ? "un" : ""); + return true; +} + +bool SmartAudio::copy_to(vtx_s *msg) +{ + if (_settings.version > 2) { return false; } + + if (_settings.version == 0) { msg->protocol = vtx_s::PROTOCOL_SMART_AUDIO_V1; } + + else if (_settings.version == 1) { msg->protocol = vtx_s::PROTOCOL_SMART_AUDIO_V2; } + + else { msg->protocol = vtx_s::PROTOCOL_SMART_AUDIO_V2_1; } + + msg->band = _settings.channel >> 3; + msg->channel = _settings.channel & 0b111; + msg->frequency = _settings.frequency; + msg->power_level = _settings.current_power_level; + bool pm = (_settings.mode & (SmartAudio::MODE_IN_RANGE_PIT_MODE | SmartAudio::MODE_OUT_RANGE_PIT_MODE)); + msg->mode = pm ? vtx_s::MODE_PIT : vtx_s::MODE_NORMAL; + + return true; +} + +int SmartAudio::set_power(uint8_t power_level, bool is_dBm) +{ + if (power_level >= _settings.number_of_power_levels) { + return PX4_ERROR; + } + + power_level = vtxtable().power_value(power_level); + + if (_settings.version == 2 && is_dBm) { power_level |= 0x80; } + + const uint8_t buf[] = {COMMAND_SET_POWER, 1, power_level}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + return ((_rx_buf[0] == COMMAND_SET_POWER) && (_rx_buf[2] == (power_level & ~0x80))) ? PX4_OK : PX4_ERROR; +} + +int SmartAudio::set_channel(uint8_t band, uint8_t channel) +{ + if (vtxtable().band_attribute(band) == Config::BandAttribute::CUSTOM) { + const uint16_t freq = vtxtable().frequency(band, channel); + + if (freq) { return set_frequency(freq, false); } + } + + const uint8_t value = ((band & 0b111) << 3) | (channel & 0b111); + const uint8_t buf[] = {COMMAND_SET_CHANNEL, 1, value}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + return ((_rx_buf[0] == COMMAND_SET_CHANNEL) && (_rx_buf[2] == value)) ? PX4_OK : PX4_ERROR; +} + +int SmartAudio::set_frequency(uint16_t frequency, bool pit) +{ + pit ? frequency |= 0x8000 : frequency &= ~0x8000; + const uint8_t buf[] = {COMMAND_SET_FREQUENCY, 2, uint8_t(frequency >> 8), uint8_t(frequency)}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + const bool success = (_rx_buf[0] == COMMAND_SET_FREQUENCY) && (_rx_buf[2] == (frequency >> 8)); + + if (_settings.version == 2) { + return (success && (_rx_buf[3] == (frequency & 0xff))) ? PX4_OK : PX4_ERROR; + + } else { + // Some v1/v2 firmwares do not echo the low byte back + // But the CRC is still correct, so we assume success + return success ? PX4_OK : PX4_ERROR; + } +} + +int SmartAudio::set_operating_mode(uint8_t mode) +{ + if (_settings.version == 0) { + return -1; + } + + const uint8_t buf[] = {COMMAND_SET_MODE, 1, mode}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + return ((_rx_buf[0] == COMMAND_SET_MODE) && (_rx_buf[2] == mode)) ? PX4_OK : PX4_ERROR; +} + +int SmartAudio::set_pit_mode(bool onoff) +{ + // only v2 supports pit mode + if (_settings.version == 0) { + return 0; + } + + uint8_t mode{SET_MODE_UNLOCKED}; + + if (onoff) { + if (_settings.mode & MODE_OUT_RANGE_PIT_MODE) { + mode |= SET_MODE_OUT_RANGE; + + } else { + mode |= SET_MODE_IN_RANGE; + } + + } else { + mode |= SET_MODE_DISABLE_PIT_MODE; + } + + return set_operating_mode(mode); +} + +int SmartAudio::transmit(const uint8_t *buf, size_t len) +{ + if (len > 28) { return -1; } + + // copy the message data + memcpy(_tx_buf + 3, buf, len); + // shift the command to the left + // const uint8_t cmd = buf[0]; + _tx_buf[3] = (buf[0] << 1) | 0x01; + // compute the CRC + _tx_buf[3 + len] = crc8(_tx_buf + 1, 2 + len); + // some devices need an additional 0 byte at the end + _tx_buf[4 + len] = 0; + + // send command + if (_serial->write(_tx_buf, 5 + len) < ssize_t(5 + len)) { + return -errno; + } + + _serial->flush(); + + return rx_msg(); +} + +bool SmartAudio::is_rsp(uint8_t cmd, uint8_t length) +{ + if (length == 0) { return false; } + + if (cmd == COMMAND_SET_POWER && length == 0x03) { return true; } + + if (cmd == COMMAND_SET_CHANNEL && length == 0x03) { return true; } + + if (cmd == COMMAND_SET_FREQUENCY && length == 0x04) { return true; } + + if (cmd == COMMAND_SET_MODE && length == 0x03) { return true; } + + if (((cmd == COMMAND_GET_SETTINGS) || (cmd == COMMAND_GET_SETTINGS_V2) || + (cmd == COMMAND_GET_SETTINGS_V21)) && length >= 0x06) { return true; } + + return false; +} + +int SmartAudio::rx_parser(uint8_t c) +{ + enum { + SYNC1 = 0, + SYNC2 = 1, + COMMAND = 2, + LENGTH = 3, + DATA = 4, + CRC = 5, + }; + + switch (_read_state) { + case SYNC1: + PX4_DEBUG("SYNC1 %x", c); + + if (c == 0xAA) { + _read_state = SYNC2; + } + + return 1; + + case SYNC2: + PX4_DEBUG("SYNC2 %x", c); + + if (c == 0x55) { + _read_state = COMMAND; + return 3; + + } else { + _read_state = SYNC1; + return 1; + } + + case COMMAND: + PX4_DEBUG("COMMAND %x", c); + _rx_buf[0] = c; + _read_state = LENGTH; + return 2; + + case LENGTH: + PX4_DEBUG("LENGTH %x", c); + + if (c >= 20) { + _read_state = SYNC1; + return 1; + } + + _rx_buf[1] = c; + + // command response: has one length too many + if (is_rsp(_rx_buf[0], c) && c) { c--; } + + _read_length = c; + _read_data_length = 0; + _read_state = _read_length ? DATA : CRC; + return _read_length + 1; + + case DATA: + PX4_DEBUG("DATA %x", c); + _rx_buf[2 + _read_data_length] = c; + + if (++_read_data_length >= _read_length) { + _read_state = CRC; + } + + return _read_length - _read_data_length + 1; + + case CRC: + PX4_DEBUG("CRC %x", c); + _read_state = SYNC1; + + if (!is_rsp(_rx_buf[0], _rx_buf[1])) { return 1; } + + return (c == crc8(_rx_buf, 2u + _read_length)) ? 0 : -CRC; + } + + return -6000; +} + +uint8_t SmartAudio::crc8(const uint8_t *data, const uint8_t len) +{ + // Implementation adapted from lib/crc/crc.c + uint8_t crc{}; + const uint8_t poly{0xd5}; + + for (uint8_t i = 0 ; i < len; i++) { + crc ^= data[i]; + + for (uint8_t j = 0; j < 8; j++) { + if (crc & (1u << 7u)) { + crc = (uint8_t)((crc << 1u) ^ poly); + + } else { + crc = (uint8_t)(crc << 1u); + } + } + } + + return crc; +} + +} diff --git a/src/drivers/vtx/smart_audio.h b/src/drivers/vtx/smart_audio.h new file mode 100644 index 0000000000..69c82b3adb --- /dev/null +++ b/src/drivers/vtx/smart_audio.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include "protocol.h" + +namespace vtx +{ + +/** + * Implementation for working with SmartAudio protocol. + * @author Niklas Hauser + */ +class SmartAudio final : public Protocol +{ +public: + static constexpr int BAUDRATE{4800}; + + struct Settings { + uint8_t channel; + uint8_t current_power_level; + uint8_t mode; + uint16_t frequency; + uint8_t current_power_dBm; + uint8_t number_of_power_levels; + uint8_t power_levels[8]; + uint8_t version; + } __attribute__((packed)); + + enum { + MODE_SET_FREQUENCY = 0b0001, + MODE_PIT_MODE_ACTIVE = 0b0010, + MODE_IN_RANGE_PIT_MODE = 0b0100, + MODE_OUT_RANGE_PIT_MODE = 0b1000, + MODE_UNLOCKED = 0b10000, + }; + const Settings &settings() const { return _settings; } + + inline SmartAudio() : Protocol(BAUDRATE) + { + _tx_buf[1] = 0xAA; + _tx_buf[2] = 0x55; + } + virtual ~SmartAudio() = default; + + inline int set_power(int16_t power_level) override + { + // negative values do not force dBm mode + if (power_level < 0) { return set_power(-power_level, false); } + + return set_power(power_level, true); + } + inline int set_frequency(int16_t frequency_MHz) override + { + if (frequency_MHz < 0) { return set_frequency(-frequency_MHz, true); } + + return set_frequency(frequency_MHz, false); + } + int set_channel(uint8_t band, uint8_t channel) override; + int get_settings() override; + int set_pit_mode(bool onoff) override; + bool print_settings() override; + bool copy_to(vtx_s *msg) override; + + // Additional methods not part of the Protocol interface + int set_frequency(uint16_t frequency_MHz, bool pit = false); + int set_power(uint8_t power_level, bool is_dBm = true); + int set_operating_mode(uint8_t mode); + +private: + int rx_parser(uint8_t c) override; + int transmit(const uint8_t *buf, size_t len); + static uint8_t crc8(const uint8_t *data, uint8_t len); + static bool is_rsp(uint8_t cmd, uint8_t length); + + enum { + COMMAND_GET_SETTINGS = 0x01, + COMMAND_SET_POWER = 0x02, + COMMAND_SET_CHANNEL = 0x03, + COMMAND_SET_FREQUENCY = 0x04, + COMMAND_SET_MODE = 0x05, + + COMMAND_GET_SETTINGS_V2 = 0x09, + COMMAND_GET_SETTINGS_V21 = 0x11, + }; + + enum { + SET_MODE_IN_RANGE = 0b0001, + SET_MODE_OUT_RANGE = 0b0010, + SET_MODE_DISABLE_PIT_MODE = 0b0100, + SET_MODE_UNLOCKED = 0b1000, + }; + + Settings _settings{.version = 99}; + uint8_t _read_length{}; + uint8_t _read_data_length{}; +}; + +} diff --git a/src/drivers/vtx/tramp.cpp b/src/drivers/vtx/tramp.cpp new file mode 100644 index 0000000000..b841777d7f --- /dev/null +++ b/src/drivers/vtx/tramp.cpp @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "tramp.h" +#include +#include +#include +#include +#include + +namespace vtx +{ + +using namespace time_literals; + +int Tramp::get_settings() +{ + const int rv = get_status(); + + if (rv != 0) { return rv; } + + px4_usleep(30_ms); + return get_temperature(); +} + + +bool Tramp::print_settings() +{ + PX4_INFO("Tramp:"); + PX4_INFO(" frequency: %hu MHz", _settings.frequency); + PX4_INFO(" requested power: %hu mW", _settings.requested_power_mW); + PX4_INFO(" power: %hu mW", _settings.power_mW); + PX4_INFO(" pit mode: %s", _settings.pit_mode ? "on" : "off"); + + PX4_INFO(" temperature: %hi C", _settings.temperature); + + PX4_INFO(" min frequency: %hu MHz", _settings.min_frequency); + PX4_INFO(" max frequency: %hu MHz", _settings.max_frequency); + PX4_INFO(" max power: %hu mW", _settings.max_power_mW); + return true; +} + +bool Tramp::copy_to(vtx_s *msg) +{ + msg->protocol = vtx_s::PROTOCOL_TRAMP; + + msg->frequency = _settings.frequency; + msg->power_level = _requested_power_level; + msg->mode = _settings.pit_mode ? vtx_s::MODE_PIT : vtx_s::MODE_NORMAL; + + return true; +} + +int Tramp::get_status() +{ + const uint8_t buf[] = {COMMAND_GET_SETTINGS}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + _settings.frequency = (_rx_buf[2] | (_rx_buf[3] << 8)); + _settings.requested_power_mW = (_rx_buf[4] | (_rx_buf[5] << 8)); + _settings.control_mode = _rx_buf[6]; + _settings.pit_mode = _rx_buf[7]; + _settings.power_mW = (_rx_buf[8] | (_rx_buf[9] << 8)); + + return PX4_OK; +} + +int Tramp::get_temperature() +{ + const uint8_t buf[] = {COMMAND_GET_TEMPERATURE}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + _settings.temperature = int16_t(_rx_buf[6] | (_rx_buf[7] << 8)); + + return PX4_OK; +} + +int Tramp::reset() +{ + const uint8_t buf[] = {COMMAND_RESET}; + const int rv = transmit(buf, sizeof(buf)); + + if (rv != 0) { return rv; } + + _settings.min_frequency = int16_t(_rx_buf[2] | (_rx_buf[3] << 8)); + _settings.max_frequency = int16_t(_rx_buf[4] | (_rx_buf[5] << 8)); + _settings.max_power_mW = int16_t(_rx_buf[6] | (_rx_buf[7] << 8)); + + return PX4_OK; +} + +int Tramp::set_power(int16_t power_level) +{ + int16_t power_mW; + _requested_power_level = -1; + + if (power_level < 0) { + power_mW = -power_level; + + } else { + power_mW = vtxtable().power_value(power_level); + + if (power_mW == 0) { return -EINVAL; } + + _requested_power_level = power_level; + } + + const uint8_t buf[] = {COMMAND_SET_POWER, uint8_t(power_mW), uint8_t(power_mW >> 8)}; + int rv = transmit(buf, sizeof(buf)); + + if (rv) { return rv; } + + px4_usleep(30_ms); + rv = get_status(); + + if (rv) { return rv; } + + return (_settings.requested_power_mW == power_mW) ? PX4_OK : PX4_ERROR; +} + +int Tramp::set_frequency(int16_t frequency_MHz) +{ + if (frequency_MHz < 0) { return -EINVAL; } // Tramp does not support pit frequency setting + + const uint8_t buf[] = {COMMAND_SET_FREQUENCY, uint8_t(frequency_MHz), uint8_t(frequency_MHz >> 8)}; + int rv = transmit(buf, sizeof(buf)); + + if (rv) { return rv; } + + px4_usleep(30_ms); + rv = get_status(); + + if (rv) { return rv; } + + return (_settings.frequency == frequency_MHz) ? PX4_OK : PX4_ERROR; +} + +int Tramp::set_pit_mode(bool onoff) +{ + const uint8_t mode = onoff ? 0u : 1u; + const uint8_t buf[] = {COMMAND_SET_MODE, mode}; + int rv = transmit(buf, sizeof(buf)); + + if (rv) { return rv; } + + px4_usleep(30_ms); + rv = get_status(); + + if (rv) { return rv; } + + return (_settings.pit_mode == onoff ? 1u : 0u) ? PX4_OK : PX4_ERROR; +} + +int Tramp::transmit(const uint8_t *buf, size_t len) +{ + if (len > 28) { return -1; } + + memset(_tx_buf + 1, 0, sizeof(_tx_buf) - 1); + // copy the message data + memcpy(_tx_buf + 1, buf, len); + // compute the CRC + _tx_buf[offsetof(Frame, crc)] = crc8(_tx_buf); + + // send command + if (_serial->write(_tx_buf, sizeof(Frame)) < ssize_t(sizeof(Frame))) { + return -errno; + } + + _serial->flush(); + + return rx_msg(); +} + +int Tramp::rx_parser(uint8_t c) +{ + enum { + SYNC = 0, + COMMAND = 1, + DATA = 2, + CRC = 3, + END = 4, + }; + static constexpr uint8_t CRCPOS{offsetof(Frame, crc)}; + + switch (_read_state) { + case SYNC: + PX4_DEBUG("SYNC %x", c); + + if (c == 0x0F) { + _rx_buf[0] = c; + _read_state = COMMAND; + return 1; + + } else { + _read_state = SYNC; + return 1; + } + + case COMMAND: + PX4_DEBUG("COMMAND %x", c); + + switch (c) { + case COMMAND_RESET: + case COMMAND_GET_TEMPERATURE: + case COMMAND_GET_SETTINGS: + case COMMAND_SET_POWER: + case COMMAND_SET_FREQUENCY: + case COMMAND_SET_MODE: + _rx_buf[1] = c; + _read_state = DATA; + _read_data_length = 2; + return CRCPOS; + } + + _read_state = SYNC; + return 1; + + case DATA: + PX4_DEBUG("DATA %x", c); + _rx_buf[_read_data_length] = c; + + if (++_read_data_length >= CRCPOS) { + _read_state = CRC; + } + + return sizeof(Frame) - _read_data_length; + + case CRC: + PX4_DEBUG("CRC %x", c); + _read_state = END; + _rx_buf[CRCPOS] = c; + return 1; + + case END: + PX4_DEBUG("END %x", c); + _read_state = SYNC; + + // small letter commands with empty payloads were sent by us + if ((_rx_buf[1] & 0x20) && !_rx_buf[2] && !_rx_buf[3]) { return 1; } + + // large letter command do not get a response + // Check the CRC for the received data + return (_rx_buf[CRCPOS] == crc8(_rx_buf)) ? 0 : -CRC; + } + + return -6000; +} + +uint8_t Tramp::crc8(const uint8_t *data) +{ + uint8_t crc{0}; + + for (uint_fast8_t ii{1}; ii < offsetof(Frame, crc); ii++) { + crc += data[ii]; + } + + return crc; +} + +} diff --git a/src/drivers/vtx/tramp.h b/src/drivers/vtx/tramp.h new file mode 100644 index 0000000000..13b68730a8 --- /dev/null +++ b/src/drivers/vtx/tramp.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include "protocol.h" + +namespace vtx +{ + +/** + * Implementation for working with Tramp protocol. + * @author Niklas Hauser + */ +class Tramp final : public Protocol +{ +public: + static constexpr int BAUDRATE{9600}; + + struct Settings { + // command 'v' + uint16_t frequency; + uint16_t requested_power_mW; + uint8_t control_mode; + uint8_t pit_mode; + uint16_t power_mW; + + // command 's' + int16_t temperature; + + // command 'r' + uint16_t min_frequency; + uint16_t max_frequency; + uint16_t max_power_mW; + }; + const Settings &settings() const { return _settings; } + + inline Tramp() : Protocol(BAUDRATE) + { + _tx_buf[0] = 0x0F; + } + + int get_settings() override; + int reset() override; + int set_power(int16_t power_mW) override; + int set_frequency(int16_t frequency_MHz) override; + int set_pit_mode(bool onoff) override; + bool print_settings() override; + bool copy_to(vtx_s *msg) override; + + // Additional methods not part of the Protocol interface + int get_status(); + int get_temperature(); + +private: + int rx_parser(uint8_t c) override; + int transmit(const uint8_t *buf, size_t len); + static uint8_t crc8(const uint8_t *data); + + enum { + COMMAND_RESET = 'r', // 0x72 + COMMAND_GET_TEMPERATURE = 's', // 0x73 + COMMAND_GET_SETTINGS = 'v', // 0x76 + COMMAND_SET_POWER = 'P', // 0x50 + COMMAND_SET_FREQUENCY = 'F', // 0x46 + COMMAND_SET_MODE = 'I', // 0x49 + }; + + struct Frame { + uint8_t start{0x0F}; + uint8_t command; + uint8_t payload[12]; + uint8_t crc; + uint8_t end{0x00}; + } __attribute__((packed)); + static_assert(sizeof(Frame) == 16, "Tramp frame size wrong"); + + Settings _settings{}; + uint8_t _read_data_length{}; + int8_t _requested_power_level{-1}; +}; + +} // namespace vtx diff --git a/src/drivers/vtxtable/CMakeLists.txt b/src/drivers/vtxtable/CMakeLists.txt new file mode 100644 index 0000000000..d85a0561ea --- /dev/null +++ b/src/drivers/vtxtable/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__vtxtable + MAIN vtxtable + COMPILE_FLAGS + SRCS + VtxTable.cpp + DEPENDS + crc + ) diff --git a/src/drivers/vtxtable/Kconfig b/src/drivers/vtxtable/Kconfig new file mode 100644 index 0000000000..f19d09a1dd --- /dev/null +++ b/src/drivers/vtxtable/Kconfig @@ -0,0 +1,28 @@ +menuconfig DRIVERS_VTXTABLE + bool "VTX Table" + default n + ---help--- + Manages the configuration of VTX frequencies and power levels. + +if DRIVERS_VTXTABLE + + config VTXTABLE_CONFIG_FILE + string "VTX Configuration File Path" + default "/fs/microsd/vtx_config" + ---help--- + Allows saving/loading the VTX table and aux config + from a file system. + + config VTXTABLE_USE_STORAGE + bool + default y if VTXTABLE_CONFIG_FILE != "" + default n if !(VTXTABLE_CONFIG_FILE != "") + + config VTXTABLE_AUX_MAP + string "VTX Auxiliary Map" + default n + ---help--- + Enable the AUX map for mapping AUX channel ranges to VTX + table settings. + +endif # DRIVERS_VTXTABLE diff --git a/src/drivers/vtxtable/VtxTable.cpp b/src/drivers/vtxtable/VtxTable.cpp new file mode 100644 index 0000000000..8c8174d433 --- /dev/null +++ b/src/drivers/vtxtable/VtxTable.cpp @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VtxTable.hpp" + +#include +#include +#ifdef CONFIG_VTXTABLE_USE_STORAGE +#include +#endif + +using namespace time_literals; + +static vtx::Config vtxtable_data; + +vtx::Config &vtxtable() +{ + return vtxtable_data; +} + +#ifdef CONFIG_VTXTABLE_USE_STORAGE +int vtxtable_store(const char *filename) +{ + if (filename == nullptr) { filename = CONFIG_VTXTABLE_CONFIG_FILE; } + + const int rv = vtxtable().store(filename); + + if (rv < 0) { + PX4_ERR("%s is not accessible!", filename); + return PX4_ERROR; + } + + PX4_INFO("saved to %s", filename); + return PX4_OK; +} + +int vtxtable_load(const char *filename) +{ + if (filename == nullptr) { filename = CONFIG_VTXTABLE_CONFIG_FILE; } + + const int rv = vtxtable().load(filename); + + if (rv < 0) { + switch (rv) { + case -ENOENT: + PX4_ERR("%s not found!", filename); + break; + + case -EPROTO: + PX4_ERR("VTX config serialization format is unsupported in %s!", filename); + break; + + case -EBADMSG: + PX4_ERR("VTX config has a corrupt CRC in %s!", filename); + break; + + case -EMSGSIZE: + PX4_ERR("VTX config in %s is incomplete!", filename); + break; + + default: + PX4_ERR("Loading VTX config from %s failed!", filename); + break; + } + + return PX4_ERROR; + } + + PX4_INFO("loaded from %s", filename); + return PX4_OK; +} + +static struct work_s storing_work; +#endif + +static void vtxtable_autosave() +{ +#ifdef CONFIG_VTXTABLE_USE_STORAGE + work_queue(LPWORK, &storing_work, [](void *) { vtxtable_store(nullptr); }, nullptr, USEC2TICK(1_s)); +#endif +} + +static int vtxtable_print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Manages the VTX frequency, power level and RC mapping table for VTX configuration. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vtxtable", "driver"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Shows the current VTX table configuration."); + + PRINT_MODULE_USAGE_COMMAND_DESCR("name", "Sets the VTX table name: "); + + PRINT_MODULE_USAGE_COMMAND_DESCR("bands", "Sets the number of bands: "); + PRINT_MODULE_USAGE_COMMAND_DESCR("band", "Sets the band frequencies: <1-index> "); + PRINT_MODULE_USAGE_COMMAND_DESCR("channels", "Sets the number of channels: "); + + PRINT_MODULE_USAGE_COMMAND_DESCR("powerlevels", "Sets number of power levels: "); + PRINT_MODULE_USAGE_COMMAND_DESCR("powervalues", "Sets the power level values: "); + PRINT_MODULE_USAGE_COMMAND_DESCR("powerlabels", "Sets the power level labels: <3 chars...>"); + +#ifdef CONFIG_VTXTABLE_AUX_MAP + PRINT_MODULE_USAGE_COMMAND_DESCR("", "Sets an entry in the mapping table: <0-index> "); +#endif + PRINT_MODULE_USAGE_COMMAND_DESCR("clear", "Clears the VTX table configuration."); + +#ifdef CONFIG_VTXTABLE_USE_STORAGE + PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Saves the VTX config to a file: "); + PRINT_MODULE_USAGE_COMMAND_DESCR("load", "Loads the VTX config from a file: "); +#endif + + return 0; +} + +void vtxtable_print_status() +{ + if (vtxtable().bands() == 0) { + PX4_INFO("VTX table: "); + + } else { + PX4_INFO("VTX table %ux%u: %s", vtxtable().bands(), vtxtable().channels(), vtxtable().name()); + vtxtable_print_frequencies(); + } + + if (vtxtable().power_levels() == 0) { + PX4_INFO("Power levels: "); + + } else { + PX4_INFO("Power levels:"); + vtxtable_print_power_levels(); + } + +#ifdef CONFIG_VTXTABLE_AUX_MAP + + if (vtxtable().map_size() == 0) { + PX4_INFO("RC mapping: "); + + } else { + PX4_INFO("RC mapping:"); + vtxtable_print_aux_map(); + } + +#endif +} + +int vtxtable_custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "status")) { + vtxtable_print_status(); + return PX4_OK; + } + + if (!strcmp(argv[0], "name")) { + if (argc < 2) { + return vtxtable_print_usage("not enough arguments"); + } + + vtxtable_autosave(); + return vtxtable().set_name(argv[1]) ? PX4_OK : PX4_ERROR; + } + + if (!strcmp(argv[0], "band")) { + if (argc < 5) { + return vtxtable_print_usage("not enough arguments"); + } + + const size_t band = atoi(argv[1]) - 1; + vtxtable().set_band_name(band, argv[2]); + vtxtable().set_band_letter(band, argv[3][0]); + vtxtable().set_band_attribute(band, + argv[4][0] == 'F' ? vtx::Config::BandAttribute::FACTORY : vtx::Config::BandAttribute::CUSTOM); + + for (int i = 5; i < argc; i++) { + vtxtable().set_frequency(band, i - 5, atoi(argv[i])); + } + + vtxtable_autosave(); + return PX4_OK; + } + + if (!strcmp(argv[0], "bands")) { + if (argc < 2) { + return vtxtable_print_usage("not enough arguments"); + } + + vtxtable_autosave(); + return vtxtable().set_bands(atoi(argv[1])) ? PX4_OK : PX4_ERROR; + } + + if (!strcmp(argv[0], "channels")) { + if (argc < 2) { + return vtxtable_print_usage("not enough arguments"); + } + + vtxtable_autosave(); + return vtxtable().set_channels(atoi(argv[1])) ? PX4_OK : PX4_ERROR; + } + + if (!strcmp(argv[0], "powervalues")) { + uint8_t levels{1}; + + for (; levels < argc; levels++) { + vtxtable().set_power_value(levels - 1, atoi(argv[levels])); + } + + vtxtable_autosave(); + return PX4_OK; + } + + if (!strcmp(argv[0], "powerlabels")) { + uint8_t levels{1}; + + for (; levels < argc; levels++) { + vtxtable().set_power_label(levels - 1, argv[levels]); + } + + vtxtable_autosave(); + return PX4_OK; + } + + if (!strcmp(argv[0], "powerlevels")) { + if (argc < 2) { + return vtxtable_print_usage("not enough arguments"); + } + + vtxtable_autosave(); + return vtxtable().set_power_levels(atoi(argv[1])) ? PX4_OK : PX4_ERROR; + } + +#ifdef CONFIG_VTXTABLE_AUX_MAP + char *endptr {}; + strtol(argv[0], &endptr, 10); + + if (endptr != argv[0]) { + if (argc < 7) { + return vtxtable_print_usage("not enough arguments"); + } + + vtxtable_autosave(); + return vtxtable().set_map_entry( + atoi(argv[0]), atoi(argv[1]) + 4 - 1, atoi(argv[2]), + atoi(argv[3]), atoi(argv[4]), atoi(argv[5]), atoi(argv[6])) ? PX4_OK : PX4_ERROR; + } + +#endif + + if (!strcmp(argv[0], "clear")) { + vtxtable().clear(); + return PX4_OK; + } + +#ifdef CONFIG_VTXTABLE_USE_STORAGE + const char *const filename = (argc == 2) ? argv[1] : nullptr; + + if (!strcmp(argv[0], "save")) { + return vtxtable_store(filename); + } + + if (!strcmp(argv[0], "load")) { + return vtxtable_load(filename); + } + +#endif + + return vtxtable_print_usage("unknown command"); +} + +void vtxtable_print_frequencies() +{ + for (uint8_t b = 0; b < vtxtable().bands(); b++) { + PX4_INFO_RAW("INFO [vtxtable] %c: %-*s=", vtxtable().band_letter(b), vtx::Config::NAME_LENGTH, vtxtable().band_name(b)); + + for (uint8_t c = 0; c < vtxtable().channels(); c++) { + PX4_INFO_RAW(" %4hu", vtxtable().frequency(b, c)); + } + + if (vtxtable().band_attribute(b) == vtx::Config::BandAttribute::CUSTOM) { + PX4_INFO_RAW(" CUSTOM\n"); + + } else { + PX4_INFO_RAW("\n"); + } + } +} + +void vtxtable_print_power_levels() +{ + for (uint8_t p = 0; p < vtxtable().power_levels(); p++) { + PX4_INFO(" %u: %2hi = %s", p + 1, vtxtable().power_value(p), vtxtable().power_label(p)); + } +} + +#ifdef CONFIG_VTXTABLE_AUX_MAP +void vtxtable_print_aux_map() +{ + for (uint8_t i = 0; i < vtx::Config::MAP_LENGTH; i++) { + uint8_t rc_channel{}, band{}, channel{}; + int8_t power_level{}; + uint16_t start_range{}, end_range{}; + vtxtable().map_entry(i, &rc_channel, &band, &channel, &power_level, &start_range, &end_range); + + if (!start_range && !end_range) { break; } + + if (!band && !channel) { + if (power_level == -1) { + PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = pit mode", + i, rc_channel, start_range, end_range); + + } else { + PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = %s", + i, rc_channel, start_range, end_range, + vtxtable().power_label(power_level - 1)); + } + + } else { + PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = %u + %u", + i, rc_channel, start_range, end_range, + band, channel); + } + } +} +#endif + +extern "C" __EXPORT int vtxtable_main(int argc, char *argv[]) +{ + if (argc <= 1) { + return vtxtable_print_usage("missing command"); + } + argc--; argv++; + return vtxtable_custom_command(argc, argv); +} diff --git a/src/drivers/vtxtable/VtxTable.hpp b/src/drivers/vtxtable/VtxTable.hpp new file mode 100644 index 0000000000..de367398cf --- /dev/null +++ b/src/drivers/vtxtable/VtxTable.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "config.h" + +/** + * @author Niklas Hauser + */ + +extern vtx::Config &vtxtable(); + +#ifdef CONFIG_VTXTABLE_USE_STORAGE +extern int vtxtable_store(const char *filename); +extern int vtxtable_load(const char *filename); +#endif + +extern void vtxtable_print_frequencies(); +extern void vtxtable_print_power_levels(); +#ifdef CONFIG_VTXTABLE_AUX_MAP +extern void vtxtable_print_aux_map(); +#endif diff --git a/src/drivers/vtxtable/config.h b/src/drivers/vtxtable/config.h new file mode 100644 index 0000000000..f22b76e0f4 --- /dev/null +++ b/src/drivers/vtxtable/config.h @@ -0,0 +1,545 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +extern "C" { +#include +} + +namespace vtx +{ + +/** + * Class storing the VTX frequencies and power levels and the map from RC channels to VTX settings. + * Everything is 0-indexed. + * @author Niklas Hauser + */ +class Config +{ +public: + static constexpr size_t BANDS{24}; + static constexpr size_t CHANNELS{16}; + static constexpr size_t POWER_LEVELS{16}; + static constexpr size_t MAP_LENGTH{160}; + static constexpr size_t NAME_LENGTH{16}; + static constexpr size_t BAND_NAME_LENGTH{12}; + static constexpr size_t POWER_LABEL_LENGTH{4}; + enum class BandAttribute : uint8_t { + FACTORY = 0, + CUSTOM = 1, + }; + // Allows to detect changes in the configuration + using ChangeType = uint16_t; + inline ChangeType get_change() { return _change_value; } + + constexpr Config() = default; + inline ~Config() + { + pthread_mutex_destroy(&_mutex); + } + +#ifdef CONFIG_VTXTABLE_AUX_MAP + // ================================== RC MAP ================================== + inline int set_map_entry(uint8_t index, uint8_t rc_channel, uint8_t band, uint8_t channel, + int8_t power_level, uint16_t start_range, uint16_t end_range) + { + if (index >= MAP_LENGTH) { + return -EINVAL; + } + + LockGuard lg{_mutex}; + _data.rc_map[index].rc_channel = rc_channel; + _data.rc_map[index].band = band; + _data.rc_map[index].channel = channel; + _data.rc_map[index].power_level = power_level; + _data.rc_map[index].start_range = start_range; + _data.rc_map[index].end_range = end_range; + _change_value++; + + return 0; + } + + inline int map_entry(uint8_t index, uint8_t *rc_channel, uint8_t *band, uint8_t *channel, + int8_t *power_level, uint16_t *start_range, uint16_t *end_range) const + { + if (index >= MAP_LENGTH) { + return -EINVAL; + } + + LockGuard lg{_mutex}; + + if (rc_channel) { *rc_channel = _data.rc_map[index].rc_channel; } + + if (band) { *band = _data.rc_map[index].band; } + + if (channel) { *channel = _data.rc_map[index].channel; } + + if (power_level) { *power_level = _data.rc_map[index].power_level; } + + if (start_range) { *start_range = _data.rc_map[index].start_range; } + + if (end_range) { *end_range = _data.rc_map[index].end_range; } + + return 0; + } + + inline int map_lookup(uint16_t *rc_values, size_t rc_count, + int8_t *band, int8_t *channel, int8_t *power_level) const + { + LockGuard lg{_mutex}; + + for (uint8_t i = 0; i < MAP_LENGTH; i++) { + if (_data.rc_map[i].rc_channel >= rc_count || _data.rc_map[i].is_empty()) { + continue; + } + + const uint16_t pwm_value = rc_values[_data.rc_map[i].rc_channel]; + + if (pwm_value >= _data.rc_map[i].start_range && + pwm_value < _data.rc_map[i].end_range) { + if (_data.rc_map[i].band) { *band = _data.rc_map[i].band; } + + if (_data.rc_map[i].channel) { *channel = _data.rc_map[i].channel; } + + if (_data.rc_map[i].power_level) { *power_level = _data.rc_map[i].power_level; } + } + } + + return 0; + } + + inline int map_clear() + { + LockGuard lg{_mutex}; + memset(&_data.rc_map, 0, sizeof(_data.rc_map)); + _change_value++; + return 0; + } + + inline size_t map_size() const + { + LockGuard lg{_mutex}; + size_t count{}; + + for (uint_fast8_t i = 0; i < MAP_LENGTH; i++) { + if (!_data.rc_map[i].is_empty()) { + count++; + } + } + + return count; + } +#endif + + // ================================ VTX TABLE ================================= + inline const char *name() const + { + LockGuard lg{_mutex}; + return _data.table.name; + } + inline bool set_name(const char *name) + { + if (!name) { return false; } + + LockGuard lg{_mutex}; + strncpy(_data.table.name, name, NAME_LENGTH); + _data.table.name[NAME_LENGTH] = 0; + _change_value++; + return true; + } + inline uint16_t frequency(uint8_t band, uint8_t channel) const + { + LockGuard lg{_mutex}; + + if (band >= _data.table.bands || channel >= _data.table.channels) { + return 0; + } + + return _data.table.frequency[band][channel]; + } + inline bool set_frequency(uint8_t band, uint8_t channel, uint16_t frequency) + { + if (band >= BANDS || channel >= CHANNELS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.frequency[band][channel] = frequency; + _change_value++; + return true; + } + inline bool find_band_channel(uint16_t frequency, uint8_t *band, uint8_t *channel) const + { + LockGuard lg{_mutex}; + + for (uint8_t bandi = 0; bandi < _data.table.bands; bandi++) { + for (uint8_t channeli = 0; channeli < _data.table.channels; channeli++) { + if (_data.table.frequency[bandi][channeli] == frequency) { + *band = bandi; + *channel = channeli; + return true; + } + } + } + + return false; + } + + inline size_t channels() const + { + LockGuard lg{_mutex}; + return _data.table.channels; + } + inline bool set_channels(size_t channels) + { + if (channels > CHANNELS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.channels = channels; + _change_value++; + return true; + } + + inline size_t bands() const + { + LockGuard lg{_mutex}; + return _data.table.bands; + } + inline bool set_bands(size_t bands) + { + if (bands > BANDS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.bands = bands; + _change_value++; + return true; + } + + inline const char *band_name(uint8_t band) const + { + LockGuard lg{_mutex}; + + if (band >= _data.table.bands) { + return "?"; + } + + return const_cast(_data.table.band[band]); + } + inline bool set_band_name(uint8_t band, const char *name) + { + if (band >= BANDS || !name) { + return false; + } + + LockGuard lg{_mutex}; + strncpy(_data.table.band[band], name, BAND_NAME_LENGTH); + _data.table.band[band][BAND_NAME_LENGTH] = 0; + _change_value++; + return true; + } + + inline char band_letter(uint8_t band) const + { + LockGuard lg{_mutex}; + + if (band >= _data.table.bands) { + return '?'; + } + + const char c = _data.table.letter[band]; + return c ? c : '?'; + } + inline bool set_band_letter(uint8_t band, char c) + { + if (band >= BANDS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.letter[band] = c; + _change_value++; + return true; + } + + inline BandAttribute band_attribute(uint8_t band) const + { + LockGuard lg{_mutex}; + + if (band >= _data.table.bands) { + return BandAttribute::FACTORY; + } + + return _data.table.attribute[band]; + } + inline bool set_band_attribute(uint8_t band, BandAttribute attribute) + { + if (band >= BANDS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.attribute[band] = attribute; + _change_value++; + return true; + } + + inline size_t power_levels() const + { + LockGuard lg{_mutex}; + return _data.table.power_levels; + } + inline bool set_power_levels(size_t levels) + { + if (levels > POWER_LEVELS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.power_levels = levels; + _change_value++; + return true; + } + + inline int16_t power_value(uint8_t level) const + { + LockGuard lg{_mutex}; + + if (level >= _data.table.power_levels) { + return 0; + } + + return _data.table.power_value[level]; + } + inline bool set_power_value(uint8_t level, int16_t value) + { + if (level >= POWER_LEVELS) { + return false; + } + + LockGuard lg{_mutex}; + _data.table.power_value[level] = value; + _change_value++; + return true; + } + inline const char *power_label(uint8_t level) const + { + LockGuard lg{_mutex}; + + if (level >= _data.table.power_levels) { + return "?"; + } + + return _data.table.power_label[level]; + } + inline bool set_power_label(uint8_t level, const char *label) + { + if (level >= POWER_LEVELS || !label) { + return false; + } + + LockGuard lg{_mutex}; + strncpy(_data.table.power_label[level], label, POWER_LABEL_LENGTH); + _data.table.power_label[level][POWER_LABEL_LENGTH] = 0; + _change_value++; + return true; + } + + inline void table_clear() + { + LockGuard lg{_mutex}; + _data.table = {}; + _change_value++; + } + + inline void clear() + { + LockGuard lg{_mutex}; + _data = {}; + _change_value++; + } + +#ifdef CONFIG_VTXTABLE_USE_STORAGE + inline int store(const char *filename) const + { + LockGuard lg{_mutex}; + int fd = open(filename, O_WRONLY | O_CREAT | O_TRUNC); + + if (fd < 0) { + return errno; + } + + // first write the magic/version + const uint64_t magic = MAGIC; + int written = write(fd, &magic, sizeof(MAGIC)); + + // Then write the rest of the data + written += write(fd, &_data, sizeof(Storage)); + + // finally compute and write the crc16 + const uint16_t crc = crc16_signature(CRC16_INITIAL, sizeof(Storage), (const uint8_t *)&_data); + written += write(fd, &crc, sizeof(crc)); + + close(fd); + return (written == FILE_SIZE) ? 0 : -EMSGSIZE; + } + + inline int load(const char *filename) + { + if (access(filename, R_OK)) { + return -ENOENT; + } + + LockGuard lg{_mutex}; + int fd = open(filename, O_RDONLY); + + if (fd < 0) { + return errno; + } + + // Check file size first + const off_t file_size = lseek(fd, 0, SEEK_END); + + if (file_size != FILE_SIZE) { + close(fd); + return -EMSGSIZE; + } + + // Check the version next + lseek(fd, 0, SEEK_SET); + uint64_t magic{}; + const int read_size = read(fd, &magic, sizeof(magic)); + + if (read_size != sizeof(magic) || magic != MAGIC) { + close(fd); + return -EPROTO; + } + + // Compute and check crc16 of the data in chunks + const size_t CHUNK{128}; + uint8_t buffer[CHUNK]; + size_t remaining{sizeof(Storage)}; + uint16_t computed_crc{0xffff}; + + while (remaining > 0) { + const int r = read(fd, buffer, (remaining < CHUNK) ? remaining : CHUNK); + + if (r <= 0) { + close(fd); + return -EIO; + } + + computed_crc = crc16_signature(computed_crc, r, buffer) ^ CRC16_OUTPUT_XOR; + remaining -= r; + } + + // read the stored crc16 + uint16_t stored_crc{}; + const int r = read(fd, &stored_crc, sizeof(stored_crc)); + + if (r != sizeof(stored_crc)) { + close(fd); + return -EIO; + } + + if (computed_crc != stored_crc) { + close(fd); + return -EBADMSG; + } + + // now read into the data structure directly + lseek(fd, sizeof(magic), SEEK_SET); + read(fd, &_data, sizeof(Storage)); + close(fd); + + _change_value++; + return 0; + } +#endif + +private: +#ifdef CONFIG_VTXTABLE_AUX_MAP + struct MapEntry { + uint8_t rc_channel; + uint8_t band; + uint8_t channel; + int8_t power_level; + uint16_t start_range; + uint16_t end_range; + constexpr bool is_empty() const { return !start_range && !end_range; } + } __attribute__((packed)); +#endif + + struct Table { + uint16_t frequency[BANDS][CHANNELS] {}; + int16_t power_value[POWER_LEVELS] {}; + char band[BANDS][BAND_NAME_LENGTH + 1] {}; + char power_label[POWER_LEVELS][POWER_LABEL_LENGTH + 1] {}; + char name[NAME_LENGTH + 1] {}; + char letter[BANDS] {}; + BandAttribute attribute[BANDS] {}; + uint8_t bands{}; + uint8_t channels{}; + uint8_t power_levels{}; + } __attribute__((packed)); + + struct Storage { + Table table{}; +#ifdef CONFIG_VTXTABLE_AUX_MAP + MapEntry rc_map[MAP_LENGTH]; +#endif + } __attribute__((packed)); + +#ifdef CONFIG_VTXTABLE_USE_STORAGE + static constexpr uint16_t VERSION {3}; + static constexpr uint64_t MAGIC{0x767478'636e66'0000ull | VERSION}; // "vtxcnf" + version magic + static constexpr size_t FILE_SIZE{sizeof(Storage) + sizeof(MAGIC) + sizeof(uint16_t)}; +#endif + + mutable pthread_mutex_t _mutex = PTHREAD_MUTEX_INITIALIZER; + Storage _data{}; + ChangeType _change_value{}; +}; + +} diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9f585500eb..5e366273d3 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -150,6 +150,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_rates_setpoint", 20); add_topic("vehicle_roi", 1000); add_topic("vehicle_status"); + add_topic("vtx"); add_optional_topic("vtol_vehicle_status", 200); add_topic("wind", 1000); add_topic("fixed_wing_lateral_setpoint");