You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
233 lines
6.4 KiB
233 lines
6.4 KiB
/* |
|
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. |
|
// @User: Advanced |
|
// @Bitmask: 0:Pitmode |
|
AP_GROUPINFO("OPTIONS", 6, AP_VideoTX, _options, 0), |
|
|
|
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_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))); |
|
} |
|
} |
|
|
|
// 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 |
|
} |
|
|
|
bool AP_VideoTX::have_params_changed() const |
|
{ |
|
return update_power() |
|
|| update_band() |
|
|| update_channel() |
|
|| update_options(); |
|
} |
|
|
|
// 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() |
|
{ |
|
if (_defaults_set) { |
|
return; |
|
} |
|
|
|
if (!_options.configured()) { |
|
_options.set_and_save(_current_options); |
|
} |
|
if (!_channel.configured()) { |
|
_channel.set_and_save(_current_channel); |
|
} |
|
if (!_band.configured()) { |
|
_band.set_and_save(_current_band); |
|
} |
|
if (!_power_mw.configured()) { |
|
_power_mw.set_and_save(_current_power); |
|
} |
|
|
|
_defaults_set = true; |
|
// 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); |
|
} |
|
|
|
namespace AP { |
|
AP_VideoTX& vtx() { |
|
return *AP_VideoTX::get_singleton(); |
|
} |
|
}; |