Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -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;
}
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 {
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
apvtx.set_defaults();
@ -206,9 +206,9 @@ void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) {
apvtx.set_power_dbm(vtx->power);
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 {
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
apvtx.set_defaults();
@ -230,18 +230,19 @@ void AP_CRSF_Telem::update_params()
_vtx_options_change_pending = vtx.update_options() || _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(),
AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()),
vtx.get_channel(), vtx.get_configured_channel(),
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.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX;
_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()));
@ -288,7 +289,11 @@ void AP_CRSF_Telem::update_params()
_vtx_dbm_update = true;
} else if (_vtx_options_change_pending) {
_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;
// calculate command crc

2
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -87,7 +87,7 @@ public:
uint8_t origin; // address
uint8_t power; // power in dBm
uint16_t frequency; // frequency in Mhz
bool pitmode; // disable 0, enable 1
uint8_t pitmode; // disable 0, enable 1
} PACKED;
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
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

15
libraries/AP_RCTelemetry/AP_VideoTX.h
View File

@ -44,12 +44,12 @@ public:
};
enum VideoBand {
VTX_BAND_A,
VTX_BAND_B,
VTX_BAND_E,
VTX_BAND_FATSHARK,
VTX_BAND_RACEBAND,
VTX_BAND_LOW_RACEBAND,
BAND_A,
BAND_B,
BAND_E,
FATSHARK,
RACEBAND,
LOW_RACEBAND,
MAX_BANDS
};
@ -70,16 +70,19 @@ public:
bool update_power() const { return _defaults_set && _power_mw != _current_power; }
// 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); }
uint8_t get_configured_band() const { return _band; }
uint8_t get_band() const { return _current_band; }
bool update_band() const { return _defaults_set && _band != _current_band; }
// get / set the frequency 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_channel() const { return _current_channel; }
bool update_channel() const { return _defaults_set && _channel != _current_channel; }
// 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 update_options() const { return _defaults_set && _options != _current_options; }