Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_VideoTX/AP_SmartAudio.cpp

644 lines
23 KiB
C++
Raw Blame History

/*
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",
768, 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) {
debug("update mode '%c%c%c%c'", (mode & 0x8) ? 'U' : 'L',
(mode & 0x4) ? 'N' : ' ', (mode & 0x2) ? 'O' : ' ', (mode & 0x1) ? 'I' : ' ');
set_operation_mode(mode);
} else if (_vtx_freq_change_pending) {
debug("update frequency");
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) {
debug("update power");
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: '%c%c%c%c%c', CH: %u, PWR: %u, DBM: %u FREQ: %u, BND: %u",
settings->version,
(settings->mode & 0x10) ? 'U' : 'L',
(settings->mode & 0x8) ? 'O' : ' ',
(settings->mode & 0x4) ? 'I' : ' ',
(settings->mode & 0x2) ? 'P' : ' ',
(settings->mode & 0x1) ? 'F' : 'C',
settings->channel, settings->power, settings->power_in_dbm, 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) >> 1);
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
Reference in New Issue View Git Blame Copy Permalink