From 111a4a735b210bf677ae61e1747a786f06d62726 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 8 Jul 2020 21:55:39 +0100 Subject: [PATCH] AP_RCTelemetry: add setters for vtx configuration deal with CRSF pitmode and options correctly --- libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp | 21 +++++++++++++-------- libraries/AP_RCTelemetry/AP_CRSF_Telem.h | 2 +- libraries/AP_RCTelemetry/AP_VideoTX.cpp | 2 +- libraries/AP_RCTelemetry/AP_VideoTX.h | 15 +++++++++------ 4 files changed, 24 insertions(+), 16 deletions(-) diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index bb99abbdea..b970a945e5 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -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 diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index 5ae7c6c0c9..d351ccdc94 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -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 { diff --git a/libraries/AP_RCTelemetry/AP_VideoTX.cpp b/libraries/AP_RCTelemetry/AP_VideoTX.cpp index 2e4a73177b..8a094ca0d3 100644 --- a/libraries/AP_RCTelemetry/AP_VideoTX.cpp +++ b/libraries/AP_RCTelemetry/AP_VideoTX.cpp @@ -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 diff --git a/libraries/AP_RCTelemetry/AP_VideoTX.h b/libraries/AP_RCTelemetry/AP_VideoTX.h index cb512d2de3..3c395f793a 100644 --- a/libraries/AP_RCTelemetry/AP_VideoTX.h +++ b/libraries/AP_RCTelemetry/AP_VideoTX.h @@ -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; }