Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCTelemetry: add setters for vtx configuration

Browse Source 
deal with CRSF pitmode and options correctly
This commit is contained in:
Andy Piper 2020-07-08 21:55:39 +01:00 committed by Peter Barker
parent 2ba458a75d
commit 111a4a735b
4 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -178,9 +178,9 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
break; break;
} }
if (vtx->is_in_pitmode) { if (vtx->is_in_pitmode) {
apvtx.set_options(apvtx.get_options() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} else { } else {
apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} }
// make sure the configured values now reflect reality // make sure the configured values now reflect reality
apvtx.set_defaults(); apvtx.set_defaults();
@ -206,9 +206,9 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) {
apvtx.set_power_dbm(vtx->power); apvtx.set_power_dbm(vtx->power);
if (vtx->pitmode) { if (vtx->pitmode) {
apvtx.set_options(apvtx.get_options() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} else { } else {
apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} }
// make sure the configured values now reflect reality // make sure the configured values now reflect reality
apvtx.set_defaults(); apvtx.set_defaults();
@ -230,18 +230,19 @@ void AP_CRSF_Telem::update_params()
_vtx_options_change_pending = vtx.update_options() || _vtx_options_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) { if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
debug("update_params(): freq %d, chan: %d->%d, band: %d->%d, pwr: %d->%d", debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d",
vtx.get_frequency_mhz(), vtx.get_frequency_mhz(),
AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()),
vtx.get_channel(), vtx.get_configured_channel(), vtx.get_channel(), vtx.get_configured_channel(),
vtx.get_band(), vtx.get_configured_band(), vtx.get_band(), vtx.get_configured_band(),
vtx.get_power_mw(), vtx.get_configured_power_mw()); vtx.get_power_mw(), vtx.get_configured_power_mw(),
vtx.get_options(), vtx.get_configured_options());
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND;
_telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX; _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX;
_telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
_telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX; _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX;
// make the desired frequency match the desired band and channel // make the desired frequency match the desired band and channel
if (_vtx_freq_change_pending) { if (_vtx_freq_change_pending) {
vtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel())); vtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()));
@ -288,7 +289,11 @@ void AP_CRSF_Telem::update_params()
_vtx_dbm_update = true; _vtx_dbm_update = true;
} else if (_vtx_options_change_pending) { } else if (_vtx_options_change_pending) {
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE; _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE;
_telem.ext.command.payload[1] = vtx.get_configured_options() & uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE); if (vtx.get_configured_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)) {
_telem.ext.command.payload[1] = 1;
} else {
_telem.ext.command.payload[1] = 0;
}
} }
_telem_pending = true; _telem_pending = true;
// calculate command crc // calculate command crc

2
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -87,7 +87,7 @@ public:
uint8_t origin; // address uint8_t origin; // address
uint8_t power; // power in dBm uint8_t power; // power in dBm
uint16_t frequency; // frequency in Mhz uint16_t frequency; // frequency in Mhz
bool pitmode; // disable 0, enable 1 uint8_t pitmode; // disable 0, enable 1
} PACKED; } PACKED;
struct LinkStatisticsFrame { struct LinkStatisticsFrame {

2
libraries/AP_RCTelemetry/AP_VideoTX.cpp
View File

@ -127,7 +127,7 @@ bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& c
// set the current power // set the current power
void AP_VideoTX::set_configured_power_mw(uint16_t power) { void AP_VideoTX::set_configured_power_mw(uint16_t power) {
_power_mw.set_and_save(power); _power_mw.set_and_save_ifchanged(power);
} }
// set the power in dbm, rounding appropriately // set the power in dbm, rounding appropriately

15
libraries/AP_RCTelemetry/AP_VideoTX.h
View File

@ -44,12 +44,12 @@ public:
}; };
enum VideoBand { enum VideoBand {
VTX_BAND_A, BAND_A,
VTX_BAND_B, BAND_B,
VTX_BAND_E, BAND_E,
VTX_BAND_FATSHARK, FATSHARK,
VTX_BAND_RACEBAND, RACEBAND,
VTX_BAND_LOW_RACEBAND, LOW_RACEBAND,
MAX_BANDS MAX_BANDS
}; };
@ -70,16 +70,19 @@ public:
bool update_power() const { return _defaults_set && _power_mw != _current_power; } bool update_power() const { return _defaults_set && _power_mw != _current_power; }
// get / set the frequency band // get / set the frequency band
void set_band(uint8_t band) { _current_band = band; } void set_band(uint8_t band) { _current_band = band; }
void set_configured_band(uint8_t band) { _band.set_and_save_ifchanged(band); }
uint8_t get_configured_band() const { return _band; } uint8_t get_configured_band() const { return _band; }
uint8_t get_band() const { return _current_band; } uint8_t get_band() const { return _current_band; }
bool update_band() const { return _defaults_set && _band != _current_band; } bool update_band() const { return _defaults_set && _band != _current_band; }
// get / set the frequency channel // get / set the frequency channel
void set_channel(uint8_t channel) { _current_channel = channel; } void set_channel(uint8_t channel) { _current_channel = channel; }
void set_configured_channel(uint8_t channel) { _channel.set_and_save_ifchanged(channel); }
uint8_t get_configured_channel() const { return _channel; } uint8_t get_configured_channel() const { return _channel; }
uint8_t get_channel() const { return _current_channel; } uint8_t get_channel() const { return _current_channel; }
bool update_channel() const { return _defaults_set && _channel != _current_channel; } bool update_channel() const { return _defaults_set && _channel != _current_channel; }
// get / set vtx option // get / set vtx option
void set_options(uint8_t options) { _current_options = options; } 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_configured_options() const { return _options; }
uint8_t get_options() const { return _current_options; } uint8_t get_options() const { return _current_options; }
bool update_options() const { return _defaults_set && _options != _current_options; } bool update_options() const { return _defaults_set && _options != _current_options; }