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 : Code by Andy Piper, ported from betaflight vtx_tramp
16 : */
17 :
18 : #include "AP_Tramp.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_TRAMP_ENABLED
25 :
26 : #define AP_TRAMP_UART_BAUD 9600
27 : // request and response size is 16 bytes
28 : #define AP_TRAMP_UART_BUFSIZE_RX 32
29 : #define AP_TRAMP_UART_BUFSIZE_TX 32
30 :
31 : // Define periods between requests
32 : #define TRAMP_MIN_REQUEST_PERIOD_US (200 * 1000) // 200ms
33 : #define TRAMP_STATUS_REQUEST_PERIOD_US (1000 * 1000) // 1s
34 :
35 : //#define TRAMP_DEBUG
36 : #ifdef TRAMP_DEBUG
37 : # define debug(fmt, args...) do { hal.console->printf("TRAMP: " fmt "\n", ##args); } while (0)
38 : #else
39 : # define debug(fmt, args...) do {} while(0)
40 : #endif
41 :
42 : extern const AP_HAL::HAL &hal;
43 :
44 2098 : AP_Tramp::AP_Tramp()
45 : {
46 2098 : singleton = this;
47 2098 : }
48 :
49 : AP_Tramp *AP_Tramp::singleton;
50 :
51 : // Calculate tramp protocol checksum of provided buffer
52 0 : uint8_t AP_Tramp::checksum(uint8_t *buf)
53 : {
54 0 : uint8_t cksum = 0;
55 :
56 0 : for (int i = 1 ; i < TRAMP_BUF_SIZE - 2; i++) {
57 0 : cksum += buf[i];
58 : }
59 :
60 0 : return cksum;
61 : }
62 :
63 : // Send tramp protocol frame to device
64 0 : void AP_Tramp::send_command(uint8_t cmd, uint16_t param)
65 : {
66 0 : if (port == nullptr) {
67 0 : return;
68 : }
69 :
70 0 : memset(request_buffer, 0, ARRAY_SIZE(request_buffer));
71 0 : request_buffer[0] = 0x0F;
72 0 : request_buffer[1] = cmd;
73 0 : request_buffer[2] = param & 0xFF;
74 0 : request_buffer[3] = (param >> 8) & 0xFF;
75 0 : request_buffer[14] = checksum(request_buffer);
76 :
77 0 : port->write(request_buffer, TRAMP_BUF_SIZE);
78 0 : port->flush();
79 :
80 0 : _packets_sent++;
81 :
82 : debug("send command '%c': %u", cmd, param);
83 : }
84 :
85 : // Process response and return code if valid else 0
86 0 : char AP_Tramp::handle_response(void)
87 : {
88 0 : _packets_rcvd++;
89 0 : _packets_sent = _packets_rcvd;
90 :
91 0 : const uint8_t respCode = response_buffer[1];
92 :
93 0 : switch (respCode) {
94 0 : case 'r': {
95 0 : const uint16_t min_freq = response_buffer[2]|(response_buffer[3] << 8);
96 : // Check we're not reading the request (indicated by freq zero)
97 0 : if (min_freq != 0) {
98 : // Got response, update device limits
99 0 : device_limits.rf_freq_min = min_freq;
100 0 : device_limits.rf_freq_max = response_buffer[4]|(response_buffer[5] << 8);
101 0 : device_limits.rf_power_max = response_buffer[6]|(response_buffer[7] << 8);
102 : debug("device limits: min freq: %u, max freq: %u, max power %u",
103 : unsigned(device_limits.rf_freq_min), unsigned(device_limits.rf_freq_max), unsigned(device_limits.rf_power_max));
104 0 : return 'r';
105 : }
106 0 : break;
107 : }
108 0 : case 'v': {
109 0 : const uint16_t freq = response_buffer[2]|(response_buffer[3] << 8);
110 : // Check we're not reading the request (indicated by freq zero)
111 0 : if (freq != 0) {
112 : // Got response, update device status
113 0 : const uint16_t power = response_buffer[4]|(response_buffer[5] << 8);
114 0 : cur_control_mode = response_buffer[6]; // Currently only used for race lock
115 0 : const bool pit_mode = response_buffer[7];
116 0 : cur_act_power = response_buffer[8]|(response_buffer[9] << 8);
117 :
118 : // update the vtx
119 0 : AP_VideoTX& vtx = AP::vtx();
120 0 : bool update_pending = vtx.have_params_changed();
121 0 : vtx.set_frequency_mhz(freq);
122 :
123 : AP_VideoTX::VideoBand band;
124 : uint8_t channel;
125 0 : if (vtx.get_band_and_channel(freq, band, channel)) {
126 0 : vtx.set_band(band);
127 0 : vtx.set_channel(channel);
128 : }
129 :
130 0 : vtx.set_power_mw(power);
131 0 : vtx.set_actual_power_mw(cur_act_power);
132 :
133 : // VTX-reported rf_power_max is unreliable, so warn on the user cap instead
134 0 : if (!_act_power_warned && cur_act_power != 0) {
135 0 : const uint16_t user_cap = vtx.get_max_power_mw();
136 0 : const bool over_cap = (user_cap != 0 && cur_act_power > user_cap);
137 0 : const bool over_request = (power != 0 && cur_act_power > power + (power / 2));
138 0 : if (over_cap || over_request) {
139 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
140 : "VTX: requested %umW, actual %umW, cap %umW",
141 : unsigned(power), unsigned(cur_act_power), unsigned(user_cap));
142 0 : _act_power_warned = true;
143 : }
144 : }
145 :
146 0 : if (pit_mode) {
147 0 : vtx.set_options(vtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
148 : } else {
149 0 : vtx.set_options(vtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
150 : }
151 :
152 : // make sure the configured values now reflect reality
153 : // if they do then announce if there were changes
154 0 : if (!vtx.set_defaults() && update_pending && !vtx.have_params_changed()) {
155 0 : vtx.announce_vtx_settings();
156 : }
157 :
158 : debug("device config: freq: %u, cfg pwr: %umw, act pwr: %umw, pitmode: %u, ctrl: 0x%02x",
159 : unsigned(freq), unsigned(power), unsigned(cur_act_power),
160 : unsigned(pit_mode), unsigned(cur_control_mode));
161 :
162 : // update the "_configuration_finished" flag, otherwise OSD item VTX_POWER blinks forever
163 0 : vtx.set_configuration_finished(!update_pending);
164 :
165 0 : return 'v';
166 : }
167 0 : break;
168 : }
169 0 : case 's': {
170 0 : const uint16_t temp = (int16_t)(response_buffer[6]|(response_buffer[7] << 8));
171 : // Check we're not reading the request (indicated by temp zero)
172 0 : if (temp != 0) {
173 : // Got response, update device status
174 0 : cur_temp = temp;
175 0 : return 's';
176 : }
177 0 : break;
178 : }
179 : }
180 :
181 : // Likely reading a request, return zero to indicate not accepted
182 0 : return 0;
183 : }
184 :
185 : // Reset receiver state machine
186 0 : void AP_Tramp::reset_receiver(void)
187 : {
188 0 : port->discard_input();
189 0 : receive_state = ReceiveState::S_WAIT_LEN;
190 0 : receive_pos = 0;
191 0 : }
192 :
193 : // returns completed response code or 0
194 0 : char AP_Tramp::receive_response()
195 : {
196 0 : if (port == nullptr) {
197 0 : return 0;
198 : }
199 :
200 : // wait for complete packet
201 0 : const uint16_t bytesNeeded = TRAMP_BUF_SIZE - receive_pos;
202 0 : if (port->available() < bytesNeeded) {
203 0 : return 0;
204 : }
205 :
206 : // sanity check
207 0 : if (bytesNeeded == 0) {
208 0 : reset_receiver();
209 0 : return 0;
210 : }
211 :
212 0 : for (uint16_t i = 0; i < bytesNeeded; i++) {
213 0 : const int16_t b = port->read();
214 0 : if (b < 0) {
215 : // uart claimed bytes available, but there were none
216 0 : return 0;
217 : }
218 0 : const uint8_t c = uint8_t(b);
219 0 : response_buffer[receive_pos++] = c;
220 :
221 0 : switch (receive_state) {
222 0 : case ReceiveState::S_WAIT_LEN: {
223 0 : if (c == 0x0F) {
224 : // Found header byte, advance to wait for code
225 0 : receive_state = ReceiveState::S_WAIT_CODE;
226 : } else {
227 : // Unexpected header, reset state machine
228 0 : reset_receiver();
229 : }
230 0 : break;
231 : }
232 0 : case ReceiveState::S_WAIT_CODE: {
233 0 : if (c == 'r' || c == 'v' || c == 's') {
234 : // Code is for response is one we're interested in, advance to data
235 0 : receive_state = ReceiveState::S_DATA;
236 : } else {
237 : // Unexpected code, reset state machine
238 0 : reset_receiver();
239 : }
240 0 : break;
241 : }
242 0 : case ReceiveState::S_DATA: {
243 0 : if (receive_pos == TRAMP_BUF_SIZE) {
244 : // Buffer is full, calculate checksum
245 0 : const uint8_t cksum = checksum(response_buffer);
246 :
247 : // Reset state machine ready for next response
248 0 : reset_receiver();
249 :
250 0 : if ((response_buffer[TRAMP_BUF_SIZE-2] == cksum) && (response_buffer[TRAMP_BUF_SIZE-1] == 0)) {
251 : // Checksum is correct, process response
252 0 : const char r = handle_response();
253 :
254 : // Check response valid else keep on reading
255 0 : if (r != 0) {
256 0 : return r;
257 : }
258 : }
259 : }
260 0 : break;
261 : }
262 0 : default:
263 : // Invalid state, reset state machine
264 0 : reset_receiver();
265 0 : break;
266 : }
267 : }
268 :
269 0 : return 0;
270 : }
271 :
272 0 : void AP_Tramp::send_query(uint8_t cmd)
273 : {
274 : // Reset receive buffer and issue command
275 0 : reset_receiver();
276 0 : send_command(cmd, 0);
277 0 : }
278 :
279 0 : void AP_Tramp::set_status(TrampStatus _status)
280 : {
281 0 : status = _status;
282 : #ifdef TRAMP_DEBUG
283 : switch (status) {
284 : case TrampStatus::TRAMP_STATUS_OFFLINE:
285 : debug("status: OFFLINE");
286 : break;
287 : case TrampStatus::TRAMP_STATUS_INIT:
288 : debug("status: INIT");
289 : break;
290 : case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT:
291 : debug("status: ONLINE_MONITOR_FREQPWRPIT");
292 : break;
293 : case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP:
294 : debug("status: ONLINE_MONITOR_TEMP");
295 : break;
296 : case TrampStatus::TRAMP_STATUS_ONLINE_CONFIG:
297 : debug("status: ONLINE_CONFIG");
298 : break;
299 : }
300 : #endif
301 0 : }
302 :
303 0 : void AP_Tramp::process_requests()
304 : {
305 0 : if (port == nullptr) {
306 0 : return;
307 : }
308 :
309 0 : bool configUpdateRequired = false;
310 :
311 : // Read response from device
312 0 : const char replyCode = receive_response();
313 0 : const uint32_t now = AP_HAL::micros();
314 :
315 : #ifdef TRAMP_DEBUG
316 : if (replyCode != 0) {
317 : debug("receive response '%c'", replyCode);
318 : }
319 : #endif
320 :
321 : // Act on state
322 0 : switch (status) {
323 0 : case TrampStatus::TRAMP_STATUS_OFFLINE: {
324 : // Offline, check for response
325 0 : if (replyCode == 'r') {
326 : // Device replied to reset? request, enter init
327 0 : set_status(TrampStatus::TRAMP_STATUS_INIT);
328 0 : } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
329 : // Min request period exceeded, issue another reset?
330 0 : send_query('r');
331 :
332 : // Update last time
333 0 : last_time_us = now;
334 : }
335 0 : break;
336 : }
337 0 : case TrampStatus::TRAMP_STATUS_INIT: {
338 : // Initializing, check for response
339 0 : if (replyCode == 'v') {
340 : // Device replied to freq / power / pit query, enter online
341 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
342 0 : } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
343 :
344 0 : update_baud_rate();
345 :
346 : // Min request period exceeded, issue another query
347 0 : send_query('v');
348 :
349 : // Update last time
350 0 : last_time_us = now;
351 : }
352 0 : break;
353 : }
354 0 : case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT: {
355 : // Note after config a status update request is made, a new status
356 : // request is made, this request is handled above and should prevent
357 : // subsequent config updates if the config is now correct
358 0 : if (retry_count > 0 && ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US)) {
359 0 : AP_VideoTX& vtx = AP::vtx();
360 : // Config retries remain and min request period exceeded, check freq
361 0 : if (!is_race_lock_enabled() && vtx.update_frequency()) {
362 : debug("Updating frequency to %uMhz", vtx.get_configured_frequency_mhz());
363 : // Freq can be and needs to be updated, issue request
364 0 : send_command('F', vtx.get_configured_frequency_mhz());
365 :
366 : // Set flag
367 0 : configUpdateRequired = true;
368 0 : } else if (!is_race_lock_enabled() && vtx.update_power()) {
369 : debug("Updating power to %umw\n", vtx.get_configured_power_mw());
370 : // Power can be and needs to be updated, issue request
371 0 : send_command('P', vtx.get_configured_power_mw());
372 :
373 : // Set flag
374 0 : configUpdateRequired = true;
375 0 : } else if (vtx.update_options()) {
376 : // Pit mode needs to be updated, issue request
377 0 : send_command('I', vtx.has_option(AP_VideoTX::VideoOptions::VTX_PITMODE) ? 0 : 1);
378 :
379 : // Set flag
380 0 : configUpdateRequired = true;
381 : }
382 :
383 0 : if (configUpdateRequired) {
384 : // Update required, decrement retry count
385 0 : retry_count--;
386 :
387 : // Update last time
388 0 : last_time_us = now;
389 :
390 : // Advance state
391 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_CONFIG);
392 : } else {
393 : // No update required, reset retry count
394 0 : retry_count = 0;
395 : }
396 : }
397 :
398 : /* Was a config update made? */
399 0 : if (!configUpdateRequired) {
400 : /* No, look to continue monitoring */
401 0 : if ((now - last_time_us) >= TRAMP_STATUS_REQUEST_PERIOD_US) {
402 : // Request period exceeded, issue freq/power/pit query
403 0 : send_query('v');
404 :
405 : // Update last time
406 0 : last_time_us = now;
407 0 : } else if (replyCode == 'v') {
408 : // Got reply, issue temp query
409 0 : send_query('s');
410 :
411 : // Wait for reply
412 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP);
413 :
414 : // Update last time
415 0 : last_time_us = now;
416 : }
417 : }
418 :
419 0 : break;
420 : }
421 0 : case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP: {
422 : // Check request time
423 0 : if (replyCode == 's') {
424 : // Got reply, return to request freq/power/pit
425 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP);
426 0 : } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
427 : // Timed out after min request period, return to request freq/power/pit query
428 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
429 : }
430 0 : break;
431 : }
432 0 : case TrampStatus::TRAMP_STATUS_ONLINE_CONFIG: {
433 : // Param should now be set, check time
434 0 : if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
435 : // Min request period exceeded, re-query
436 0 : send_query('v');
437 :
438 : // Advance state
439 0 : set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
440 :
441 : // Update last time
442 0 : last_time_us = now;
443 : }
444 0 : break;
445 : }
446 0 : default:
447 : // Invalid state, reset
448 0 : set_status(TrampStatus::TRAMP_STATUS_OFFLINE);
449 0 : break;
450 : }
451 : }
452 :
453 0 : bool AP_Tramp::is_device_ready()
454 : {
455 0 : return status >= TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT;
456 : }
457 :
458 0 : void AP_Tramp::set_frequency(uint16_t freq)
459 : {
460 : uint8_t freqValid;
461 :
462 : // Check frequency valid
463 0 : if (device_limits.rf_freq_min != 0 && device_limits.rf_freq_max != 0) {
464 0 : freqValid = (freq >= device_limits.rf_freq_min && freq <= device_limits.rf_freq_max);
465 : } else {
466 0 : freqValid = (freq >= VTX_TRAMP_MIN_FREQUENCY_MHZ && freq <= VTX_TRAMP_MAX_FREQUENCY_MHZ);
467 : }
468 :
469 : // Is frequency valid?
470 0 : if (freqValid) {
471 : // Requested freq changed, reset retry count
472 0 : retry_count = VTX_TRAMP_MAX_RETRIES;
473 : } else {
474 : debug("requested frequency %u is invalid", freq);
475 : // not valid reset to default
476 0 : AP::vtx().set_configured_frequency_mhz(AP::vtx().get_frequency_mhz());
477 : }
478 0 : }
479 :
480 7525129 : void AP_Tramp::update()
481 : {
482 7525129 : if (port == nullptr) {
483 7525129 : return;
484 : }
485 :
486 0 : AP_VideoTX& vtx = AP::vtx();
487 :
488 0 : if (vtx.have_params_changed() && retry_count == 0) {
489 : // check changes in the order they will be processed; re-arm retries
490 : // only on real changes so a VTX rejecting a value can't loop forever
491 0 : if (vtx.update_frequency() || vtx.update_band() || vtx.update_channel()) {
492 0 : if (vtx.update_frequency()) {
493 0 : vtx.update_configured_channel_and_band();
494 : } else {
495 0 : vtx.update_configured_frequency();
496 : }
497 0 : const uint16_t conf_freq = vtx.get_configured_frequency_mhz();
498 0 : if (conf_freq != _last_conf_freq) {
499 0 : _last_conf_freq = conf_freq;
500 0 : set_frequency(conf_freq);
501 : }
502 : }
503 0 : else if (vtx.update_power()) {
504 0 : const uint16_t conf_power = vtx.get_configured_power_mw();
505 0 : if (conf_power != _last_conf_power) {
506 0 : _last_conf_power = conf_power;
507 0 : retry_count = VTX_TRAMP_MAX_RETRIES;
508 0 : _power_warn_pending = true;
509 0 : } else if (_power_warn_pending) {
510 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
511 : "VTX: rejected power %umW", unsigned(conf_power));
512 0 : _power_warn_pending = false;
513 : }
514 : }
515 0 : else if (vtx.update_options()) {
516 0 : const uint16_t conf_options = vtx.get_configured_options();
517 0 : if (conf_options != _last_conf_options) {
518 0 : _last_conf_options = conf_options;
519 0 : retry_count = VTX_TRAMP_MAX_RETRIES;
520 : }
521 : }
522 : }
523 :
524 0 : process_requests();
525 : }
526 :
527 : // we missed a response too many times - update the baud rate in case the temperature has increased
528 0 : void AP_Tramp::update_baud_rate()
529 : {
530 : // on my Unify Pro32 the VTX will respond immediately on power up to a settings request, so 5 packets is easily more than enough
531 : // we want to bias autobaud to only frequency hop when the current frequency is clearly exhausted, but after that hop quickly
532 : // on a Foxeer Reaper Infinity the actual response baudrate is more like 9400, so auto-bauding down in the first instance.
533 0 : if (_packets_sent - _packets_rcvd < 5) {
534 0 : return;
535 : }
536 :
537 0 : if (_smartbaud_direction == AutobaudDirection::UP
538 0 : && _smartbaud == VTX_TRAMP_SMARTBAUD_MAX) {
539 0 : _smartbaud_direction = AutobaudDirection::DOWN;
540 0 : } else if (_smartbaud_direction == AutobaudDirection::DOWN
541 0 : && _smartbaud == VTX_TRAMP_SMARTBAUD_MIN) {
542 0 : _smartbaud_direction = AutobaudDirection::UP;
543 : }
544 :
545 0 : _smartbaud += VTX_TRAMP_SMARTBAUD_STEP * int32_t(_smartbaud_direction);
546 :
547 : debug("autobaud: %d", int(_smartbaud));
548 :
549 0 : port->discard_input();
550 0 : port->begin(_smartbaud);
551 :
552 0 : _packets_sent = _packets_rcvd = 0;
553 : }
554 :
555 :
556 2086 : bool AP_Tramp::init(void)
557 : {
558 2086 : if (AP::vtx().get_enabled() == 0) {
559 : debug("protocol is not active");
560 2082 : return false;
561 : }
562 :
563 : // init uart
564 4 : port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Tramp, 0);
565 4 : if (port != nullptr) {
566 0 : port->configure_parity(0);
567 0 : port->set_stop_bits(1);
568 0 : port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
569 0 : port->set_options((port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV));
570 :
571 0 : port->begin(AP_TRAMP_UART_BAUD, AP_TRAMP_UART_BUFSIZE_RX, AP_TRAMP_UART_BUFSIZE_TX);
572 : debug("port opened");
573 :
574 0 : AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::Tramp);
575 :
576 0 : return true;
577 : }
578 4 : return false;
579 : }
580 :
581 : #endif // AP_TRAMP_ENABLED
|
