Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 :
15 : SmartAudio protocol parsing and data structures taken from betaflight
16 : */
17 :
18 : #include "AP_SmartAudio.h"
19 : #include <AP_Math/crc.h>
20 : #include <GCS_MAVLink/GCS.h>
21 : #include <AP_HAL/utility/sparse-endian.h>
22 : #include <AP_SerialManager/AP_SerialManager.h>
23 :
24 : #if AP_SMARTAUDIO_ENABLED
25 :
26 : #ifdef SA_DEBUG
27 : # define debug(fmt, args...) do { hal.console->printf("SA: " fmt "\n", ##args); } while (0)
28 : #else
29 : # define debug(fmt, args...) do {} while(0)
30 : #endif
31 :
32 : extern const AP_HAL::HAL &hal;
33 :
34 2098 : AP_SmartAudio::AP_SmartAudio()
35 : {
36 2098 : _singleton = this;
37 2098 : }
38 :
39 : AP_SmartAudio *AP_SmartAudio::_singleton;
40 :
41 : // initialization start making a request settings to the vtx
42 2086 : bool AP_SmartAudio::init()
43 : {
44 : debug("SmartAudio init");
45 :
46 2086 : if (AP::vtx().get_enabled()==0) {
47 : debug("SmartAudio protocol it's not active");
48 2082 : return false;
49 : }
50 :
51 : // init uart
52 4 : _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SmartAudio, 0);
53 4 : if (_port!=nullptr) {
54 0 : _port->configure_parity(0);
55 0 : _port->set_stop_bits(AP::vtx().has_option(AP_VideoTX::VideoOptions::VTX_SA_ONE_STOP_BIT) ? 1 : 2);
56 0 : _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
57 0 : _port->set_options((_port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV)
58 0 : | AP_HAL::UARTDriver::OPTION_HDPLEX | AP_HAL::UARTDriver::OPTION_PULLDOWN_TX | AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);
59 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_SmartAudio::loop, void),
60 : "SmartAudio",
61 : 768, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
62 0 : return false;
63 : }
64 :
65 0 : AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::SmartAudio);
66 :
67 0 : return true;
68 : }
69 4 : return false;
70 : }
71 :
72 0 : void AP_SmartAudio::loop()
73 : {
74 0 : AP_VideoTX &vtx = AP::vtx();
75 :
76 0 : while (!hal.scheduler->is_system_initialized()) {
77 0 : hal.scheduler->delay(100);
78 : }
79 :
80 : // allocate response buffer
81 : uint8_t _response_buffer[AP_SMARTAUDIO_MAX_PACKET_SIZE];
82 :
83 : // 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)
84 0 : _port->begin(_smartbaud, AP_SMARTAUDIO_UART_BUFSIZE_RX, AP_SMARTAUDIO_UART_BUFSIZE_TX);
85 :
86 :
87 : while (true) {
88 : // now time to control loop switching
89 0 : uint32_t now = AP_HAL::millis();
90 :
91 : // when pending request and last request sended is timeout, take another packet to send
92 0 : if (!_is_waiting_response) {
93 : // command to process
94 : Packet current_command;
95 :
96 : // repeatedly initialize UART until we know what the VTX is
97 0 : if (!_initialised) {
98 : // request settings every second
99 0 : if (requests_queue.is_empty() && !hal.util->get_soft_armed() && now - _last_request_sent_ms > 1000) {
100 0 : request_settings();
101 : }
102 : }
103 :
104 0 : if (requests_queue.pop(current_command)) {
105 : // send the popped command from bugger
106 0 : send_request(current_command.frame, current_command.frame_size);
107 :
108 0 : now = AP_HAL::millis();
109 : // it takes roughly 15ms to send a request, don't turn around and try and read until
110 : // this time has elapsed
111 0 : hal.scheduler->delay(20);
112 :
113 0 : _last_request_sent_ms = now;
114 :
115 : // next loop we expect a response
116 0 : _is_waiting_response = true;
117 : }
118 : }
119 :
120 : // nothing going on so give CPU to someone else
121 0 : if (!_is_waiting_response || !_initialised) {
122 0 : hal.scheduler->delay(100);
123 : }
124 :
125 : // On my Unify Pro32 the SmartAudio response is sent exactly 100ms after the request
126 : // and the initial response is 40ms long so we should wait at least 140ms before giving up
127 0 : if (now - _last_request_sent_ms < 200 && _is_waiting_response) {
128 :
129 : // setup scheduler delay to 50 ms again after response processes
130 0 : if (!read_response(_response_buffer)) {
131 0 : hal.scheduler->delay(10);
132 : } else {
133 : // successful response, wait another 100ms to give the VTX a chance to recover
134 : // before sending another command. This is needed on the Atlatl v1.
135 0 : hal.scheduler->delay(100);
136 : }
137 :
138 0 : } else if (_is_waiting_response) { // timeout
139 : // process autobaud routine
140 0 : update_baud_rate();
141 0 : _port->discard_input();
142 0 : _inline_buffer_length = 0;
143 0 : _is_waiting_response = false;
144 : debug("response timeout");
145 0 : } else if (_initialised) {
146 0 : if (AP::vtx().have_params_changed() ||_vtx_power_change_pending
147 0 : || _vtx_freq_change_pending || _vtx_options_change_pending) {
148 0 : update_vtx_params();
149 0 : set_configuration_pending(true);
150 0 : vtx.set_configuration_finished(false);
151 : // we've tried to update something, re-request the settings so that they
152 : // are reflected correctly
153 0 : request_settings();
154 0 : } else if (is_configuration_pending()) {
155 0 : AP::vtx().announce_vtx_settings();
156 0 : set_configuration_pending(false);
157 0 : vtx.set_configuration_finished(true);
158 : }
159 : }
160 0 : }
161 : }
162 :
163 : // send requests to the VTX to match the configured VTX parameters
164 0 : void AP_SmartAudio::update_vtx_params()
165 : {
166 0 : AP_VideoTX& vtx = AP::vtx();
167 :
168 0 : _vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || vtx.update_frequency() || _vtx_freq_change_pending;
169 0 : _vtx_power_change_pending = vtx.update_power() || _vtx_power_change_pending;
170 0 : _vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending;
171 :
172 0 : if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
173 : // make the desired frequency match the desired band and channel
174 0 : if (_vtx_freq_change_pending) {
175 0 : if (vtx.update_band() || vtx.update_channel()) {
176 0 : vtx.update_configured_frequency();
177 : } else {
178 0 : vtx.update_configured_channel_and_band();
179 : }
180 : }
181 :
182 : debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d",
183 : vtx.get_frequency_mhz(), vtx.get_configured_frequency_mhz(),
184 : vtx.get_channel(), vtx.get_configured_channel(),
185 : vtx.get_band(), vtx.get_configured_band(),
186 : vtx.get_power_mw(), vtx.get_configured_power_mw(),
187 : vtx.get_options() & 0xF, vtx.get_configured_options() & 0xF);
188 :
189 0 : uint8_t opts = vtx.get_configured_options();
190 0 : uint8_t pitMode = vtx.get_configured_pitmode();
191 : uint8_t mode;
192 : // check if we are turning pitmode on or off, but only on SA 2.0+
193 0 : if (pitMode != vtx.get_pitmode() && _protocol_version >= SMARTAUDIO_SPEC_PROTOCOL_v2) {
194 0 : if (vtx.get_pitmode()) {
195 : debug("Turning OFF pitmode");
196 : // turn it off
197 0 : mode = 0x04 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
198 : } else {
199 : debug("Turning ON pitmode");
200 : // turn it on (in range pitmode flag)
201 0 : mode = 0x01 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
202 : }
203 : } else {
204 0 : mode = ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
205 0 : if (pitMode) {
206 0 : mode |= 0x01;
207 : }
208 : }
209 :
210 0 : if (pitMode) {// prevent power changes in pitmode as this takes the VTX out of pitmode
211 0 : _vtx_power_change_pending = false;
212 : }
213 :
214 : // prioritize pitmode changes
215 0 : if (_vtx_options_change_pending) {
216 : debug("update mode '%c%c%c%c'", (mode & 0x8) ? 'U' : 'L',
217 : (mode & 0x4) ? 'N' : ' ', (mode & 0x2) ? 'O' : ' ', (mode & 0x1) ? 'I' : ' ');
218 0 : set_operation_mode(mode);
219 0 : } else if (_vtx_freq_change_pending) {
220 : debug("update frequency");
221 0 : if (_vtx_use_set_freq) {
222 0 : set_frequency(vtx.get_configured_frequency_mhz(), false);
223 : } else {
224 0 : set_channel(vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel());
225 : }
226 0 : } else if (_vtx_power_change_pending) {
227 : debug("update power (ver %u)", _protocol_version);
228 0 : switch (_protocol_version) {
229 0 : case SMARTAUDIO_SPEC_PROTOCOL_v21:
230 0 : set_power(vtx.get_configured_power_dbm() | 0x80);
231 0 : break;
232 0 : case SMARTAUDIO_SPEC_PROTOCOL_v2:
233 0 : set_power(vtx.get_configured_power_level());
234 0 : break;
235 0 : default: // v1
236 0 : switch(vtx.get_configured_power_level()) {
237 0 : case 1: set_power(16); break; // 200mw
238 0 : case 2: set_power(25); break; // 500mw
239 0 : case 3: set_power(40); break; // 800mw
240 0 : default: set_power(7); break; // 25mw
241 : }
242 0 : break;
243 : }
244 : }
245 0 : } else {
246 0 : vtx.set_configuration_finished(true);
247 : }
248 0 : }
249 : /**
250 : * Sends an SmartAudio Command to the vtx, waits response on the update event
251 : * @param frameBuffer frameBuffer to send over the wire
252 : * @param size size of the framebuffer wich needs to be sended
253 : */
254 0 : void AP_SmartAudio::send_request(const Frame& requestFrame, uint8_t size)
255 : {
256 0 : AP_VideoTX &vtx = AP::vtx();
257 :
258 0 : if (size <= 0 || _port == nullptr) {
259 0 : return;
260 : }
261 :
262 0 : const uint8_t *request = reinterpret_cast<const uint8_t*>(&requestFrame);
263 :
264 : // write request
265 0 : if (vtx.has_option(AP_VideoTX::VideoOptions::VTX_PULLDOWN)) {
266 0 : _port->write((uint8_t)0x00);
267 : }
268 0 : _port->write(request, size);
269 0 : _port->flush();
270 :
271 0 : _packets_sent++;
272 : #ifdef SA_DEBUG
273 : print_bytes_to_hex_string("send_request():", request, size);
274 : #endif
275 : }
276 :
277 : /**
278 : * Reads the response from vtx in the wire
279 : * - response_buffer, response buffer to fill in
280 : * - inline_buffer_length , used to passthrought the response lenght in case the response where splitted
281 : **/
282 0 : bool AP_SmartAudio::read_response(uint8_t *response_buffer)
283 : {
284 0 : int16_t incoming_bytes_count = _port->available();
285 :
286 0 : const uint8_t response_header_size= sizeof(FrameHeader);
287 :
288 : // check if it is a response in the wire
289 0 : if (incoming_bytes_count <= 0) {
290 0 : return false;
291 : }
292 :
293 : // wait until we have enough bytes to read a header
294 0 : if (incoming_bytes_count < response_header_size && _inline_buffer_length == 0) {
295 0 : return false;
296 : }
297 :
298 : // now have at least the header, read it if necessary
299 0 : if (_inline_buffer_length == 0) {
300 0 : uint8_t b = _port->read();
301 : // didn't see a sync byte, discard and go around again
302 0 : if (b != SMARTAUDIO_SYNC_BYTE) {
303 0 : return false;
304 : }
305 0 : response_buffer[_inline_buffer_length++] = b;
306 :
307 0 : b = _port->read();
308 : // didn't see a header byte, discard and reset
309 0 : if (b != SMARTAUDIO_HEADER_BYTE) {
310 0 : _inline_buffer_length = 0;
311 0 : return false;
312 : }
313 :
314 0 : response_buffer[_inline_buffer_length++] = b;
315 :
316 : // read the rest of the header
317 0 : for (; _inline_buffer_length < response_header_size; _inline_buffer_length++) {
318 0 : b = _port->read();
319 0 : response_buffer[_inline_buffer_length] = b;
320 : }
321 :
322 0 : FrameHeader* header = (FrameHeader*)response_buffer;
323 0 : incoming_bytes_count -= response_header_size;
324 :
325 : // implementations that ignore the CRC also appear to not account for it in the frame length
326 0 : if (ignore_crc()) {
327 0 : header->length++;
328 : }
329 0 : _packet_size = header->length;
330 : }
331 :
332 : // read the rest of the packet
333 0 : for (uint8_t i= 0; i < incoming_bytes_count && _inline_buffer_length < _packet_size + response_header_size; i++) {
334 0 : uint8_t response_in_bytes = _port->read();
335 :
336 : // check for overflow
337 0 : if (_inline_buffer_length >= AP_SMARTAUDIO_MAX_PACKET_SIZE) {
338 0 : _inline_buffer_length = 0;
339 0 : _is_waiting_response = false;
340 0 : return false;
341 : }
342 :
343 0 : response_buffer[_inline_buffer_length++] = response_in_bytes;
344 : }
345 :
346 : // didn't get the whole packet
347 0 : if (_inline_buffer_length < _packet_size + response_header_size) {
348 0 : return false;
349 : }
350 :
351 : #ifdef SA_DEBUG
352 : print_bytes_to_hex_string("read_response():", response_buffer, _inline_buffer_length);
353 : #endif
354 0 : _is_waiting_response = false;
355 :
356 0 : bool correct_parse = parse_response_buffer(response_buffer, _inline_buffer_length);
357 0 : response_buffer = nullptr;
358 0 : _inline_buffer_length=0;
359 0 : _packet_size = 0;
360 0 : _packets_rcvd++;
361 : // reset the lost packets to 0
362 0 : _packets_sent =_packets_rcvd;
363 0 : return correct_parse;
364 : }
365 :
366 : // format a simple command and push into the request queue
367 0 : void AP_SmartAudio::push_command_only_frame(uint8_t cmd_id)
368 : {
369 : Packet command;
370 : // according to the spec the length should include the CRC, but no implementation appears to
371 : // do this
372 0 : command.frame.header.init(cmd_id, 0);
373 0 : command.frame_size = SMARTAUDIO_COMMAND_FRAME_SIZE;
374 0 : command.frame.payload[0] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_COMMAND_FRAME_SIZE - 1);
375 0 : requests_queue.push_force(command);
376 0 : }
377 :
378 : // format an 8-bit command and push into the request queue
379 0 : void AP_SmartAudio::push_uint8_command_frame(uint8_t cmd_id, uint8_t data)
380 : {
381 : Packet command;
382 0 : command.frame.header.init(cmd_id, sizeof(uint8_t));
383 0 : command.frame_size = SMARTAUDIO_U8_COMMAND_FRAME_SIZE;
384 :
385 0 : command.frame.payload[0] = data;
386 0 : command.frame.payload[1] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_U8_COMMAND_FRAME_SIZE - 1);
387 0 : requests_queue.push_force(command);
388 0 : }
389 :
390 : // format an 16-bit command and push into the request queue
391 0 : void AP_SmartAudio::push_uint16_command_frame(uint8_t cmd_id, uint16_t data)
392 : {
393 : Packet command;
394 0 : command.frame.header.init(cmd_id, sizeof(uint16_t));
395 0 : command.frame_size = SMARTAUDIO_U16_COMMAND_FRAME_SIZE;
396 0 : put_be16_ptr(command.frame.payload, data);
397 0 : command.frame.payload[2] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_U16_COMMAND_FRAME_SIZE - 1);
398 0 : requests_queue.push_force(command);
399 0 : }
400 :
401 : /**
402 : * Sends get settings command.
403 : * */
404 0 : void AP_SmartAudio::request_settings()
405 : {
406 : debug("request_settings()");
407 0 : push_command_only_frame(SMARTAUDIO_CMD_GET_SETTINGS);
408 0 : }
409 :
410 0 : void AP_SmartAudio::set_operation_mode(uint8_t mode)
411 : {
412 : debug("Setting mode to 0x%x", mode);
413 0 : push_uint8_command_frame(SMARTAUDIO_CMD_SET_MODE, mode);
414 0 : }
415 :
416 : /**
417 : * Sets the frequency to transmit in the vtx.
418 : * When isPitModeFreq active the freq will be set to be used when in pitmode (in range)
419 : */
420 0 : void AP_SmartAudio::set_frequency(uint16_t frequency, bool isPitModeFreq)
421 : {
422 : debug("Setting frequency to %d with pitmode == %d", frequency, isPitModeFreq);
423 0 : push_uint16_command_frame(SMARTAUDIO_CMD_SET_FREQUENCY,
424 0 : frequency | (isPitModeFreq ? SMARTAUDIO_SET_PITMODE_FREQ : 0x00));
425 0 : }
426 :
427 : // enqueue a set channel request
428 0 : void AP_SmartAudio::set_channel(uint8_t channel)
429 : {
430 : debug("Setting channel to %d", channel);
431 0 : push_uint8_command_frame(SMARTAUDIO_CMD_SET_CHANNEL, channel);
432 0 : }
433 :
434 : /**
435 : * Request pitMode Frequency setted into the vtx hardware
436 : * */
437 0 : void AP_SmartAudio::request_pit_mode_frequency()
438 : {
439 : debug("Requesting pit mode frequency");
440 0 : push_uint16_command_frame(SMARTAUDIO_CMD_SET_FREQUENCY, SMARTAUDIO_GET_PITMODE_FREQ);
441 0 : }
442 :
443 : // send vtx request to set power
444 0 : void AP_SmartAudio::set_power(uint8_t power_level)
445 : {
446 : debug("Setting power to %d", power_level);
447 0 : push_uint8_command_frame(SMARTAUDIO_CMD_SET_POWER, power_level);
448 0 : }
449 :
450 :
451 0 : void AP_SmartAudio::set_band_channel(const uint8_t band, const uint8_t channel)
452 : {
453 : debug("Setting band/channel to %d/%d", band, channel);
454 0 : push_uint16_command_frame(SMARTAUDIO_CMD_SET_CHANNEL, SMARTAUDIO_BANDCHAN_TO_INDEX(band, channel));
455 0 : }
456 :
457 0 : void AP_SmartAudio::unpack_frequency(AP_SmartAudio::Settings *settings, const uint16_t frequency)
458 : {
459 0 : if (frequency & SMARTAUDIO_GET_PITMODE_FREQ) {
460 0 : settings->pitmodeFrequency = frequency;
461 : } else {
462 0 : settings->frequency = frequency;
463 : }
464 0 : }
465 :
466 : // SmartAudio v1/v2
467 0 : void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsResponseFrame *frame)
468 : {
469 0 : settings->channel = frame->channel % VTX_MAX_CHANNELS;
470 0 : settings->band = frame->channel / VTX_MAX_CHANNELS;
471 0 : settings->power = frame->power;
472 0 : settings->mode = frame->operationMode;
473 0 : settings->num_power_levels = 0;
474 0 : unpack_frequency(settings, be16toh(frame->frequency));
475 0 : }
476 :
477 : // SmartAudio v2.1
478 0 : void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsExtendedResponseFrame *frame)
479 : {
480 0 : unpack_settings(settings, &frame->settings);
481 0 : settings->power_in_dbm = frame->power_dbm;
482 : // the level count comes from the wire; clamp it to the size of the
483 : // destination (and source) arrays to avoid overrunning them
484 0 : uint8_t num_power_levels = frame->num_power_levels + 1;
485 0 : if (num_power_levels > sizeof(settings->power_levels)) {
486 0 : num_power_levels = sizeof(settings->power_levels);
487 : }
488 0 : settings->num_power_levels = num_power_levels;
489 0 : memcpy(settings->power_levels, frame->power_levels, num_power_levels);
490 0 : }
491 :
492 : #ifdef SA_DEBUG
493 : void AP_SmartAudio::print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t len)
494 : {
495 : hal.console->printf("SA: %s ", msg);
496 : for (uint8_t i = 0; i < len; i++) {
497 : hal.console->printf("0x%02X ", buf[i]);
498 : }
499 : hal.console->printf("\n");
500 : }
501 : #endif
502 :
503 0 : void AP_SmartAudio::print_settings(const Settings* settings)
504 : {
505 : debug("SETTINGS: VER: %u, MD: '%c%c%c%c%c', CH: %u, PWR: %u, DBM: %u FREQ: %u, BND: %u",
506 : settings->version,
507 : (settings->mode & 0x10) ? 'U' : 'L',// (L)ocked or (U)nlocked
508 : (settings->mode & 0x8) ? 'O' : ' ', // (O)ut-range pitmode
509 : (settings->mode & 0x4) ? 'I' : ' ', // (I)n-range pitmode
510 : (settings->mode & 0x2) ? 'P' : ' ', // (P)itmode running
511 : (settings->mode & 0x1) ? 'F' : 'C', // Set (F)requency or (C)hannel
512 : settings->channel, settings->power, settings->power_in_dbm, settings->frequency, settings->band);
513 0 : }
514 :
515 0 : void AP_SmartAudio::update_vtx_settings(const Settings& settings)
516 : {
517 0 : AP_VideoTX& vtx = AP::vtx();
518 :
519 0 : vtx.set_enabled(true);
520 0 : vtx.set_frequency_mhz(settings.frequency);
521 0 : vtx.set_band(settings.band);
522 0 : vtx.set_channel(settings.channel);
523 : // SA21 sends us a complete packet with the supported power levels
524 0 : if (settings.version == SMARTAUDIO_SPEC_PROTOCOL_v21) {
525 0 : vtx.set_power_dbm(settings.power_in_dbm);
526 : // learn them all
527 0 : vtx.update_all_power_dbm(settings.num_power_levels, settings.power_levels);
528 0 : } else if (settings.version == SMARTAUDIO_SPEC_PROTOCOL_v2) {
529 0 : vtx.set_power_level(settings.power, AP_VideoTX::PowerActive::Active);
530 : // learn them all - it's not possible to know the mw values in v2.0 so just have to go from the spec
531 0 : uint8_t power[] { 0, 14, 23, 27, 29 };
532 0 : vtx.update_all_power_dbm(5, power);
533 : } else {
534 0 : vtx.set_power_level(settings.power, AP_VideoTX::PowerActive::Active);
535 : }
536 : // it seems like the spec is wrong, on a unify pro32 this setting is inverted
537 0 : _vtx_use_set_freq = !(settings.mode & 1);
538 :
539 : // PITMODE | UNLOCKED
540 : // SmartAudio 2.1 dropped support for outband pitmode so we won't support it
541 0 : uint8_t opts = ((settings.mode & 0x2) >> 1) | ((settings.mode & 0x10) >> 1);
542 0 : vtx.set_options(opts);
543 :
544 : // make sure the configured values now reflect reality
545 0 : vtx.set_defaults();
546 :
547 0 : _initialised = true;
548 :
549 0 : _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
550 0 : }
551 :
552 0 : bool AP_SmartAudio::parse_response_buffer(const uint8_t *buffer, uint8_t buffer_length)
553 : {
554 0 : const FrameHeader *header = (const FrameHeader *)buffer;
555 0 : const uint8_t fullFrameLength = sizeof(FrameHeader) + header->length;
556 0 : const uint8_t headerPayloadLength = fullFrameLength - 1; // subtract crc byte from length
557 0 : const uint8_t *startPtr = buffer + 2;
558 0 : const uint8_t *endPtr = buffer + headerPayloadLength;
559 :
560 0 : if ((crc8_dvb_s2_update(0x00, startPtr, headerPayloadLength-2)!=*(endPtr) && !ignore_crc())
561 0 : || header->headerByte != SMARTAUDIO_HEADER_BYTE
562 0 : || header->syncByte != SMARTAUDIO_SYNC_BYTE) {
563 : debug("parse_response_buffer() failed - invalid CRC or header");
564 0 : return false;
565 : }
566 0 : AP_VideoTX& vtx = AP::vtx();
567 0 : Settings settings {};
568 :
569 : // the length field is taken from the wire, so before interpreting the
570 : // buffer as a particular frame each case checks that we actually
571 : // received enough bytes for it. A short or corrupt frame would
572 : // otherwise be parsed from stale buffer contents.
573 0 : switch (header->command) {
574 0 : case SMARTAUDIO_RSP_GET_SETTINGS_V1:
575 0 : if (buffer_length < sizeof(SettingsResponseFrame)) {
576 0 : return false;
577 : }
578 0 : _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v1;
579 0 : unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
580 0 : settings.version = SMARTAUDIO_SPEC_PROTOCOL_v1;
581 0 : print_settings(&settings);
582 0 : update_vtx_settings(settings);
583 0 : break;
584 :
585 0 : case SMARTAUDIO_RSP_GET_SETTINGS_V2:
586 0 : if (buffer_length < sizeof(SettingsResponseFrame)) {
587 0 : return false;
588 : }
589 0 : _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v2;
590 0 : unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
591 0 : settings.version = SMARTAUDIO_SPEC_PROTOCOL_v2;
592 0 : print_settings(&settings);
593 0 : update_vtx_settings(settings);
594 0 : break;
595 :
596 0 : case SMARTAUDIO_RSP_GET_SETTINGS_V21:
597 0 : if (buffer_length < sizeof(SettingsExtendedResponseFrame)) {
598 0 : return false;
599 : }
600 0 : _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v21;
601 0 : unpack_settings(&settings, (const SettingsExtendedResponseFrame *)buffer);
602 0 : settings.version = SMARTAUDIO_SPEC_PROTOCOL_v21;
603 0 : print_settings(&settings);
604 0 : update_vtx_settings(settings);
605 0 : break;
606 :
607 0 : case SMARTAUDIO_RSP_SET_FREQUENCY: {
608 0 : if (buffer_length < sizeof(U16ResponseFrame)) {
609 0 : return false;
610 : }
611 0 : const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
612 0 : unpack_frequency(&settings, resp->payload);
613 0 : vtx.set_frequency_mhz(settings.frequency);
614 0 : vtx.set_configured_frequency_mhz(vtx.get_frequency_mhz());
615 0 : vtx.update_configured_channel_and_band();
616 : debug("Frequency was set to %d", settings.frequency);
617 : }
618 0 : break;
619 :
620 0 : case SMARTAUDIO_RSP_SET_CHANNEL: {
621 0 : if (buffer_length < sizeof(U8ResponseFrame)) {
622 0 : return false;
623 : }
624 0 : const U8ResponseFrame *resp = (const U8ResponseFrame *)buffer;
625 0 : vtx.set_band(resp->payload / VTX_MAX_CHANNELS);
626 0 : vtx.set_channel(resp->payload % VTX_MAX_CHANNELS);
627 0 : vtx.set_configured_channel(vtx.get_channel());
628 0 : vtx.set_configured_band(vtx.get_band());
629 0 : vtx.update_configured_frequency();
630 : debug("Channel was set to %d", resp->payload);
631 : }
632 0 : break;
633 :
634 0 : case SMARTAUDIO_RSP_SET_POWER: {
635 0 : if (buffer_length < sizeof(U16ResponseFrame)) {
636 0 : return false;
637 : }
638 0 : const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
639 0 : const uint8_t power = resp->payload & 0xFF;
640 0 : switch (_protocol_version) {
641 0 : case SMARTAUDIO_SPEC_PROTOCOL_v21:
642 0 : if (vtx.get_configured_power_dbm() != power) {
643 0 : vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
644 : }
645 0 : vtx.set_power_dbm(power);
646 0 : vtx.set_configured_power_mw(vtx.get_power_mw());
647 0 : break;
648 0 : case SMARTAUDIO_SPEC_PROTOCOL_v2:
649 0 : if (vtx.get_configured_power_level() != power) {
650 0 : vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
651 : }
652 0 : vtx.set_power_level(power);
653 0 : vtx.set_configured_power_mw(vtx.get_power_mw());
654 0 : break;
655 0 : default:
656 0 : if (vtx.get_configured_power_dac() != power) {
657 0 : vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
658 : }
659 0 : vtx.set_power_dac(power);
660 0 : vtx.set_configured_power_mw(vtx.get_power_mw());
661 0 : break;
662 : }
663 : debug("Power was set to %d", power);
664 : }
665 0 : break;
666 :
667 0 : case SMARTAUDIO_RSP_SET_MODE: {
668 0 : if (buffer_length < sizeof(FrameHeader) + 1) {
669 0 : return false;
670 : }
671 0 : vtx.set_options(vtx.get_configured_options()); // easiest to just make them match
672 : debug("Mode was set to 0x%x", buffer[4]);
673 : }
674 0 : break;
675 :
676 0 : default:
677 0 : return false;
678 : }
679 0 : return true;
680 : }
681 :
682 : // we missed a response too many times - update the baud rate in case the temperature has increased
683 0 : void AP_SmartAudio::update_baud_rate()
684 : {
685 : // on my Unify Pro32 the VTX will respond immediately on power up to a settings request, so 10 packets is easily more than enough
686 : // we want to bias autobaud to only frequency hop when the current frequency is clearly exhausted, but after that hop quickly
687 0 : if (_packets_sent - _packets_rcvd < 10) {
688 0 : return;
689 : }
690 :
691 0 : if ((_smartbaud_direction == 1) && (_smartbaud == AP_SMARTAUDIO_SMARTBAUD_MAX)) {
692 0 : _smartbaud_direction = -1;
693 0 : } else if ((_smartbaud_direction == -1 && _smartbaud == AP_SMARTAUDIO_SMARTBAUD_MIN)) {
694 0 : _smartbaud_direction = 1;
695 : }
696 :
697 0 : _smartbaud += AP_SMARTAUDIO_SMARTBAUD_STEP * _smartbaud_direction;
698 :
699 : debug("autobaud: %d", int(_smartbaud));
700 :
701 0 : _port->begin(_smartbaud);
702 : }
703 :
704 : #endif // AP_SMARTAUDIO_ENABLED
|
