mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 18:47:21 +08:00
[vtx] Add VTX driver with Tramp and SmartAudio support
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_mr-tropic (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Sync ROS 2 messages to px4_msgs / sync_to_px4_msgs (push) Has been cancelled
Docs - Crowdin - Upload Guide sources (en) / upload-to-crowdin (push) Has been cancelled
Docs - Deploy PX4 User Guide to AWS / build (push) Has been cancelled
Docs - Deploy PX4 User Guide to AWS / deploy (push) Has been cancelled
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_mr-tropic (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Sync ROS 2 messages to px4_msgs / sync_to_px4_msgs (push) Has been cancelled
Docs - Crowdin - Upload Guide sources (en) / upload-to-crowdin (push) Has been cancelled
Docs - Deploy PX4 User Guide to AWS / build (push) Has been cancelled
Docs - Deploy PX4 User Guide to AWS / deploy (push) Has been cancelled
This commit is contained in:
Niklas Hauser
committed by
Niklas Hauser
Niklas Hauser
parent
fad4450d7d
commit
c0c265cd1f
@@ -123,3 +123,9 @@ if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
|
|||||||
rc.thermal_cal
|
rc.thermal_cal
|
||||||
)
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
if(CONFIG_DRIVERS_VTXTABLE)
|
||||||
|
px4_add_romfs_files(
|
||||||
|
rc.vtxtable
|
||||||
|
)
|
||||||
|
endif()
|
||||||
|
|||||||
@@ -0,0 +1,8 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
#
|
||||||
|
# VTX table loading script.
|
||||||
|
#
|
||||||
|
# NOTE: Script variables are declared/initialized/unset in the rcS script.
|
||||||
|
#
|
||||||
|
|
||||||
|
vtxtable load
|
||||||
@@ -612,6 +612,16 @@ else
|
|||||||
fi
|
fi
|
||||||
unset RC_LOGGING
|
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.
|
# Set additional parameters and env variables for selected AUTOSTART.
|
||||||
#
|
#
|
||||||
|
|||||||
@@ -15,6 +15,8 @@ CONFIG_DRIVERS_DSHOT=y
|
|||||||
CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
|
CONFIG_DRIVERS_GNSS_SEPTENTRIO=y
|
||||||
CONFIG_DRIVERS_GPS=y
|
CONFIG_DRIVERS_GPS=y
|
||||||
CONFIG_DRIVERS_PPS_CAPTURE=y
|
CONFIG_DRIVERS_PPS_CAPTURE=y
|
||||||
|
CONFIG_DRIVERS_VTX=y
|
||||||
|
CONFIG_VTX_CRSF_MSP_SUPPORT=y
|
||||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||||
CONFIG_BMI088_ACCELEROMETER_INT2=y
|
CONFIG_BMI088_ACCELEROMETER_INT2=y
|
||||||
CONFIG_COMMON_LIGHT=y
|
CONFIG_COMMON_LIGHT=y
|
||||||
@@ -92,3 +94,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
|||||||
CONFIG_SYSTEMCMDS_UORB=y
|
CONFIG_SYSTEMCMDS_UORB=y
|
||||||
CONFIG_SYSTEMCMDS_VER=y
|
CONFIG_SYSTEMCMDS_VER=y
|
||||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||||
|
CONFIG_WQ_TTY_STACKSIZE=2500
|
||||||
|
|||||||
@@ -160,7 +160,7 @@ CONFIG_NSH_BUILTIN_APPS=y
|
|||||||
CONFIG_NSH_CMDPARMS=y
|
CONFIG_NSH_CMDPARMS=y
|
||||||
CONFIG_NSH_CROMFSETC=y
|
CONFIG_NSH_CROMFSETC=y
|
||||||
CONFIG_NSH_LINELEN=128
|
CONFIG_NSH_LINELEN=128
|
||||||
CONFIG_NSH_MAXARGUMENTS=15
|
CONFIG_NSH_MAXARGUMENTS=24
|
||||||
CONFIG_NSH_NESTDEPTH=8
|
CONFIG_NSH_NESTDEPTH=8
|
||||||
CONFIG_NSH_QUOTE=y
|
CONFIG_NSH_QUOTE=y
|
||||||
CONFIG_NSH_ROMFSETC=y
|
CONFIG_NSH_ROMFSETC=y
|
||||||
|
|||||||
@@ -350,6 +350,7 @@
|
|||||||
- [FrSky Telemetry](peripherals/frsky_telemetry.md)
|
- [FrSky Telemetry](peripherals/frsky_telemetry.md)
|
||||||
- [TBS Crossfire (CRSF) Telemetry](telemetry/crsf_telemetry.md)
|
- [TBS Crossfire (CRSF) Telemetry](telemetry/crsf_telemetry.md)
|
||||||
- [Satellite Comms (Iridium/RockBlock)](advanced_features/satcom_roadblock.md)
|
- [Satellite Comms (Iridium/RockBlock)](advanced_features/satcom_roadblock.md)
|
||||||
|
- [Analog Video Transmitters](vtx/index.md)
|
||||||
|
|
||||||
- [Power Systems](power_systems/index.md)
|
- [Power Systems](power_systems/index.md)
|
||||||
- [Battery Estimation Tuning](config/battery.md)
|
- [Battery Estimation Tuning](config/battery.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 <index> <aux_channel> <band> <channel> <power> <start_pwm> <end_pwm>
|
||||||
|
```
|
||||||
|
|
||||||
|
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).
|
||||||
@@ -240,6 +240,7 @@ set(msg_files
|
|||||||
VehicleThrustSetpoint.msg
|
VehicleThrustSetpoint.msg
|
||||||
VehicleTorqueSetpoint.msg
|
VehicleTorqueSetpoint.msg
|
||||||
VelocityLimits.msg
|
VelocityLimits.msg
|
||||||
|
Vtx.msg
|
||||||
WheelEncoders.msg
|
WheelEncoders.msg
|
||||||
YawEstimatorStatus.msg
|
YawEstimatorStatus.msg
|
||||||
versioned/ActuatorMotors.msg
|
versioned/ActuatorMotors.msg
|
||||||
|
|||||||
+32
@@ -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
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -46,6 +46,9 @@
|
|||||||
#include "QueueBuffer.hpp"
|
#include "QueueBuffer.hpp"
|
||||||
#include "CrsfParser.hpp"
|
#include "CrsfParser.hpp"
|
||||||
#include "Crc8.hpp"
|
#include "Crc8.hpp"
|
||||||
|
#ifdef CONFIG_VTX_CRSF_MSP_SUPPORT
|
||||||
|
#include <px4_platform_common/param.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
#define CRSF_CHANNEL_VALUE_MIN 172
|
#define CRSF_CHANNEL_VALUE_MIN 172
|
||||||
#define CRSF_CHANNEL_VALUE_MAX 1811
|
#define CRSF_CHANNEL_VALUE_MAX 1811
|
||||||
@@ -60,6 +63,7 @@ enum CRSF_PAYLOAD_SIZE {
|
|||||||
CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX = -1,
|
CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX = -1,
|
||||||
CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22,
|
CRSF_PAYLOAD_SIZE_RC_CHANNELS = 22,
|
||||||
CRSF_PAYLOAD_SIZE_ATTITUDE = 6,
|
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
|
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 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 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);
|
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[] = {
|
static const CrsfPacketDescriptor_t crsf_packet_descriptors[] = {
|
||||||
{CRSF_PACKET_TYPE_RC_CHANNELS_PACKED, CRSF_PAYLOAD_SIZE_RC_CHANNELS, ProcessChannelData},
|
{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, CRSF_PAYLOAD_SIZE_LINK_STATISTICS, ProcessLinkStatistics},
|
||||||
{CRSF_PACKET_TYPE_LINK_STATISTICS_TX, CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX, ProcessLinkStatisticsTx},
|
{CRSF_PACKET_TYPE_LINK_STATISTICS_TX, CRSF_PAYLOAD_SIZE_LINK_STATISTICS_TX, ProcessLinkStatisticsTx},
|
||||||
{CRSF_PACKET_TYPE_ELRS_STATUS, CRSF_PAYLOAD_SIZE_ELRS_STATUS, ProcessElrsStatus},
|
{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))
|
#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;
|
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)
|
static CrsfPacketDescriptor_t *FindCrsfDescriptor(const enum CRSF_PACKET_TYPE packet_type)
|
||||||
{
|
{
|
||||||
uint32_t i;
|
uint32_t i;
|
||||||
|
|||||||
@@ -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
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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 <fcntl.h>
|
||||||
|
#include <termios.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#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, "<file:dev>", "VTX device", false);
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("<int>", "Sets an entry in the mapping table: <index> <aux channel> <band> <channel> <power level> <start range> <end range>");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
extern "C" __EXPORT int vtx_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
return VTX::main(argc, argv);
|
||||||
|
}
|
||||||
@@ -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 <float.h>
|
||||||
|
|
||||||
|
#include <board_config.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <lib/perf/perf_counter.h>
|
||||||
|
#include <px4_platform_common/Serial.hpp>
|
||||||
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
#include <px4_platform_common/getopt.h>
|
||||||
|
#include <px4_platform_common/log.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <px4_platform_common/module_params.h>
|
||||||
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||||
|
#include <uORB/PublicationMulti.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/topics/vtx.h>
|
||||||
|
#include <uORB/topics/input_rc.h>
|
||||||
|
|
||||||
|
#include "protocol.h"
|
||||||
|
#include "../vtxtable/VtxTable.hpp"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @author Niklas Hauser <niklas@auterion.com>
|
||||||
|
*/
|
||||||
|
class VTX : public ModuleBase<VTX>, 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<px4::params::VTX_BAND>) _param_vtx_band,
|
||||||
|
(ParamInt<px4::params::VTX_CHANNEL>) _param_vtx_channel,
|
||||||
|
(ParamInt<px4::params::VTX_FREQUENCY>) _param_vtx_frequency,
|
||||||
|
(ParamInt<px4::params::VTX_POWER>) _param_vtx_power,
|
||||||
|
(ParamBool<px4::params::VTX_PIT_MODE>) _param_vtx_pit_mode,
|
||||||
|
(ParamInt<px4::params::VTX_MAP_CONFIG>) _param_vtx_map_config,
|
||||||
|
(ParamInt<px4::params::VTX_DEVICE>) _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_s> _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{};
|
||||||
|
};
|
||||||
@@ -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
|
||||||
@@ -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 <board_config.h>
|
||||||
|
#include <px4_platform_common/Serial.hpp>
|
||||||
|
#include <px4_platform_common/log.h>
|
||||||
|
#include <uORB/topics/vtx.h>
|
||||||
|
#include "../vtxtable/VtxTable.hpp"
|
||||||
|
|
||||||
|
namespace vtx
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Parser class for working with Tramp protocol.
|
||||||
|
* @author Niklas Hauser <niklas@auterion.com>
|
||||||
|
*/
|
||||||
|
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
|
||||||
@@ -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 <string.h>
|
||||||
|
#include <cerrno>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <px4_platform_common/log.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -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 <niklas@auterion.com>
|
||||||
|
*/
|
||||||
|
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{};
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
@@ -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 <string.h>
|
||||||
|
#include <cerrno>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <px4_platform_common/log.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@@ -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 <niklas@auterion.com>
|
||||||
|
*/
|
||||||
|
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
|
||||||
@@ -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
|
||||||
|
)
|
||||||
@@ -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
|
||||||
@@ -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 <px4_platform_common/log.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||||
|
#include <px4_platform_common/workqueue.h>
|
||||||
|
#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: <string>");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("bands", "Sets the number of bands: <int>");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("band", "Sets the band frequencies: <1-index> <name> <letter> <attribute> <frequencies...>");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("channels", "Sets the number of channels: <int>");
|
||||||
|
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("powerlevels", "Sets number of power levels: <int>");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("powervalues", "Sets the power level values: <int...>");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("powerlabels", "Sets the power level labels: <3 chars...>");
|
||||||
|
|
||||||
|
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("<int>", "Sets an entry in the mapping table: <0-index> <aux channel> <band> <channel> <power level> <start range> <end range>");
|
||||||
|
#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: <file>");
|
||||||
|
PRINT_MODULE_USAGE_COMMAND_DESCR("load", "Loads the VTX config from a file: <file>");
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void vtxtable_print_status()
|
||||||
|
{
|
||||||
|
if (vtxtable().bands() == 0) {
|
||||||
|
PX4_INFO("VTX table: <not configured>");
|
||||||
|
|
||||||
|
} 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: <not configured>");
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_INFO("Power levels:");
|
||||||
|
vtxtable_print_power_levels();
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||||
|
|
||||||
|
if (vtxtable().map_size() == 0) {
|
||||||
|
PX4_INFO("RC mapping: <not configured>");
|
||||||
|
|
||||||
|
} 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);
|
||||||
|
}
|
||||||
@@ -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 <niklas@auterion.com>
|
||||||
|
*/
|
||||||
|
|
||||||
|
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
|
||||||
File diff suppressed because it is too large
Load Diff
@@ -150,6 +150,7 @@ void LoggedTopics::add_default_topics()
|
|||||||
add_topic("vehicle_rates_setpoint", 20);
|
add_topic("vehicle_rates_setpoint", 20);
|
||||||
add_topic("vehicle_roi", 1000);
|
add_topic("vehicle_roi", 1000);
|
||||||
add_topic("vehicle_status");
|
add_topic("vehicle_status");
|
||||||
|
add_topic("vtx");
|
||||||
add_optional_topic("vtol_vehicle_status", 200);
|
add_optional_topic("vtol_vehicle_status", 200);
|
||||||
add_topic("wind", 1000);
|
add_topic("wind", 1000);
|
||||||
add_topic("fixed_wing_lateral_setpoint");
|
add_topic("fixed_wing_lateral_setpoint");
|
||||||
|
|||||||
Reference in New Issue
Block a user