Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCTelemetry: add support for SmartAudio protocol.

Browse Source 
treat VTX frequency as a first class citizen, match band/channel to frequency
update CRSF to use new VTX configuration
use a 6-position switch to control VTX power
add support for VTX GCS announcement
announce VTX updates on CRSF
support pitmode-until-arm and pitmode-on-disarm

Co-authored-by: luis.martinez.exts <luis.martinez@juntadeandalucia.es>
This commit is contained in:
Andy Piper 2021-01-29 19:54:26 +00:00 committed by Andrew Tridgell
parent ea3af39773
commit a98f85502a
3 changed files with 266 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -421,7 +421,9 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
}
// make sure the configured values now reflect reality
apvtx.set_defaults();
if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) {
AP::vtx().announce_vtx_settings();
}
_vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
}
@ -449,7 +451,9 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) {
apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
}
// make sure the configured values now reflect reality
apvtx.set_defaults();
if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) {
AP::vtx().announce_vtx_settings();
}
_vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
}
@ -546,14 +550,22 @@ void AP_CRSF_Telem::update_vtx_params()
{
AP_VideoTX& vtx = AP::vtx();
_vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || _vtx_freq_change_pending;
_vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || vtx.update_frequency() || _vtx_freq_change_pending;
_vtx_power_change_pending = vtx.update_power() || _vtx_power_change_pending;
_vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending;
if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
// make the desired frequency match the desired band and channel
if (_vtx_freq_change_pending) {
if (vtx.update_band() || vtx.update_channel()) {
vtx.update_configured_frequency();
} else {
vtx.update_configured_channel_and_band();
}
}
debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d",
vtx.get_frequency_mhz(),
AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()),
vtx.get_frequency_mhz(), vtx.get_configured_frequency_mhz(),
vtx.get_channel(), vtx.get_configured_channel(),
vtx.get_band(), vtx.get_configured_band(),
vtx.get_power_mw(), vtx.get_configured_power_mw(),
@ -564,11 +576,6 @@ void AP_CRSF_Telem::update_vtx_params()
_telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
_telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX;
// make the desired frequency match the desired band and channel
if (_vtx_freq_change_pending) {
vtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()));
}
uint8_t len = 5;
if (_vtx_freq_change_pending && _vtx_freq_update) {
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_FREQ;
@ -588,25 +595,22 @@ void AP_CRSF_Telem::update_vtx_params()
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_POWER;
if (vtx.get_configured_power_mw() < 26) {
vtx.set_configured_power_mw(25);
_telem.ext.command.payload[1] = 0;
} else if (vtx.get_configured_power_mw() < 201) {
if (vtx.get_configured_power_mw() < 101) {
vtx.set_configured_power_mw(100);
} else {
vtx.set_configured_power_mw(200);
}
_telem.ext.command.payload[1] = 1;
} else if (vtx.get_configured_power_mw() < 501) {
if (vtx.get_configured_power_mw() < 401) {
vtx.set_configured_power_mw(400);
} else {
vtx.set_configured_power_mw(500);
}
_telem.ext.command.payload[1] = 2;
} else {
vtx.set_configured_power_mw(800);
_telem.ext.command.payload[1] = 3;
}
_telem.ext.command.payload[1] = vtx.get_configured_power_level();
_vtx_dbm_update = true;
} else if (_vtx_options_change_pending) {
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE;

230
libraries/AP_RCTelemetry/AP_VideoTX.cpp
View File

@ -59,11 +59,17 @@ const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Video Transmitter Options
// @Description: Video Transmitter Options.
// @Description: Video Transmitter Options. Pitmode puts the VTX in a low power state. Unlocked enables certain restricted frequencies and power levels. Do not enable the Unlocked option unless you have appropriate permissions in your jurisdiction to transmit at high power levels.
// @User: Advanced
// @Bitmask: 0:Pitmode
// @Bitmask: 0:Pitmode,1:Pitmode until armed,2:Pitmode when disarmed,3:Unlocked
AP_GROUPINFO("OPTIONS", 6, AP_VideoTX, _options, 0),
// @Param: MAX_POWER
// @DisplayName: Video Transmitter Max Power Level
// @Description: Video Transmitter Maximum Power Level. Different VTXs support different power levels, this prevents the power aux switch from requesting too high a power level. The switch supports 6 power levels and the selected power will be a subdivision between 0 and this setting.
// @Range: 25 1000
AP_GROUPINFO("MAX_POWER", 7, AP_VideoTX, _max_power_mw, 800),
AP_GROUPEND
};
@ -104,6 +110,7 @@ bool AP_VideoTX::init(void)
_current_power = _power_mw;
_current_band = _band;
_current_channel = _channel;
_current_frequency = _frequency_mhz;
_current_options = _options;
_current_enabled = _enabled;
_initialized = true;
@ -171,6 +178,38 @@ uint8_t AP_VideoTX::get_configured_power_dbm() const {
}
}
// get the power "level"
uint8_t AP_VideoTX::get_configured_power_level() const {
if (_power_mw < 26) {
return 0;
} else if (_power_mw < 201) {
return 1;
} else if (_power_mw < 501) {
return 2;
} else { // 800
return 3;
}
}
// set the power "level"
void AP_VideoTX::set_power_level(uint8_t level) {
switch (level) {
case 1:
_current_power = 200;
break;
case 2:
_current_power = 500;
break;
case 3:
_current_power = 800;
break;
case 0:
default:
_current_power = 25;
break;
}
}
// set the current channel
void AP_VideoTX::set_enabled(bool enabled) {
_current_enabled = enabled;
@ -189,6 +228,15 @@ void AP_VideoTX::update(void)
crsf->update();
}
#endif
// manipulate pitmode if pitmode-on-disarm or power-on-arm is set
if (has_option(VideoOptions::VTX_PITMODE_ON_DISARM) || has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)) {
if (hal.util->get_soft_armed() && has_option(VideoOptions::VTX_PITMODE)) {
_options &= ~uint8_t(VideoOptions::VTX_PITMODE);
} else if (!hal.util->get_soft_armed() && !has_option(VideoOptions::VTX_PITMODE)
&& has_option(VideoOptions::VTX_PITMODE_ON_DISARM)) {
_options |= uint8_t(VideoOptions::VTX_PITMODE);
}
}
}
bool AP_VideoTX::have_params_changed() const
@ -196,15 +244,53 @@ bool AP_VideoTX::have_params_changed() const
return update_power()
|| update_band()
|| update_channel()
|| update_frequency()
|| update_options();
}
// update the configured frequency to match the channel and band
void AP_VideoTX::update_configured_frequency()
{
_frequency_mhz.set_and_save(get_frequency_mhz(_band, _channel));
}
// update the configured channel and band to match the frequency
void AP_VideoTX::update_configured_channel_and_band()
{
VideoBand band;
uint8_t channel;
if (get_band_and_channel(_frequency_mhz, band, channel)) {
_band.set_and_save(band);
_channel.set_and_save(channel);
} else {
update_configured_frequency();
}
}
// set the current configured values if not currently set in storage
// this is necessary so that the current settings can be seen
void AP_VideoTX::set_defaults()
bool AP_VideoTX::set_defaults()
{
if (_defaults_set) {
return;
return false;
}
// check that our current view of freqency matches band/channel
// if not then force one to be correct
uint16_t calced_freq = get_frequency_mhz(_current_band, _current_channel);
if (_current_frequency != calced_freq) {
if (_current_frequency > 0) {
VideoBand band;
uint8_t channel;
if (get_band_and_channel(_current_frequency, band, channel)) {
_current_band = band;
_current_channel = channel;
} else {
_current_frequency = calced_freq;
}
} else {
_current_frequency = calced_freq;
}
}
if (!_options.configured()) {
@ -214,16 +300,148 @@ void AP_VideoTX::set_defaults()
_channel.set_and_save(_current_channel);
}
if (!_band.configured()) {
_band.set_and_save(_current_band);
_frequency_mhz.set_and_save(_current_band);
}
if (!_power_mw.configured()) {
_power_mw.set_and_save(_current_power);
}
if (!_frequency_mhz.configured()) {
_frequency_mhz.set_and_save(_current_frequency);
}
// Now check that the user didn't screw up by selecting incompatible options
if (_frequency_mhz != get_frequency_mhz(_band, _channel)) {
if (_frequency_mhz > 0) {
update_configured_channel_and_band();
} else {
update_configured_frequency();
}
}
_defaults_set = true;
announce_vtx_settings();
return true;
}
void AP_VideoTX::announce_vtx_settings() const
{
// Output a friendly message so the user knows the VTX has been detected
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VTX: Freq: %dMHz, Power: %dmw, Band: %d, Chan: %d",
_frequency_mhz.get(), _power_mw.get(), _band.get() + 1, _channel.get() + 1);
_frequency_mhz.get(), has_option(VideoOptions::VTX_PITMODE) ? 0 : _power_mw.get(),
_band.get() + 1, _channel.get() + 1);
}
// change the video power based on switch input
void AP_VideoTX::change_power(int8_t position)
{
if (position < 0 || position > 5) {
return;
}
uint16_t power = 0;
// 0 or 25
if (_max_power_mw < 100) {
switch (position) {
case 2:
case 3:
case 4:
case 5:
power = 25;
break;
default:
power = 0;
break;
}
}
// 0, 25 or 100
else if (_max_power_mw < 200) {
switch (position) {
case 2:
case 3:
power = 25;
break;
case 4:
case 5:
power = 100;
break;
default:
power = 0;
break;
}
}
// 0, 25, 100 or 200
else if (_max_power_mw < 500) {
switch (position) {
case 2:
power = 25;
break;
case 3:
power = 100;
break;
case 4:
case 5:
power = 200;
break;
default:
power = 0;
break;
}
}
// 0, 25, 100, 200 or 500
else if (_max_power_mw < 800) {
switch (position) {
case 1:
power = 25;
break;
case 2:
power = 100;
break;
case 3:
power = 200;
break;
case 4:
case 5:
power = 500;
break;
default:
power = 0;
break;
}
}
// full range
else {
switch (position) {
case 1:
power = 25;
break;
case 2:
power = 100;
break;
case 3:
power = 200;
break;
case 4:
power = 500;
break;
case 5:
power = _max_power_mw; // some VTX's support 1000mw
break;
default:
power = 0;
break;
}
}
if (power == 0) {
set_configured_options(get_configured_options() | uint8_t(VideoOptions::VTX_PITMODE));
} else {
if (has_option(VideoOptions::VTX_PITMODE)) {
set_configured_options(get_configured_options() & ~uint8_t(VideoOptions::VTX_PITMODE));
}
set_configured_power_mw(power);
}
}
namespace AP {

30
libraries/AP_RCTelemetry/AP_VideoTX.h
View File

@ -16,6 +16,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <RC_Channel/RC_Channel.h>
#define VTX_MAX_CHANNELS 8
@ -40,7 +41,10 @@ public:
static const struct AP_Param::GroupInfo var_info[];
enum class VideoOptions {
VTX_PITMODE = 1 << 0
VTX_PITMODE = (1 << 0),
VTX_PITMODE_UNTIL_ARM = (1 << 1),
VTX_PITMODE_ON_DISARM = (1 << 2),
VTX_UNLOCKED = (1 << 3),
};
enum VideoBand {
@ -58,16 +62,24 @@ public:
static uint16_t get_frequency_mhz(uint8_t band, uint8_t channel) { return VIDEO_CHANNELS[band][channel]; }
static bool get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel);
void set_frequency_mhz(uint16_t freq) { _frequency_mhz.set_and_save(freq); }
uint16_t get_frequency_mhz() const { return _frequency_mhz; }
void set_frequency_mhz(uint16_t freq) { _current_frequency = freq; }
void set_configured_frequency_mhz(uint16_t freq) { _frequency_mhz.set_and_save_ifchanged(freq); }
uint16_t get_frequency_mhz() const { return _current_frequency; }
uint16_t get_configured_frequency_mhz() const { return _frequency_mhz; }
bool update_frequency() const { return _defaults_set && _frequency_mhz != _current_frequency; }
void update_configured_frequency();
// get / set power level
void set_power_mw(uint16_t power) { _current_power = power; }
void set_power_level(uint8_t level);
void set_power_dbm(uint8_t power);
void set_configured_power_mw(uint16_t power);
uint16_t get_configured_power_mw() const { return _power_mw; }
uint16_t get_power_mw() const { return _current_power; }
uint8_t get_configured_power_dbm() const;
uint8_t get_configured_power_level() const;
bool update_power() const { return _defaults_set && _power_mw != _current_power; }
// change the video power based on switch input
void change_power(int8_t position);
// get / set the frequency band
void set_band(uint8_t band) { _current_band = band; }
void set_configured_band(uint8_t band) { _band.set_and_save_ifchanged(band); }
@ -80,11 +92,13 @@ public:
uint8_t get_configured_channel() const { return _channel; }
uint8_t get_channel() const { return _current_channel; }
bool update_channel() const { return _defaults_set && _channel != _current_channel; }
void update_configured_channel_and_band();
// get / set vtx option
void set_options(uint8_t options) { _current_options = options; }
void set_configured_options(uint8_t options) { _options.set_and_save_ifchanged(options); }
uint8_t get_configured_options() const { return _options; }
uint8_t get_options() const { return _current_options; }
bool has_option(VideoOptions option) const { return _options.get() & uint8_t(option); }
bool update_options() const { return _defaults_set && _options != _current_options; }
// get / set whether the vtx is enabled
void set_enabled(bool enabled);
@ -93,18 +107,22 @@ public:
// have the parameters been updated
bool have_params_changed() const;
// set configured defaults from current settings
void set_defaults();
// set configured defaults from current settings, return true if defaults were set
bool set_defaults();
// display the current VTX settings in the GCS
void announce_vtx_settings() const;
static AP_VideoTX *singleton;
private:
// channel frequency
AP_Int16 _frequency_mhz;
uint16_t _current_frequency;
// power output in mw
AP_Int16 _power_mw;
uint16_t _current_power;
AP_Int16 _max_power_mw;
// frequency band
AP_Int8 _band;