mirror of https://github.com/ArduPilot/ardupilot
634 lines
22 KiB
C++
634 lines
22 KiB
C++
/*
|
|
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/>.
|
|
|
|
SmartAudio protocol parsing and data structures taken from betaflight
|
|
*/
|
|
|
|
#include "AP_SmartAudio.h"
|
|
#include <AP_Math/crc.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
|
|
#if HAL_SMARTAUDIO_ENABLED
|
|
|
|
#ifdef SA_DEBUG
|
|
# define debug(fmt, args...) do { hal.console->printf("SA: " fmt "\n", ##args); } while (0)
|
|
#else
|
|
# define debug(fmt, args...) do {} while(0)
|
|
#endif
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
AP_SmartAudio::AP_SmartAudio()
|
|
{
|
|
_singleton = this;
|
|
}
|
|
|
|
AP_SmartAudio *AP_SmartAudio::_singleton;
|
|
|
|
// initialization start making a request settings to the vtx
|
|
bool AP_SmartAudio::init()
|
|
{
|
|
debug("SmartAudio init");
|
|
|
|
if (AP::vtx().get_enabled()==0) {
|
|
debug("SmartAudio protocol it's not active");
|
|
return false;
|
|
}
|
|
|
|
// init uart
|
|
_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SmartAudio, 0);
|
|
if (_port!=nullptr) {
|
|
_port->configure_parity(0);
|
|
_port->set_stop_bits(2);
|
|
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
|
_port->set_options((_port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV)
|
|
| AP_HAL::UARTDriver::OPTION_HDPLEX | AP_HAL::UARTDriver::OPTION_PULLDOWN_TX | AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_SmartAudio::loop, void),
|
|
"SmartAudio",
|
|
512, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void AP_SmartAudio::loop()
|
|
{
|
|
while (!hal.scheduler->is_system_initialized()) {
|
|
hal.scheduler->delay(100);
|
|
}
|
|
|
|
uint8_t res_retries=0;
|
|
// allocate response buffer
|
|
uint8_t _response_buffer[AP_SMARTAUDIO_MAX_PACKET_SIZE];
|
|
|
|
// initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
|
|
_port->begin(_smartbaud, AP_SMARTAUDIO_UART_BUFSIZE_RX, AP_SMARTAUDIO_UART_BUFSIZE_TX);
|
|
|
|
//bool tried = false;
|
|
|
|
while (true) {
|
|
// now time to control loop switching
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
// when pending request and last request sended is timeout, take another packet to send
|
|
if (!_is_waiting_response) {
|
|
// command to process
|
|
Packet current_command;
|
|
|
|
// repeatedly initialize UART until we know what the VTX is
|
|
if (!_initialised) {
|
|
// request settings every second
|
|
if (requests_queue.is_empty() && !hal.util->get_soft_armed() && now - _last_request_sent_ms > 1000) {
|
|
request_settings();
|
|
}
|
|
}
|
|
|
|
if (requests_queue.pop(current_command)) {
|
|
// send the popped command from bugger
|
|
send_request(current_command.frame, current_command.frame_size);
|
|
|
|
now = AP_HAL::millis();
|
|
// it takes roughly 15ms to send a request, don't turn around and try and read until
|
|
// this time has elapsed
|
|
hal.scheduler->delay(20);
|
|
|
|
_last_request_sent_ms = now;
|
|
|
|
// next loop we expect a response
|
|
_is_waiting_response = true;
|
|
}
|
|
}
|
|
|
|
// nothing going on so give CPU to someone else
|
|
if (!_is_waiting_response && !_initialised) {
|
|
hal.scheduler->delay(100);
|
|
}
|
|
|
|
// On my Unify Pro32 the SmartAudio response is sent exactly 100ms after the request
|
|
// and the initial response is 40ms long so we should wait at least 140ms before giving up
|
|
if (now - _last_request_sent_ms < 200 && _is_waiting_response) {
|
|
|
|
// setup sheduler delay to 50 ms again after response processes
|
|
if (!read_response(_response_buffer)) {
|
|
hal.scheduler->delay(10);
|
|
res_retries++;
|
|
} else {
|
|
res_retries = 0;
|
|
}
|
|
|
|
} else if (_is_waiting_response) { // timeout
|
|
// process autobaud routine
|
|
update_baud_rate();
|
|
_port->discard_input();
|
|
_inline_buffer_length = 0;
|
|
_is_waiting_response = false;
|
|
} else {
|
|
if (AP::vtx().have_params_changed() ||_vtx_power_change_pending
|
|
|| _vtx_freq_change_pending || _vtx_options_change_pending) {
|
|
update_vtx_params();
|
|
_vtx_gcs_pending = true;
|
|
// we've tried to udpate something, re-request the settings so that they
|
|
// are reflected correctly
|
|
request_settings();
|
|
} else if (_vtx_gcs_pending) {
|
|
AP::vtx().announce_vtx_settings();
|
|
_vtx_gcs_pending = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// send requests to the VTX to match the configured VTX parameters
|
|
void AP_SmartAudio::update_vtx_params()
|
|
{
|
|
AP_VideoTX& vtx = AP::vtx();
|
|
|
|
_vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || vtx.update_frequency() || _vtx_freq_change_pending;
|
|
_vtx_power_change_pending = vtx.update_power() || _vtx_power_change_pending;
|
|
_vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending;
|
|
|
|
if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
|
|
// make the desired frequency match the desired band and channel
|
|
if (_vtx_freq_change_pending) {
|
|
if (vtx.update_band() || vtx.update_channel()) {
|
|
vtx.update_configured_frequency();
|
|
} else {
|
|
vtx.update_configured_channel_and_band();
|
|
}
|
|
}
|
|
|
|
debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d",
|
|
vtx.get_frequency_mhz(), vtx.get_configured_frequency_mhz(),
|
|
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_options(), vtx.get_configured_options());
|
|
|
|
uint8_t opts = vtx.get_configured_options();
|
|
uint8_t pitModeRunning = (vtx.get_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
|
|
uint8_t pitMode = opts & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE);
|
|
uint8_t mode;
|
|
// check if we are turning pitmode on or off, but only on SA 2.1 as older versions
|
|
// appear not to work properly
|
|
if (pitMode != pitModeRunning && _protocol_version >= SMARTAUDIO_SPEC_PROTOCOL_v21) {
|
|
if (pitModeRunning) {
|
|
debug("Turning OFF pitmode");
|
|
// turn it off
|
|
mode = 0x04 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
|
|
} else {
|
|
debug("Turning ON pitmode");
|
|
// turn it on
|
|
mode = 0x01 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
|
|
}
|
|
} else {
|
|
mode = ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
|
|
if (pitMode) {
|
|
mode |= 0x01;
|
|
}
|
|
}
|
|
|
|
if (pitMode) {// prevent power changes in pitmode as this takes the VTX out of pitmode
|
|
_vtx_power_change_pending = false;
|
|
}
|
|
|
|
// prioritize pitmode changes
|
|
if (_vtx_options_change_pending) {
|
|
set_operation_mode(mode);
|
|
} else if (_vtx_freq_change_pending) {
|
|
if (_vtx_use_set_freq) {
|
|
set_frequency(vtx.get_configured_frequency_mhz(), false);
|
|
} else {
|
|
set_channel(vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel());
|
|
}
|
|
} else if (_vtx_power_change_pending) {
|
|
switch (_protocol_version) {
|
|
case SMARTAUDIO_SPEC_PROTOCOL_v21:
|
|
set_power(vtx.get_configured_power_dbm() | 0x80);
|
|
break;
|
|
case SMARTAUDIO_SPEC_PROTOCOL_v2:
|
|
set_power(vtx.get_configured_power_level());
|
|
break;
|
|
default: // v1
|
|
switch(vtx.get_configured_power_level()) {
|
|
case 1: set_power(16); break; // 200mw
|
|
case 2: set_power(25); break; // 500mw
|
|
case 3: set_power(40); break; // 800mw
|
|
default: set_power(7); break; // 25mw
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Sends an SmartAudio Command to the vtx, waits response on the update event
|
|
* @param frameBuffer frameBuffer to send over the wire
|
|
* @param size size of the framebuffer wich needs to be sended
|
|
*/
|
|
void AP_SmartAudio::send_request(const Frame& requestFrame, uint8_t size)
|
|
{
|
|
if (size <= 0 || _port == nullptr) {
|
|
return;
|
|
}
|
|
|
|
const uint8_t *request = reinterpret_cast<const uint8_t*>(&requestFrame);
|
|
|
|
// write request
|
|
_port->write(request, size);
|
|
_port->flush();
|
|
|
|
_packets_sent++;
|
|
#ifdef SA_DEBUG
|
|
print_bytes_to_hex_string("send_request():", request, size,0);
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* Reads the response from vtx in the wire
|
|
* - response_buffer, response buffer to fill in
|
|
* - inline_buffer_length , used to passthrought the response lenght in case the response where splitted
|
|
**/
|
|
bool AP_SmartAudio::read_response(uint8_t *response_buffer)
|
|
{
|
|
int16_t incoming_bytes_count = _port->available();
|
|
|
|
const uint8_t response_header_size= sizeof(FrameHeader);
|
|
|
|
// check if it is a response in the wire
|
|
if (incoming_bytes_count <= 0) {
|
|
return false;
|
|
}
|
|
|
|
// wait until we have enough bytes to read a header
|
|
if (incoming_bytes_count < response_header_size && _inline_buffer_length == 0) {
|
|
return false;
|
|
}
|
|
|
|
// now have at least the header, read it if necessary
|
|
if (_inline_buffer_length == 0) {
|
|
uint8_t b = _port->read();
|
|
// didn't see a sync byte, discard and go around again
|
|
if (b != SMARTAUDIO_SYNC_BYTE) {
|
|
return false;
|
|
}
|
|
response_buffer[_inline_buffer_length++] = b;
|
|
|
|
b = _port->read();
|
|
// didn't see a header byte, discard and reset
|
|
if (b != SMARTAUDIO_HEADER_BYTE) {
|
|
_inline_buffer_length = 0;
|
|
return false;
|
|
}
|
|
|
|
response_buffer[_inline_buffer_length++] = b;
|
|
|
|
// read the rest of the header
|
|
for (; _inline_buffer_length < response_header_size; _inline_buffer_length++) {
|
|
b = _port->read();
|
|
response_buffer[_inline_buffer_length] = b;
|
|
}
|
|
|
|
FrameHeader* header = (FrameHeader*)response_buffer;
|
|
incoming_bytes_count -= response_header_size;
|
|
|
|
_packet_size = header->length;
|
|
}
|
|
|
|
// read the rest of the packet
|
|
for (uint8_t i= 0; i < incoming_bytes_count && _inline_buffer_length < _packet_size + response_header_size; i++) {
|
|
uint8_t response_in_bytes = _port->read();
|
|
|
|
// check for overflow
|
|
if (_inline_buffer_length >= AP_SMARTAUDIO_MAX_PACKET_SIZE) {
|
|
_inline_buffer_length = 0;
|
|
_is_waiting_response = false;
|
|
return false;
|
|
}
|
|
|
|
response_buffer[_inline_buffer_length++] = response_in_bytes;
|
|
}
|
|
|
|
// didn't get the whole packet
|
|
if (_inline_buffer_length < _packet_size + response_header_size) {
|
|
return false;
|
|
}
|
|
|
|
#ifdef SA_DEBUG
|
|
print_bytes_to_hex_string("read_response():", response_buffer, incoming_bytes_count,(_inline_buffer_length-incoming_bytes_count)>=0?(_inline_buffer_length-incoming_bytes_count):0);
|
|
#endif
|
|
_is_waiting_response=false;
|
|
|
|
bool correct_parse = parse_response_buffer(response_buffer);
|
|
response_buffer=nullptr;
|
|
_inline_buffer_length=0;
|
|
_packet_size = 0;
|
|
_packets_rcvd++;
|
|
// reset the lost packets to 0
|
|
_packets_sent =_packets_rcvd;
|
|
return correct_parse;
|
|
}
|
|
|
|
// format a simple command and push into the request queue
|
|
void AP_SmartAudio::push_command_only_frame(uint8_t cmd_id)
|
|
{
|
|
Packet command;
|
|
command.frame.header.init(cmd_id, 0);
|
|
command.frame_size = SMARTAUDIO_COMMAND_FRAME_SIZE;
|
|
command.frame.payload[0] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_COMMAND_FRAME_SIZE - 1);
|
|
requests_queue.push_force(command);
|
|
}
|
|
|
|
// format an 8-bit command and push into the request queue
|
|
void AP_SmartAudio::push_uint8_command_frame(uint8_t cmd_id, uint8_t data)
|
|
{
|
|
Packet command;
|
|
command.frame.header.init(cmd_id, sizeof(uint8_t));
|
|
command.frame_size = SMARTAUDIO_U8_COMMAND_FRAME_SIZE;
|
|
|
|
command.frame.payload[0] = data;
|
|
command.frame.payload[1] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_U8_COMMAND_FRAME_SIZE - 1);
|
|
requests_queue.push_force(command);
|
|
}
|
|
|
|
// format an 16-bit command and push into the request queue
|
|
void AP_SmartAudio::push_uint16_command_frame(uint8_t cmd_id, uint16_t data)
|
|
{
|
|
Packet command;
|
|
command.frame.header.init(cmd_id, sizeof(uint16_t));
|
|
command.frame_size = SMARTAUDIO_U16_COMMAND_FRAME_SIZE;
|
|
put_be16_ptr(command.frame.payload, data);
|
|
command.frame.payload[2] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_U16_COMMAND_FRAME_SIZE - 1);
|
|
requests_queue.push_force(command);
|
|
}
|
|
|
|
/**
|
|
* Sends get settings command.
|
|
* */
|
|
void AP_SmartAudio::request_settings()
|
|
{
|
|
debug("request_settings()");
|
|
push_command_only_frame(SMARTAUDIO_CMD_GET_SETTINGS);
|
|
}
|
|
|
|
void AP_SmartAudio::set_operation_mode(uint8_t mode)
|
|
{
|
|
debug("Setting mode to 0x%x", mode);
|
|
push_uint8_command_frame(SMARTAUDIO_CMD_SET_MODE, mode);
|
|
}
|
|
|
|
/**
|
|
* Sets the frequency to transmit in the vtx.
|
|
* When isPitModeFreq active the freq will be set to be used when in pitmode (in range)
|
|
*/
|
|
void AP_SmartAudio::set_frequency(uint16_t frequency, bool isPitModeFreq)
|
|
{
|
|
debug("Setting frequency to %d with pitmode == %d", frequency, isPitModeFreq);
|
|
push_uint16_command_frame(SMARTAUDIO_CMD_SET_FREQUENCY,
|
|
frequency | (isPitModeFreq ? SMARTAUDIO_SET_PITMODE_FREQ : 0x00));
|
|
}
|
|
|
|
// enqueue a set channel request
|
|
void AP_SmartAudio::set_channel(uint8_t channel)
|
|
{
|
|
debug("Setting channel to %d", channel);
|
|
push_uint8_command_frame(SMARTAUDIO_CMD_SET_CHANNEL, channel);
|
|
}
|
|
|
|
/**
|
|
* Request pitMode Frequency setted into the vtx hardware
|
|
* */
|
|
void AP_SmartAudio::request_pit_mode_frequency()
|
|
{
|
|
debug("Requesting pit mode frequency");
|
|
push_uint16_command_frame(SMARTAUDIO_CMD_SET_FREQUENCY, SMARTAUDIO_GET_PITMODE_FREQ);
|
|
}
|
|
|
|
// send vtx request to set power
|
|
void AP_SmartAudio::set_power(uint8_t power_level)
|
|
{
|
|
debug("Setting power to %d", power_level);
|
|
push_uint8_command_frame(SMARTAUDIO_CMD_SET_POWER, power_level);
|
|
}
|
|
|
|
|
|
void AP_SmartAudio::set_band_channel(const uint8_t band, const uint8_t channel)
|
|
{
|
|
debug("Setting band/channel to %d/%d", band, channel);
|
|
push_uint16_command_frame(SMARTAUDIO_CMD_SET_CHANNEL, SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel));
|
|
}
|
|
|
|
void AP_SmartAudio::unpack_frequency(AP_SmartAudio::Settings *settings, const uint16_t frequency)
|
|
{
|
|
if (frequency & SMARTAUDIO_GET_PITMODE_FREQ) {
|
|
settings->pitmodeFrequency = frequency;
|
|
} else {
|
|
settings->frequency = frequency;
|
|
}
|
|
}
|
|
|
|
void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsResponseFrame *frame)
|
|
{
|
|
settings->channel = frame->channel % VTX_MAX_CHANNELS;
|
|
settings->band = frame->channel / VTX_MAX_CHANNELS;
|
|
settings->power = frame->power;
|
|
settings->mode = frame->operationMode;
|
|
unpack_frequency(settings, be16toh(frame->frequency));
|
|
}
|
|
|
|
void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsExtendedResponseFrame *frame)
|
|
{
|
|
settings->channel = frame->channel % VTX_MAX_CHANNELS;
|
|
settings->band = frame->channel / VTX_MAX_CHANNELS;
|
|
settings->power = frame->power;
|
|
settings->power_in_dbm=frame->power_dbm;
|
|
settings->mode = frame->operationMode;
|
|
unpack_frequency(settings, be16toh(frame->frequency));
|
|
}
|
|
|
|
#ifdef SA_DEBUG
|
|
void AP_SmartAudio::print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t len,uint8_t offset)
|
|
{
|
|
hal.console->printf("SA: %s ", msg);
|
|
for (uint8_t i = 0; i < len; i++) {
|
|
hal.console->printf("0x%02X ", buf[i+offset]);
|
|
}
|
|
hal.console->printf("\n");
|
|
}
|
|
#endif
|
|
|
|
void AP_SmartAudio::print_settings(const Settings* settings)
|
|
{
|
|
debug("SETTINGS: VER: %u, MD: 0x%x, CH: %u, PWR: %u, FREQ: %u, BND: %u",
|
|
settings->version, settings->mode, settings->channel, settings->power, settings->frequency, settings->band);
|
|
}
|
|
|
|
void AP_SmartAudio::update_vtx_settings(const Settings& settings)
|
|
{
|
|
AP_VideoTX& vtx = AP::vtx();
|
|
|
|
vtx.set_enabled(true);
|
|
vtx.set_frequency_mhz(settings.frequency);
|
|
vtx.set_band(settings.band);
|
|
vtx.set_channel(settings.channel);
|
|
if (settings.version == SMARTAUDIO_SPEC_PROTOCOL_v21) {
|
|
vtx.set_power_dbm(settings.power_in_dbm);
|
|
} else {
|
|
vtx.set_power_level(settings.power);
|
|
}
|
|
// it seems like the spec is wrong, on a unify pro32 this setting is inverted
|
|
_vtx_use_set_freq = !(settings.mode & 1);
|
|
|
|
// PITMODE | UNLOCKED
|
|
// SmartAudio 2.1 dropped support for outband pitmode so we won't support it
|
|
uint8_t opts = ((settings.mode & 0x2) >> 1) | ((settings.mode & 0x10) >> 3);
|
|
vtx.set_options(opts);
|
|
|
|
// make sure the configured values now reflect reality
|
|
vtx.set_defaults();
|
|
|
|
_initialised = true;
|
|
|
|
_vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
|
|
}
|
|
|
|
bool AP_SmartAudio::parse_response_buffer(const uint8_t *buffer)
|
|
{
|
|
const FrameHeader *header = (const FrameHeader *)buffer;
|
|
const uint8_t fullFrameLength = sizeof(FrameHeader) + header->length;
|
|
const uint8_t headerPayloadLength = fullFrameLength - 1; // subtract crc byte from length
|
|
const uint8_t *startPtr = buffer + 2;
|
|
const uint8_t *endPtr = buffer + headerPayloadLength;
|
|
|
|
if (crc8_dvb_s2_update(0x00, startPtr, headerPayloadLength-2)!=*(endPtr)
|
|
|| header->headerByte != SMARTAUDIO_HEADER_BYTE
|
|
|| header->syncByte!=SMARTAUDIO_SYNC_BYTE) {
|
|
return false;
|
|
}
|
|
// SEND TO GCS A MESSAGE TO UNDERSTAND WHATS HAPPENING
|
|
AP_VideoTX& vtx = AP::vtx();
|
|
Settings settings {};
|
|
|
|
switch (header->command) {
|
|
case SMARTAUDIO_RSP_GET_SETTINGS_V1:
|
|
_protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v1;
|
|
unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
|
|
settings.version = SMARTAUDIO_SPEC_PROTOCOL_v1;
|
|
print_settings(&settings);
|
|
update_vtx_settings(settings);
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_GET_SETTINGS_V2:
|
|
_protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v2;
|
|
unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
|
|
settings.version = SMARTAUDIO_SPEC_PROTOCOL_v2;
|
|
print_settings(&settings);
|
|
update_vtx_settings(settings);
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_GET_SETTINGS_V21:
|
|
_protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v21;
|
|
unpack_settings(&settings, (const SettingsExtendedResponseFrame *)buffer);
|
|
settings.version = SMARTAUDIO_SPEC_PROTOCOL_v21;
|
|
print_settings(&settings);
|
|
update_vtx_settings(settings);
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_SET_FREQUENCY: {
|
|
const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
|
|
unpack_frequency(&settings, resp->payload);
|
|
vtx.set_frequency_mhz(settings.frequency);
|
|
vtx.set_configured_frequency_mhz(vtx.get_frequency_mhz());
|
|
vtx.update_configured_channel_and_band();
|
|
debug("Frequency was set to %d", settings.frequency);
|
|
}
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_SET_CHANNEL: {
|
|
const U8ResponseFrame *resp = (const U8ResponseFrame *)buffer;
|
|
vtx.set_band(resp->payload / VTX_MAX_CHANNELS);
|
|
vtx.set_channel(resp->payload % VTX_MAX_CHANNELS);
|
|
vtx.set_configured_channel(vtx.get_channel());
|
|
vtx.set_configured_band(vtx.get_band());
|
|
vtx.update_configured_frequency();
|
|
debug("Channel was set to %d", resp->payload);
|
|
}
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_SET_POWER: {
|
|
const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
|
|
const uint8_t power = resp->payload & 0xFF;
|
|
switch (_protocol_version) {
|
|
case SMARTAUDIO_SPEC_PROTOCOL_v21:
|
|
vtx.set_power_dbm(power);
|
|
vtx.set_configured_power_mw(vtx.get_power_mw());
|
|
break;
|
|
case SMARTAUDIO_SPEC_PROTOCOL_v2:
|
|
vtx.set_power_level(power);
|
|
vtx.set_configured_power_mw(vtx.get_power_mw());
|
|
break;
|
|
default:
|
|
switch(power) {
|
|
case 16: vtx.set_power_level(1); break; // 200mw
|
|
case 25: vtx.set_power_level(2); break; // 500mw
|
|
case 40: vtx.set_power_level(3); break; // 800mw
|
|
default: vtx.set_power_level(0); break; // 25mw
|
|
}
|
|
vtx.set_configured_power_mw(vtx.get_power_mw());
|
|
break;
|
|
}
|
|
debug("Power was set to to %d", power);
|
|
}
|
|
break;
|
|
|
|
case SMARTAUDIO_RSP_SET_MODE: {
|
|
vtx.set_options(vtx.get_configured_options()); // easiest to just make them match
|
|
debug("Mode was set to 0x%x", buffer[4]);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// we missed a response too many times - update the baud rate in case the temperature has increased
|
|
void AP_SmartAudio::update_baud_rate()
|
|
{
|
|
// on my Unify Pro32 the VTX will respond immediately on power up to a settings request, so 10 packets is easily more than enough
|
|
// we want to bias autobaud to only frequency hop when the current frequency is clearly exhausted, but after that hop quickly
|
|
if (_packets_sent - _packets_rcvd < 10) {
|
|
return;
|
|
}
|
|
|
|
if ((_smartbaud_direction == 1) && (_smartbaud == AP_SMARTAUDIO_SMARTBAUD_MAX)) {
|
|
_smartbaud_direction = -1;
|
|
} else if ((_smartbaud_direction == -1 && _smartbaud == AP_SMARTAUDIO_SMARTBAUD_MIN)) {
|
|
_smartbaud_direction = 1;
|
|
}
|
|
|
|
_smartbaud += AP_SMARTAUDIO_SMARTBAUD_STEP * _smartbaud_direction;
|
|
|
|
debug("autobaud: %d", int(_smartbaud));
|
|
|
|
_port->begin(_smartbaud);
|
|
}
|
|
|
|
#endif // HAL_SMARTAUDIO_ENABLED
|