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

AP_VideoTX: new library to support the SmartAudio protocol 

set the VTX parameters to what was actually configured by the VTX
use change band/change frequency when appropriate
set power levels appropriately for protocol version.

Co-authored-by: luis.martinez.exts <luis.martinez@juntadeandalucia.es>
This commit is contained in:
Andy Piper 2021-01-29 19:52:10 +00:00 committed by Andrew Tridgell
parent 7aa1e2d63b
commit 6f91e053d5
2 changed files with 892 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

623
libraries/AP_VideoTX/AP_SmartAudio.cpp Normal file
View File

@ -0,0 +1,623 @@
/*
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
if (pitMode != pitModeRunning) {
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 (_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;
}
} else if (_vtx_options_change_pending) {
set_operation_mode(mode);
}
}
}
/**
* 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);
vtx.set_power_dbm(settings.power_in_dbm);
// 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

269
libraries/AP_VideoTX/AP_SmartAudio.h Normal file
View File

@ -0,0 +1,269 @@
/*
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>
#ifndef HAL_SMARTAUDIO_ENABLED
#define HAL_SMARTAUDIO_ENABLED !HAL_MINIMIZE_FEATURES
#endif
#if HAL_SMARTAUDIO_ENABLED
#include <AP_Param/AP_Param.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_RCTelemetry/AP_VideoTX.h>
#define SMARTAUDIO_BUFFER_CAPACITY 5
// SmartAudio Serial Protocol
#define AP_SMARTAUDIO_UART_BAUD 4800
#define AP_SMARTAUDIO_SMARTBAUD_MIN 4560 // -5%
#define AP_SMARTAUDIO_SMARTBAUD_MAX 5040 // +5%
#define AP_SMARTAUDIO_SMARTBAUD_STEP 60
#define AP_SMARTAUDIO_UART_BUFSIZE_RX 16
#define AP_SMARTAUDIO_UART_BUFSIZE_TX 16
#define AP_SMARTAUDIO_MAX_PACKET_SIZE 32
#define SMARTAUDIO_SYNC_BYTE 0xAA
#define SMARTAUDIO_HEADER_BYTE 0x55
#define SMARTAUDIO_START_CODE SMARTAUDIO_SYNC_BYTE + SMARTAUDIO_HEADER_BYTE
#define SMARTAUDIO_GET_PITMODE_FREQ (1 << 14)
#define SMARTAUDIO_SET_PITMODE_FREQ (1 << 15)
#define SMARTAUDIO_FREQUENCY_MASK 0x3FFF
#define SMARTAUDIO_CMD_GET_SETTINGS 0x03
#define SMARTAUDIO_CMD_SET_POWER 0x05
#define SMARTAUDIO_CMD_SET_CHANNEL 0x07
#define SMARTAUDIO_CMD_SET_FREQUENCY 0x09
#define SMARTAUDIO_CMD_SET_MODE 0x0B
#define SMARTAUDIO_COMMAND_FRAME_SIZE (sizeof(FrameHeader) + 1)
#define SMARTAUDIO_U8_COMMAND_FRAME_SIZE (sizeof(FrameHeader) + 2)
#define SMARTAUDIO_U16_COMMAND_FRAME_SIZE (sizeof(FrameHeader) + 3)
#define SMARTAUDIO_RSP_GET_SETTINGS_V1 SMARTAUDIO_CMD_GET_SETTINGS >> 1
#define SMARTAUDIO_RSP_GET_SETTINGS_V2 (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x08
#define SMARTAUDIO_RSP_GET_SETTINGS_V21 (SMARTAUDIO_CMD_GET_SETTINGS >> 1) | 0x10
#define SMARTAUDIO_RSP_SET_POWER SMARTAUDIO_CMD_SET_POWER >> 1
#define SMARTAUDIO_RSP_SET_CHANNEL SMARTAUDIO_CMD_SET_CHANNEL >> 1
#define SMARTAUDIO_RSP_SET_FREQUENCY SMARTAUDIO_CMD_SET_FREQUENCY >> 1
#define SMARTAUDIO_RSP_SET_MODE SMARTAUDIO_CMD_SET_MODE >> 1
#define SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel) (band * VTX_MAX_CHANNELS + (channel))
// #define SA_DEBUG
class AP_SmartAudio
{
public:
enum ProtocolVersion {
SMARTAUDIO_SPEC_PROTOCOL_v1 = 0,
SMARTAUDIO_SPEC_PROTOCOL_v2 = 1,
SMARTAUDIO_SPEC_PROTOCOL_v21 = 2
};
struct Settings {
uint8_t version;
uint8_t mode;
uint8_t channel;
uint8_t power;
uint16_t frequency;
uint8_t band;
uint8_t* power_levels;
uint8_t power_in_dbm;
uint16_t pitmodeFrequency;
bool userFrequencyMode; // user is setting freq
bool initialized;
};
struct FrameHeader {
uint8_t syncByte;
uint8_t headerByte;
uint8_t command;
uint8_t length;
void init(uint8_t cmd, uint8_t payloadLength) {
syncByte = SMARTAUDIO_SYNC_BYTE;
headerByte= SMARTAUDIO_HEADER_BYTE;
length = payloadLength;
command = cmd;
}
} PACKED;
struct Frame {
FrameHeader header;
uint8_t payload[3];
} PACKED;
struct U8ResponseFrame {
FrameHeader header;
uint8_t payload;
//uint8_t reserved;
//uint8_t crc;
} PACKED;
struct U16ResponseFrame {
FrameHeader header;
uint16_t payload;
//uint8_t reserved;
//uint8_t crc;
} PACKED;
struct SettingsResponseFrame {
FrameHeader header;
uint8_t channel;
uint8_t power;
uint8_t operationMode;
uint16_t frequency;
//uint8_t crc;
} PACKED;
struct SettingsExtendedResponseFrame {
FrameHeader header;
uint8_t channel;
uint8_t power;
uint8_t operationMode;
uint16_t frequency;
uint8_t power_dbm;
uint8_t power_levels_len;
uint8_t power_dbm_levels; // first in the list of dbm levels
//uint8_t crc;
} PACKED;
// v 2.1 additions to response frame
//0x0E (current power in dBm) 0x03 (amount of power levels) 0x00(dBm level 1) 0x0E (dBm level 2) 0x14 (dBm level 3) 0x1A (dBm level 4) 0x01(CRC8)
// request packet to be processed
struct Packet {
Frame frame;
uint8_t frame_size;
} PACKED;
AP_SmartAudio();
static AP_SmartAudio *get_singleton(void)
{
return _singleton;
}
/* Do not allow copies */
AP_SmartAudio(const AP_SmartAudio &other) = delete;
AP_SmartAudio &operator=(const AP_SmartAudio&) = delete;
// init threads and lookup for io uart.
bool init();
private:
// serial interface
AP_HAL::UARTDriver *_port; // UART used to send data to SmartAudio VTX
//Pointer to singleton
static AP_SmartAudio* _singleton;
// response buffer length, permit splitted responses
uint8_t _inline_buffer_length;
// expected packet size
uint8_t _packet_size;
ProtocolVersion _protocol_version;
// statistics
uint16_t _packets_sent;
uint16_t _packets_rcvd;
bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
bool _vtx_power_change_pending;
bool _vtx_options_change_pending;
bool _vtx_gcs_pending;
bool _vtx_use_set_freq; // should frequency set by band/channel or frequency
// value for current baud adjust
int32_t _smartbaud = AP_SMARTAUDIO_UART_BAUD;
int32_t _smartbaud_direction = 1;
// hw vtx state control with 2 elements array use methods _push _peek
uint8_t _vtx_state_idx;
Settings _vtx_states_buffer[2];
Settings *_vtx_current_state;
// ready to go
volatile bool _initialised;
// RingBuffer to store outgoing request.
ObjectBuffer<Packet> requests_queue{SMARTAUDIO_BUFFER_CAPACITY};
// time the last_request is process
uint32_t _last_request_sent_ms;
// loops is waiting a response after a request
bool _is_waiting_response;
#ifdef SA_DEBUG
// utility method for debugging.
void print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t x,uint8_t offset);
#endif
void print_settings(const Settings* settings);
void update_vtx_params();
void update_vtx_settings(const Settings& settings);
// looping over requests
void loop();
// send a frame over the wire
void send_request(const Frame& requestFrame, uint8_t size);
// receive a frame response over the wire
bool read_response(uint8_t *response_buffer);
// parses the response and updates the vtx settings
bool parse_frame_response(const uint8_t *buffer);
bool parse_response_buffer(const uint8_t *buffer);
// get last reading from the fifo queue
bool get_readings(AP_VideoTX *vtx_dest);
// command functions
// request settings
void request_settings();
// change the mode
void set_operation_mode(uint8_t mode);
// change the frequency
void set_frequency(uint16_t frequency, bool isPitModeFreq);
// change the channel
void set_channel(uint8_t chan);
// get the pitmode frequency
void request_pit_mode_frequency();
// set the power
void set_power(uint16_t power_mw, uint16_t dbm);
// set the power using power_level, spec versions 1 and 2 or dbm value for spec version 2.1
void set_power(uint8_t power_level);
// set the band and channel
void set_band_channel(const uint8_t band, const uint8_t channel);
// command functions
void push_command_only_frame(uint8_t command);
void push_uint8_command_frame(uint8_t command, uint8_t data);
void push_uint16_command_frame(uint8_t command, uint16_t data);
static void unpack_frequency(Settings *settings, const uint16_t frequency);
static void unpack_settings(Settings *settings, const SettingsResponseFrame *frame);
static void unpack_settings(Settings *settings, const SettingsExtendedResponseFrame *frame);
// change baud automatically when request-response fails many times
void update_baud_rate();
};
#endif