aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RCTelemetry: move AP_VideoTX to AP_VideoTX

Browse Source
This commit is contained in:
Andy Piper 2021-02-17 19:14:17 +00:00 committed by Andrew Tridgell
parent c5e62eb6e4
commit d97079e53b
3 changed files with 1 additions and 601 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -14,7 +14,7 @@
*/
#include "AP_CRSF_Telem.h"
#include "AP_VideoTX.h"
#include <AP_VideoTX/AP_VideoTX.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Common/AP_FWVersion.h>

451
libraries/AP_RCTelemetry/AP_VideoTX.cpp
View File

@ -1,451 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_VideoTX.h"
#include "AP_CRSF_Telem.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_VideoTX *AP_VideoTX::singleton;
const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
// @Param: ENABLE
// @DisplayName: Is the Video Transmitter enabled or not
// @Description: Toggles the Video Transmitter on and off
// @Values: 0:Disable,1:Enable
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_VideoTX, _enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: POWER
// @DisplayName: Video Transmitter Power Level
// @Description: Video Transmitter Power Level. Different VTXs support different power levels, the power level chosen will be rounded down to the nearest supported power level
// @Range: 1 1000
AP_GROUPINFO("POWER", 2, AP_VideoTX, _power_mw, 0),
// @Param: CHANNEL
// @DisplayName: Video Transmitter Channel
// @Description: Video Transmitter Channel
// @User: Standard
// @Range: 0 7
AP_GROUPINFO("CHANNEL", 3, AP_VideoTX, _channel, 0),
// @Param: BAND
// @DisplayName: Video Transmitter Band
// @Description: Video Transmitter Band
// @User: Standard
// @Values: 0:Band A,1:Band B,2:Band E,3:Airwave,4:RaceBand,5:Low RaceBand
AP_GROUPINFO("BAND", 4, AP_VideoTX, _band, 0),
// @Param: FREQ
// @DisplayName: Video Transmitter Frequency
// @Description: Video Transmitter Frequency. The frequency is derived from the setting of BAND and CHANNEL
// @User: Standard
// @ReadOnly: True
// @Range: 5000 6000
AP_GROUPINFO("FREQ", 5, AP_VideoTX, _frequency_mhz, 0),
// @Param: OPTIONS
// @DisplayName: 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,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
};
extern const AP_HAL::HAL& hal;
const uint16_t AP_VideoTX::VIDEO_CHANNELS[AP_VideoTX::MAX_BANDS][VTX_MAX_CHANNELS] =
{
{ 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, /* Band A */
{ 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, /* Band B */
{ 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945}, /* Band E */
{ 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880}, /* Airwave */
{ 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917}, /* Race */
{ 5621, 5584, 5547, 5510, 5473, 5436, 5399, 5362} /* LO Race */
};
AP_VideoTX::AP_VideoTX()
{
if (singleton) {
AP_HAL::panic("Too many VTXs");
return;
}
singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
AP_VideoTX::~AP_VideoTX(void)
{
singleton = nullptr;
}
bool AP_VideoTX::init(void)
{
if (_initialized) {
return false;
}
_current_power = _power_mw;
_current_band = _band;
_current_channel = _channel;
_current_frequency = _frequency_mhz;
_current_options = _options;
_current_enabled = _enabled;
_initialized = true;
return true;
}
bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel)
{
for (uint8_t i = 0; i < AP_VideoTX::MAX_BANDS; i++) {
for (uint8_t j = 0; j < VTX_MAX_CHANNELS; j++) {
if (VIDEO_CHANNELS[i][j] == freq) {
band = VideoBand(i);
channel = j;
return true;
}
}
}
return false;
}
// set the current power
void AP_VideoTX::set_configured_power_mw(uint16_t power) {
_power_mw.set_and_save_ifchanged(power);
}
// set the power in dbm, rounding appropriately
void AP_VideoTX::set_power_dbm(uint8_t power) {
switch (power) {
case 14:
_current_power = 25;
break;
case 20:
_current_power = 100;
break;
case 23:
_current_power = 200;
break;
case 26:
_current_power = 400;
break;
case 27:
_current_power = 500;
break;
case 29:
_current_power = 800;
break;
default:
_current_power = uint16_t(roundf(powf(10, power / 10.0f)));
break;
}
}
// get the power in dbm, rounding appropriately
uint8_t AP_VideoTX::get_configured_power_dbm() const {
switch (_power_mw.get()) {
case 25: return 14;
case 100: return 20;
case 200: return 23;
case 400: return 26;
case 500: return 27;
case 800: return 29;
default:
return uint8_t(roundf(10.0f * log10f(_power_mw)));
}
}
// 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;
if (!_enabled.configured_in_storage()) {
_enabled.set_and_save(enabled);
}
}
// peiodic update
void AP_VideoTX::update(void)
{
#if HAL_CRSF_TELEM_ENABLED
AP_CRSF_Telem* crsf = AP::crsf_telem();
if (crsf != nullptr) {
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
{
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
bool AP_VideoTX::set_defaults()
{
if (_defaults_set) {
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()) {
_options.set_and_save(_current_options);
}
if (!_channel.configured()) {
_channel.set_and_save(_current_channel);
}
if (!_band.configured()) {
_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(), 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 {
AP_VideoTX& vtx() {
return *AP_VideoTX::get_singleton();
}
};

149
libraries/AP_RCTelemetry/AP_VideoTX.h
View File

@ -1,149 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <RC_Channel/RC_Channel.h>
#define VTX_MAX_CHANNELS 8
class AP_VideoTX {
public:
AP_VideoTX();
~AP_VideoTX();
/* Do not allow copies */
AP_VideoTX(const AP_VideoTX &other) = delete;
AP_VideoTX &operator=(const AP_VideoTX&) = delete;
// init - perform required initialisation
bool init();
// run any required updates
void update();
static AP_VideoTX *get_singleton(void) {
return singleton;
}
static const struct AP_Param::GroupInfo var_info[];
enum class VideoOptions {
VTX_PITMODE = (1 << 0),
VTX_PITMODE_UNTIL_ARM = (1 << 1),
VTX_PITMODE_ON_DISARM = (1 << 2),
VTX_UNLOCKED = (1 << 3),
};
enum VideoBand {
BAND_A,
BAND_B,
BAND_E,
FATSHARK,
RACEBAND,
LOW_RACEBAND,
MAX_BANDS
};
static const uint16_t VIDEO_CHANNELS[MAX_BANDS][VTX_MAX_CHANNELS];
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) { _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); }
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; }
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);
bool get_enabled() const { return _enabled; }
bool update_enabled() const { return _defaults_set && _enabled != _current_enabled; }
// have the parameters been updated
bool have_params_changed() const;
// 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;
uint16_t _current_band;
// frequency channel
AP_Int8 _channel;
uint8_t _current_channel;
// vtx options
AP_Int8 _options;
uint8_t _current_options;
AP_Int8 _enabled;
bool _current_enabled;
bool _initialized;
// when defaults have been configured
bool _defaults_set;
};
namespace AP {
AP_VideoTX& vtx();
};