ardupilot/libraries/AP_RCTelemetry/AP_VideoTX.cpp

/*
   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(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();
    }
};