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 : /*
16 : GHST protocol decoder based on betaflight implementation
17 : Code by Andy Piper
18 : */
19 :
20 : #include "AP_RCProtocol_config.h"
21 :
22 : #if AP_RCPROTOCOL_GHST_ENABLED
23 :
24 : #define CRSF_BAUDRATE 416666U
25 :
26 : #include "AP_RCProtocol.h"
27 : #include "AP_RCProtocol_GHST.h"
28 : #include <AP_HAL/AP_HAL.h>
29 : #include <AP_Math/AP_Math.h>
30 : #include <AP_Math/crc.h>
31 : #include <AP_Vehicle/AP_Vehicle_Type.h>
32 : #include <AP_RCTelemetry/AP_GHST_Telem.h>
33 : #include <AP_SerialManager/AP_SerialManager.h>
34 :
35 : /*
36 : * GHST protocol
37 : *
38 : * GHST protocol uses a single wire half duplex uart connection.
39 : *
40 : * 420000 baud
41 : * not inverted
42 : * 8 Bit
43 : * 1 Stop bit
44 : * Big endian
45 : * Max frame size is 14 bytes
46 : *
47 : * Every frame has the structure:
48 : * <Device address><Frame length><Type><Payload><CRC>
49 : *
50 : * Device address: (uint8_t)
51 : * Frame length: length in bytes including Type (uint8_t)
52 : * Type: (uint8_t)
53 : * CRC: (uint8_t)
54 : *
55 : */
56 :
57 : extern const AP_HAL::HAL& hal;
58 :
59 : //#define GHST_DEBUG
60 : //#define GHST_DEBUG_CHARS
61 : #ifdef GHST_DEBUG
62 : # define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args)
63 : static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0)
64 : {
65 : switch(byte) {
66 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_5TO8:
67 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_5TO8:
68 : return "RC5_8";
69 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_9TO12:
70 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_9TO12:
71 : return "RC9_12";
72 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_13TO16:
73 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_13TO16:
74 : return "RC13_16";
75 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI:
76 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_12_RSSI:
77 : return "RSSI";
78 : case AP_RCProtocol_GHST::GHST_UL_RC_VTX_CTRL:
79 : return "VTX_CTRL";
80 : case AP_RCProtocol_GHST::GHST_UL_VTX_SETUP:
81 : return "VTX_SETUP";
82 : }
83 : return "UNKNOWN";
84 : }
85 : #else
86 : # define debug(fmt, args...) do {} while(0)
87 : #endif
88 :
89 : #define GHST_MAX_FRAME_TIME_US 500U // 14 bytes @ 420k = ~450us
90 : #define GHST_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays
91 : #define GHST_INTER_FRAME_TIME_US 2000U // At fastest, frames are sent by the transmitter every 2 ms, 500 Hz
92 : #define GHST_HEADER_TYPE_LEN (GHST_HEADER_LEN + 1) // header length including type
93 :
94 : const uint16_t AP_RCProtocol_GHST::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = {
95 : 55, 160, 250, 19, 250, 500, 150, 250,
96 : };
97 :
98 : AP_RCProtocol_GHST* AP_RCProtocol_GHST::_singleton;
99 :
100 2094 : AP_RCProtocol_GHST::AP_RCProtocol_GHST(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend)
101 : {
102 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
103 2094 : if (_singleton != nullptr) {
104 0 : AP_HAL::panic("Duplicate GHST handler");
105 : }
106 :
107 2094 : _singleton = this;
108 : #else
109 0 : if (_singleton == nullptr) {
110 0 : _singleton = this;
111 : }
112 : #endif
113 2094 : }
114 :
115 908 : AP_RCProtocol_GHST::~AP_RCProtocol_GHST() {
116 454 : _singleton = nullptr;
117 908 : }
118 :
119 : // get the protocol string
120 0 : const char* AP_RCProtocol_GHST::get_protocol_string() const {
121 0 : return "GHST";
122 : }
123 :
124 : // return the link rate as defined by the LinkStatistics
125 0 : uint16_t AP_RCProtocol_GHST::get_link_rate() const {
126 0 : return RF_MODE_RATES[_link_status.rf_mode - GHST_RF_MODE_NORMAL];
127 : }
128 :
129 0 : void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte)
130 : {
131 : //debug("process_byte(0x%x)", byte);
132 : // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail,
133 : // however thread scheduling can introduce longer delays even when the data has been received
134 0 : if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > GHST_FRAME_TIMEOUT_US) {
135 0 : _frame_ofs = 0;
136 : }
137 :
138 : // overflow check
139 0 : if (_frame_ofs >= GHST_FRAMELEN_MAX) {
140 0 : _frame_ofs = 0;
141 : }
142 :
143 : // start of a new frame
144 0 : if (_frame_ofs == 0) {
145 0 : _start_frame_time_us = timestamp_us;
146 : }
147 :
148 0 : add_to_buffer(_frame_ofs++, byte);
149 :
150 : // need a header to get the length
151 0 : if (_frame_ofs < GHST_HEADER_TYPE_LEN) {
152 0 : return;
153 : }
154 :
155 0 : if (_frame.device_address != DeviceAddress::GHST_ADDRESS_FLIGHT_CONTROLLER) {
156 0 : return;
157 : }
158 :
159 : // parse the length
160 0 : if (_frame_ofs == GHST_HEADER_TYPE_LEN) {
161 0 : _frame_crc = crc8_dvb_s2(0, _frame.type);
162 : // check for garbage frame
163 0 : if (_frame.length > GHST_FRAME_PAYLOAD_MAX) {
164 0 : _frame_ofs = 0;
165 : }
166 0 : return;
167 : }
168 :
169 : // update crc
170 0 : if (_frame_ofs < _frame.length + GHST_HEADER_LEN) {
171 0 : _frame_crc = crc8_dvb_s2(_frame_crc, byte);
172 : }
173 :
174 : // overflow check
175 0 : if (_frame_ofs > _frame.length + GHST_HEADER_LEN) {
176 0 : _frame_ofs = 0;
177 0 : return;
178 : }
179 :
180 0 : if (_frame.length < 2) {
181 : // invalid length, we subtract 2 below
182 0 : _frame_ofs = 0;
183 0 : return;
184 : }
185 :
186 : // decode whatever we got and expect
187 0 : if (_frame_ofs == _frame.length + GHST_HEADER_LEN) {
188 0 : log_data(AP_RCProtocol::GHST, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - GHST_HEADER_LEN);
189 :
190 : // we consumed the partial frame, reset
191 0 : _frame_ofs = 0;
192 :
193 : // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index)
194 0 : if (_frame_crc != _frame.payload[_frame.length - 2]) {
195 0 : return;
196 : }
197 :
198 0 : _last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
199 : // decode here
200 0 : if (decode_ghost_packet()) {
201 0 : _last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter
202 0 : add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
203 : }
204 : }
205 : }
206 :
207 36177494 : void AP_RCProtocol_GHST::update(void)
208 : {
209 36177494 : }
210 :
211 : // write out a frame of any type
212 0 : bool AP_RCProtocol_GHST::write_frame(Frame* frame)
213 : {
214 0 : AP_HAL::UARTDriver *uart = get_current_UART();
215 :
216 0 : if (!uart) {
217 0 : return false;
218 : }
219 :
220 : // check that we haven't been too slow in responding to the new UART data. If we respond too late then we will
221 : // corrupt the next incoming control frame. incoming packets at max 126bits @500Hz @420k baud gives total budget of 2ms
222 : // per packet of which we need 300us to receive a packet. outgoing packets are 126bits which require 300us to send
223 : // leaving at most 1.4ms of delay that can be tolerated
224 0 : uint64_t tend = uart->receive_time_constraint_us(1);
225 0 : uint64_t now = AP_HAL::micros64();
226 0 : uint64_t tdelay = now - tend;
227 0 : if (tdelay > 1000) {
228 : // we've been too slow in responding
229 0 : return false;
230 : }
231 :
232 : // calculate crc
233 0 : uint8_t crc = crc8_dvb_s2(0, frame->type);
234 0 : for (uint8_t i = 0; i < frame->length - 2; i++) {
235 0 : crc = crc8_dvb_s2(crc, frame->payload[i]);
236 : }
237 0 : frame->payload[frame->length - 2] = crc;
238 :
239 0 : uart->write((uint8_t*)frame, frame->length + 2);
240 0 : uart->flush();
241 :
242 : #ifdef GHST_DEBUG
243 : hal.console->printf("GHST: writing %s:", get_frame_type(frame->type, frame->payload[0]));
244 : for (uint8_t i = 0; i < frame->length + 2; i++) {
245 : uint8_t val = ((uint8_t*)frame)[i];
246 : #ifdef GHST_DEBUG_CHARS
247 : if (val >= 32 && val <= 126) {
248 : hal.console->printf(" 0x%x '%c'", val, (char)val);
249 : } else {
250 : #endif
251 : hal.console->printf(" 0x%x", val);
252 : #ifdef GHST_DEBUG_CHARS
253 : }
254 : #endif
255 : }
256 : hal.console->printf("\n");
257 : #endif
258 0 : return true;
259 : }
260 :
261 0 : bool AP_RCProtocol_GHST::decode_ghost_packet()
262 : {
263 : #ifdef GHST_DEBUG
264 : hal.console->printf("GHST: received %s:", get_frame_type(_frame.type));
265 : uint8_t* fptr = (uint8_t*)&_frame;
266 : for (uint8_t i = 0; i < _frame.length + 2; i++) {
267 : #ifdef GHST_DEBUG_CHARS
268 : if (fptr[i] >= 32 && fptr[i] <= 126) {
269 : hal.console->printf(" 0x%x '%c'", fptr[i], (char)fptr[i]);
270 : } else {
271 : #endif
272 : hal.console->printf(" 0x%x", fptr[i]);
273 : #ifdef GHST_DEBUG_CHARS
274 : }
275 : #endif
276 : }
277 : hal.console->printf("\n");
278 : #endif
279 :
280 0 : const RadioFrame* radio_frame = (const RadioFrame*)(&_frame.payload);
281 0 : const Channels12Bit_4Chan* channels = &(radio_frame->channels);
282 0 : const uint8_t* lowres_channels = radio_frame->lowres_channels;
283 0 : bool rc_frame = false;
284 :
285 : // Scaling from Betaflight
286 : // Scaling 12bit channels (8bit channels in brackets)
287 : // OpenTx RC PWM (BF)
288 : // min -1024 0( 0) 988us
289 : // ctr 0 2048(128) 1500us
290 : // max 1024 4096(256) 2012us
291 : //
292 :
293 : // Scaling legacy (nearly 10bit)
294 : // derived from original SBus scaling, with slight correction for offset
295 : // now symmetrical around OpenTx 0 value
296 : // scaling is:
297 : // OpenTx RC PWM (BF)
298 : // min -1024 172( 22) 988us
299 : // ctr 0 992(124) 1500us
300 : // max 1024 1811(226) 2012us
301 :
302 : #define CHANNEL_RESCALE(x) (((5 * x) >> 2) - 430)
303 : #define CHANNEL_SCALE(x) (int32_t(x) / 4 + 988)
304 : #define CHANNEL_SCALE_LEGACY(x) CHANNEL_SCALE(CHANNEL_RESCALE(x))
305 :
306 : // legacy scaling
307 0 : if (_frame.type >= GHST_UL_RC_CHANS_HS4_5TO8 && _frame.type <= GHST_UL_RC_CHANS_RSSI) {
308 0 : _channels[0] = CHANNEL_SCALE_LEGACY(channels->ch0);
309 0 : _channels[1] = CHANNEL_SCALE_LEGACY(channels->ch1);
310 0 : _channels[2] = CHANNEL_SCALE_LEGACY(channels->ch2);
311 0 : _channels[3] = CHANNEL_SCALE_LEGACY(channels->ch3);
312 : } else {
313 0 : _channels[0] = CHANNEL_SCALE(channels->ch0);
314 0 : _channels[1] = CHANNEL_SCALE(channels->ch1);
315 0 : _channels[2] = CHANNEL_SCALE(channels->ch2);
316 0 : _channels[3] = CHANNEL_SCALE(channels->ch3);
317 : }
318 :
319 : #define CHANNEL_LR_RESCALE(x) (5 * x - 108)
320 : #define CHANNEL_LR_SCALE(x) (int32_t(x) * 2 + 988)
321 : #define CHANNEL_LR_SCALE_LEGACY(x) (CHANNEL_LR_RESCALE(x) + 988)
322 :
323 0 : switch (_frame.type) {
324 0 : case GHST_UL_RC_CHANS_HS4_5TO8:
325 : case GHST_UL_RC_CHANS_HS4_9TO12:
326 : case GHST_UL_RC_CHANS_HS4_13TO16: {
327 0 : uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_5TO8 + 1) * 4;
328 0 : _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[0]);
329 0 : _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[1]);
330 0 : _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[2]);
331 0 : _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[3]);
332 0 : rc_frame = true;
333 0 : break;
334 : }
335 0 : case GHST_UL_RC_CHANS_HS4_12_5TO8:
336 : case GHST_UL_RC_CHANS_HS4_12_9TO12:
337 : case GHST_UL_RC_CHANS_HS4_12_13TO16: {
338 0 : uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_12_5TO8 + 1) * 4;
339 0 : _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[0]);
340 0 : _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[1]);
341 0 : _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[2]);
342 0 : _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[3]);
343 0 : rc_frame = true;
344 0 : break;
345 : }
346 0 : case GHST_UL_RC_CHANS_RSSI:
347 : case GHST_UL_RC_CHANS_12_RSSI:
348 0 : process_link_stats_frame((uint8_t*)&_frame.payload);
349 0 : break;
350 :
351 0 : default:
352 0 : break;
353 : }
354 :
355 : #if AP_GHST_TELEM_ENABLED
356 0 : if (AP_GHST_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) {
357 0 : process_telemetry();
358 : }
359 : #endif
360 :
361 0 : return rc_frame;
362 : }
363 :
364 : // send out telemetry
365 0 : bool AP_RCProtocol_GHST::process_telemetry(bool check_constraint)
366 : {
367 :
368 0 : AP_HAL::UARTDriver *uart = get_current_UART();
369 0 : if (!uart) {
370 0 : return false;
371 : }
372 :
373 0 : if (!telem_available) {
374 : #if AP_GHST_TELEM_ENABLED
375 0 : if (AP_GHST_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) {
376 0 : telem_available = true;
377 : } else {
378 0 : return false;
379 : }
380 : #else
381 : return false;
382 : #endif
383 : }
384 0 : if (!write_frame(&_telemetry_frame)) {
385 0 : return false;
386 : }
387 :
388 : // get fresh telem_data in the next call
389 0 : telem_available = false;
390 :
391 0 : return true;
392 : }
393 :
394 : // process link statistics to get RSSI
395 0 : void AP_RCProtocol_GHST::process_link_stats_frame(const void* data)
396 : {
397 0 : const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;
398 :
399 : uint8_t rssi_dbm;
400 0 : rssi_dbm = link->rssi_dbm;
401 0 : _link_status.link_quality = link->link_quality;
402 0 : if (_use_lq_for_rssi) {
403 0 : _link_status.rssi = derive_scaled_lq_value(link->link_quality);
404 : } else{
405 : // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
406 0 : if (rssi_dbm < 50) {
407 0 : _link_status.rssi = 255;
408 0 : } else if (rssi_dbm > 120) {
409 0 : _link_status.rssi = 0;
410 : } else {
411 : // this is an approximation recommended by Remo from TBS
412 0 : _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
413 : }
414 : }
415 :
416 0 : _link_status.rf_mode = link->protocol;
417 0 : }
418 :
419 0 : bool AP_RCProtocol_GHST::is_telemetry_supported() const
420 : {
421 0 : return _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_NORMAL
422 0 : || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE
423 0 : || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_LR
424 0 : || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE250;
425 : }
426 :
427 : // process a byte provided by a uart
428 0 : void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate)
429 : {
430 : // reject RC data if we have been configured for standalone mode
431 0 : if (baudrate != CRSF_BAUDRATE && baudrate != GHST_BAUDRATE) {
432 0 : return;
433 : }
434 0 : _process_byte(AP_HAL::micros(), byte);
435 : }
436 :
437 : // change the bootstrap baud rate to Ghost standard if configured
438 4 : void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate)
439 : {
440 4 : AP_HAL::UARTDriver *uart = get_current_UART();
441 :
442 : // only change the baudrate if we are specifically bootstrapping Ghost
443 4 : if (uart == nullptr
444 4 : || baudrate != CRSF_BAUDRATE
445 0 : || baudrate == GHST_BAUDRATE
446 0 : || uart->get_baud_rate() == GHST_BAUDRATE
447 8 : || !protocol_enabled(AP_RCProtocol::GHST)) {
448 4 : return;
449 : }
450 : #if AP_RCPROTOCOL_CRSF_ENABLED
451 0 : if (protocol_enabled(AP_RCProtocol::CRSF)) {
452 : // don't fight CRSF
453 0 : return;
454 : }
455 : #endif
456 :
457 0 : uart->begin(GHST_BAUDRATE);
458 : }
459 :
460 : //returns uplink link quality on 0-255 scale
461 0 : int16_t AP_RCProtocol_GHST::derive_scaled_lq_value(uint8_t uplink_lq)
462 : {
463 0 : return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255)));
464 : }
465 :
466 : namespace AP {
467 0 : AP_RCProtocol_GHST* ghost() {
468 0 : return AP_RCProtocol_GHST::get_singleton();
469 : }
470 : };
471 :
472 : #endif // AP_RCPROTOCOL_GHST_ENABLED
|
