Line data Source code
1 : #include "AP_Frsky_SPort.h"
2 :
3 : #if AP_FRSKY_SPORT_TELEM_ENABLED
4 :
5 : #include <AP_HAL/utility/sparse-endian.h>
6 : #include <AP_BattMonitor/AP_BattMonitor.h>
7 : #include <AP_GPS/AP_GPS.h>
8 : #include <GCS_MAVLink/GCS.h>
9 : #include <AP_RPM/AP_RPM.h>
10 :
11 : #include "AP_Frsky_SPortParser.h"
12 :
13 : #include <string.h>
14 :
15 : extern const AP_HAL::HAL& hal;
16 :
17 : AP_Frsky_SPort *AP_Frsky_SPort::singleton;
18 :
19 : /*
20 : * send telemetry data
21 : * for FrSky SPort protocol (X-receivers)
22 : */
23 6666 : void AP_Frsky_SPort::send(void)
24 : {
25 6666 : const uint16_t numc = MIN(_port->available(), 1024U);
26 :
27 : // this is the constant for hub data frame
28 6666 : if (_port->txspace() < 19) {
29 3834 : return;
30 : }
31 :
32 2832 : if (numc == 0) {
33 : // no serial data to process do bg tasks
34 2783 : if (_SPort.vario_refresh) {
35 5 : calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
36 5 : _SPort.vario_refresh = false;
37 : }
38 2783 : if (_SPort.gps_refresh) {
39 2 : calc_gps_position(); // gps data is not recalculated until all of it has been sent
40 2 : _SPort.gps_refresh = false;
41 : }
42 2783 : return;
43 : }
44 :
45 147 : for (int16_t i = 0; i < numc; i++) {
46 : uint8_t readbyte;
47 98 : if (!_port->read(readbyte)) {
48 0 : break;
49 : }
50 98 : if (_SPort.sport_status == false) {
51 49 : if (readbyte == FRAME_HEAD) {
52 49 : _SPort.sport_status = true;
53 : }
54 : } else {
55 49 : const AP_BattMonitor &_battery = AP::battery();
56 49 : switch (readbyte) {
57 10 : case SENSOR_ID_VARIO: // Sensor ID 0
58 10 : switch (_SPort.vario_call) {
59 5 : case 0:
60 5 : send_sport_frame(SPORT_DATA_FRAME, ALT_ID, _SPort_data.alt_nav_meters*100 + _SPort_data.alt_nav_cm); // send altitude in cm
61 5 : break;
62 5 : case 1:
63 5 : send_sport_frame(SPORT_DATA_FRAME, VARIO_ID, _SPort_data.vario_vspd); // send vspeed cm/s
64 5 : _SPort.vario_refresh = true;
65 5 : break;
66 : }
67 10 : if (++_SPort.vario_call > 1) {
68 5 : _SPort.vario_call = 0;
69 : }
70 10 : break;
71 10 : case SENSOR_ID_FAS: // Sensor ID 2
72 10 : switch (_SPort.fas_call) {
73 4 : case 0:
74 : {
75 4 : uint8_t percentage = 0;
76 4 : IGNORE_RETURN(_battery.capacity_remaining_pct(percentage));
77 4 : send_sport_frame(SPORT_DATA_FRAME, FUEL_ID, (uint16_t)roundf(percentage)); // send battery remaining
78 4 : break;
79 : }
80 3 : case 1:
81 3 : send_sport_frame(SPORT_DATA_FRAME, VFAS_ID, (uint16_t)roundf(_battery.voltage() * 100.0f)); // send battery voltage in cV
82 3 : break;
83 3 : case 2: {
84 : float current;
85 3 : if (!_battery.current_amps(current)) {
86 0 : current = 0;
87 : }
88 3 : send_sport_frame(SPORT_DATA_FRAME, CURR_ID, (uint16_t)roundf(current * 10.0f)); // send current consumption in dA
89 3 : break;
90 : }
91 : break;
92 : }
93 10 : if (++_SPort.fas_call > 2) {
94 3 : _SPort.fas_call = 0;
95 : }
96 10 : break;
97 10 : case SENSOR_ID_GPS: // Sensor ID 3
98 10 : switch (_SPort.gps_call) {
99 2 : case 0:
100 2 : send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude
101 2 : break;
102 2 : case 1:
103 2 : send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude
104 2 : break;
105 2 : case 2:
106 2 : send_sport_frame(SPORT_DATA_FRAME, GPS_SPEED_ID, _SPort_data.speed_in_meter*1000 + _SPort_data.speed_in_centimeter*10); // send gps speed in mm/sec
107 2 : break;
108 2 : case 3:
109 2 : send_sport_frame(SPORT_DATA_FRAME, GPS_ALT_ID, _SPort_data.alt_gps_meters*100+_SPort_data.alt_gps_cm); // send gps altitude in cm
110 2 : break;
111 2 : case 4:
112 2 : send_sport_frame(SPORT_DATA_FRAME, GPS_COURS_ID, _SPort_data.yaw*100); // send heading in cd based on AHRS and not GPS
113 2 : _SPort.gps_refresh = true;
114 2 : break;
115 : }
116 10 : if (++_SPort.gps_call > 4) {
117 2 : _SPort.gps_call = 0;
118 : }
119 10 : break;
120 10 : case SENSOR_ID_RPM: // Sensor ID 4
121 : #if AP_RPM_ENABLED
122 : {
123 10 : const AP_RPM* rpm = AP::rpm();
124 10 : if (rpm == nullptr) {
125 0 : break;
126 : }
127 : int32_t value;
128 10 : if (calc_rpm(_SPort.rpm_call, value)) {
129 : // use high numbered frsky sensor ids to leave low numbered free for externally attached physical frsky sensors
130 10 : uint16_t id = RPM1_ID;
131 10 : if (_SPort.rpm_call != 0) {
132 : // only two sensors are currently supported
133 0 : id = RPM2_ID;
134 : }
135 10 : send_sport_frame(SPORT_DATA_FRAME, id, value);
136 : }
137 10 : if (++_SPort.rpm_call > MIN(rpm->num_sensors()-1, 1)) {
138 10 : _SPort.rpm_call = 0;
139 : }
140 : }
141 : #endif // AP_RPM_ENABLED
142 10 : break;
143 9 : case SENSOR_ID_SP2UR: // Sensor ID 6
144 9 : switch (_SPort.various_call) {
145 5 : case 0 :
146 5 : send_sport_frame(SPORT_DATA_FRAME, TEMP2_ID, (uint16_t)(AP::gps().num_sats() * 10 + (uint8_t)AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
147 5 : break;
148 4 : case 1:
149 4 : send_sport_frame(SPORT_DATA_FRAME, TEMP1_ID, gcs().custom_mode()); // send flight mode
150 4 : break;
151 : }
152 9 : if (++_SPort.various_call > 1) {
153 4 : _SPort.various_call = 0;
154 : }
155 9 : break;
156 0 : default:
157 : {
158 : // respond to custom user data polling
159 0 : WITH_SEMAPHORE(_sport_push_buffer.sem);
160 0 : if (_sport_push_buffer.pending && readbyte == _sport_push_buffer.packet.sensor) {
161 0 : send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
162 0 : _sport_push_buffer.pending = false;
163 : }
164 0 : }
165 0 : break;
166 : }
167 49 : _SPort.sport_status = false;
168 : }
169 : }
170 : }
171 :
172 : /*
173 : * prepare gps latitude/longitude data
174 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
175 : */
176 48 : uint32_t AP_Frsky_SPort::calc_gps_latlng(bool &send_latitude)
177 : {
178 48 : const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
179 :
180 : // alternate between latitude and longitude
181 48 : if (send_latitude == true) {
182 24 : send_latitude = false;
183 24 : if (loc.lat < 0) {
184 23 : return ((labs(loc.lat)/100)*6) | 0x40000000;
185 : } else {
186 1 : return ((labs(loc.lat)/100)*6);
187 : }
188 : } else {
189 24 : send_latitude = true;
190 24 : if (loc.lng < 0) {
191 0 : return ((labs(loc.lng)/100)*6) | 0xC0000000;
192 : } else {
193 24 : return ((labs(loc.lng)/100)*6) | 0x80000000;
194 : }
195 : }
196 : }
197 :
198 : /*
199 : * send an 8 bytes SPort frame of FrSky data - for FrSky SPort protocol (X-receivers)
200 : */
201 378 : void AP_Frsky_SPort::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
202 : {
203 : uint8_t buf[8];
204 :
205 378 : buf[0] = frame;
206 378 : buf[1] = appid & 0xFF;
207 378 : buf[2] = appid >> 8;
208 378 : memcpy(&buf[3], &data, 4);
209 :
210 378 : uint16_t sum = 0;
211 3024 : for (uint8_t i=0; i<sizeof(buf)-1; i++) {
212 2646 : sum += buf[i];
213 2646 : sum += sum >> 8;
214 2646 : sum &= 0xFF;
215 : }
216 378 : sum = 0xff - ((sum & 0xff) + (sum >> 8));
217 378 : buf[7] = (uint8_t)sum;
218 :
219 : // perform byte stuffing per SPort spec
220 378 : uint8_t len = 0;
221 : uint8_t buf2[sizeof(buf)*2+1];
222 :
223 3402 : for (uint8_t i=0; i<sizeof(buf); i++) {
224 3024 : uint8_t c = buf[i];
225 3024 : if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
226 36 : buf2[len++] = FRAME_DLE;
227 36 : buf2[len++] = c ^ FRAME_XOR;
228 : } else {
229 2988 : buf2[len++] = c;
230 : }
231 : }
232 : #ifndef HAL_BOARD_SITL
233 : /*
234 : check that we haven't been too slow in responding to the new
235 : UART data. If we respond too late then we will overwrite the next
236 : polling frame.
237 : SPort poll-to-poll period is 11.65ms, a frame takes 1.38ms
238 : but specs require we release the bus before 8ms leaving us with 6500us
239 : */
240 : const uint64_t tend_us = port->receive_time_constraint_us(1);
241 : const uint64_t now_us = AP_HAL::micros64();
242 : const uint64_t tdelay_us = now_us - tend_us;
243 : if (tdelay_us > 6500) {
244 : // we've been too slow in responding
245 : return;
246 : }
247 : #endif
248 378 : _port->write(buf2, len);
249 378 : }
250 :
251 : extern const AP_HAL::HAL& hal;
252 :
253 19 : bool AP_Frsky_SPortParser::should_process_packet(const uint8_t *packet, bool discard_duplicates)
254 : {
255 : // check for duplicate packets
256 19 : if (discard_duplicates) {
257 : /*
258 : Note: the polling byte packet[0] should be ignored in the comparison
259 : because we might get the same packet with different polling bytes
260 : We have 2 types of duplicate packets: ghost identical packets sent by the receiver
261 : and user duplicate packets sent to compensate for bad link and frame loss, this
262 : check should address both types.
263 : */
264 19 : if (memcmp(&packet[1], &_parse_state.last_packet[1], SPORT_PACKET_SIZE-1) == 0) {
265 0 : return false;
266 : }
267 19 : memcpy(_parse_state.last_packet, packet, SPORT_PACKET_SIZE);
268 : }
269 : //check CRC
270 19 : int16_t crc = 0;
271 171 : for (uint8_t i=1; i<SPORT_PACKET_SIZE; ++i) {
272 152 : crc += _parse_state.rx_buffer[i]; // 0-1FE
273 152 : crc += crc >> 8; // 0-1FF
274 152 : crc &= 0x00ff; // 0-FF
275 : }
276 19 : return (crc == 0x00ff);
277 : }
278 :
279 878 : bool AP_Frsky_SPortParser::process_byte(AP_Frsky_SPort::sport_packet_t &sport_packet, const uint8_t data)
280 : {
281 878 : switch (_parse_state.state) {
282 22 : case ParseState::START:
283 22 : if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
284 22 : _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data;
285 : }
286 22 : _parse_state.state = ParseState::IN_FRAME;
287 22 : break;
288 :
289 834 : case ParseState::IN_FRAME:
290 834 : if (data == FRAME_DLE) {
291 0 : _parse_state.state = ParseState::XOR; // XOR next byte
292 834 : } else if (data == FRAME_HEAD) {
293 341 : _parse_state.state = ParseState::IN_FRAME ;
294 341 : _parse_state.rx_buffer_count = 0;
295 341 : break;
296 493 : } else if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
297 493 : _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data;
298 : }
299 493 : break;
300 :
301 0 : case ParseState::XOR:
302 0 : if (_parse_state.rx_buffer_count < TELEMETRY_RX_BUFFER_SIZE) {
303 0 : _parse_state.rx_buffer[_parse_state.rx_buffer_count++] = data ^ STUFF_MASK;
304 : }
305 0 : _parse_state.state = ParseState::IN_FRAME;
306 0 : break;
307 :
308 22 : case ParseState::IDLE:
309 22 : if (data == FRAME_HEAD) {
310 22 : _parse_state.rx_buffer_count = 0;
311 22 : _parse_state.state = ParseState::START;
312 : }
313 22 : break;
314 :
315 : } // switch
316 :
317 878 : if (_parse_state.rx_buffer_count >= SPORT_PACKET_SIZE) {
318 19 : _parse_state.rx_buffer_count = 0;
319 19 : _parse_state.state = ParseState::IDLE;
320 : // feed the packet only if it's not a duplicate
321 19 : return get_packet(sport_packet, true);
322 : }
323 859 : return false;
324 : }
325 :
326 19 : bool AP_Frsky_SPortParser::get_packet(AP_Frsky_SPort::sport_packet_t &sport_packet, bool discard_duplicates)
327 : {
328 19 : if (!should_process_packet(_parse_state.rx_buffer, discard_duplicates)) {
329 0 : return false;
330 : }
331 :
332 : const AP_Frsky_SPort::sport_packet_t sp {
333 19 : { _parse_state.rx_buffer[0],
334 19 : _parse_state.rx_buffer[1],
335 19 : le16toh_ptr(&_parse_state.rx_buffer[2]),
336 19 : le32toh_ptr(&_parse_state.rx_buffer[4]) },
337 19 : };
338 :
339 19 : sport_packet = sp;
340 19 : return true;
341 : }
342 :
343 : /*
344 : * Calculates the sensor id from the physical sensor index [0-27]
345 : 0x00, // Physical ID 0 - Vario2 (altimeter high precision)
346 : 0xA1, // Physical ID 1 - FLVSS Lipo sensor
347 : 0x22, // Physical ID 2 - FAS-40S current sensor
348 : 0x83, // Physical ID 3 - GPS / altimeter (normal precision)
349 : 0xE4, // Physical ID 4 - RPM
350 : 0x45, // Physical ID 5 - SP2UART(Host)
351 : 0xC6, // Physical ID 6 - SPUART(Remote)
352 : 0x67, // Physical ID 7 - Ardupilot/Betaflight EXTRA DOWNLINK
353 : 0x48, // Physical ID 8 -
354 : 0xE9, // Physical ID 9 -
355 : 0x6A, // Physical ID 10 -
356 : 0xCB, // Physical ID 11 -
357 : 0xAC, // Physical ID 12 -
358 : 0x0D, // Physical ID 13 - Ardupilot/Betaflight UPLINK
359 : 0x8E, // Physical ID 14 -
360 : 0x2F, // Physical ID 15 -
361 : 0xD0, // Physical ID 16 -
362 : 0x71, // Physical ID 17 -
363 : 0xF2, // Physical ID 18 -
364 : 0x53, // Physical ID 19 -
365 : 0x34, // Physical ID 20 - Ardupilot/Betaflight EXTRA DOWNLINK
366 : 0x95, // Physical ID 21 -
367 : 0x16, // Physical ID 22 - GAS Suite
368 : 0xB7, // Physical ID 23 - IMU ACC (x,y,z)
369 : 0x98, // Physical ID 24 -
370 : 0x39, // Physical ID 25 - Power Box
371 : 0xBA // Physical ID 26 - Temp
372 : 0x1B // Physical ID 27 - ArduPilot/Betaflight DEFAULT DOWNLINK
373 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
374 : */
375 : #undef BIT
376 : #define BIT(x, index) (((x) >> index) & 0x01)
377 12 : uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id)
378 : {
379 12 : uint8_t result = physical_id;
380 12 : result += (BIT(physical_id, 0) ^ BIT(physical_id, 1) ^ BIT(physical_id, 2)) << 5;
381 12 : result += (BIT(physical_id, 2) ^ BIT(physical_id, 3) ^ BIT(physical_id, 4)) << 6;
382 12 : result += (BIT(physical_id, 0) ^ BIT(physical_id, 2) ^ BIT(physical_id, 4)) << 7;
383 12 : return result;
384 : }
385 :
386 : /*
387 : * prepare value for transmission through FrSky link
388 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
389 : */
390 262 : uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t power)
391 : {
392 262 : uint16_t res = 0;
393 262 : uint32_t abs_number = abs(number);
394 :
395 262 : if ((digits == 2) && (power == 0)) { // number encoded on 7 bits, client side needs to know if expected range is 0,127 or -63,63
396 41 : uint8_t max_value = number < 0 ? (0x1<<6)-1 : (0x1<<7)-1;
397 41 : res = constrain_int16(abs_number,0,max_value);
398 41 : if (number < 0) { // if number is negative, add sign bit in front
399 0 : res |= 1U<<6;
400 : }
401 262 : } else if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power
402 104 : if (abs_number < 100) {
403 103 : res = abs_number<<1;
404 1 : } else if (abs_number < 1270) {
405 0 : res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1;
406 : } else { // transmit max possible value (0x7F x 10^1 = 1270)
407 1 : res = 0xFF;
408 : }
409 104 : if (number < 0) { // if number is negative, add sign bit in front
410 1 : res |= 0x1<<8;
411 : }
412 117 : } else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power
413 20 : if (abs_number < 100) {
414 1 : res = abs_number<<2;
415 19 : } else if (abs_number < 1000) {
416 0 : res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1;
417 19 : } else if (abs_number < 10000) {
418 19 : res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2;
419 0 : } else if (abs_number < 127000) {
420 0 : res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3;
421 : } else { // transmit max possible value (0x7F x 10^3 = 127000)
422 0 : res = 0x1FF;
423 : }
424 20 : if (number < 0) { // if number is negative, add sign bit in front
425 0 : res |= 0x1<<9;
426 : }
427 97 : } else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power
428 45 : if (abs_number < 1000) {
429 45 : res = abs_number<<1;
430 0 : } else if (abs_number < 10240) {
431 0 : res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1;
432 : } else { // transmit max possible value (0x3FF x 10^1 = 10230)
433 0 : res = 0x7FF;
434 : }
435 45 : if (number < 0) { // if number is negative, add sign bit in front
436 0 : res |= 0x1<<11;
437 : }
438 52 : } else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power
439 52 : if (abs_number < 1000) {
440 52 : res = abs_number<<2;
441 0 : } else if (abs_number < 10000) {
442 0 : res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1;
443 0 : } else if (abs_number < 100000) {
444 0 : res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2;
445 0 : } else if (abs_number < 1024000) {
446 0 : res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3;
447 : } else { // transmit max possible value (0x3FF x 10^3 = 1023000)
448 0 : res = 0xFFF;
449 : }
450 52 : if (number < 0) { // if number is negative, add sign bit in front
451 5 : res |= 0x1<<12;
452 : }
453 : }
454 262 : return res;
455 : }
456 :
457 : /*
458 : * Push user data down the telemetry link by responding to sensor polling (sport)
459 : * or by using dedicated slots in the scheduler (fport)
460 : * for SPort and FPort protocols (X-receivers)
461 : */
462 0 : bool AP_Frsky_SPort::sport_telemetry_push(uint8_t sensor, uint8_t frame, uint16_t appid, int32_t data)
463 : {
464 0 : WITH_SEMAPHORE(_sport_push_buffer.sem);
465 0 : if (_sport_push_buffer.pending) {
466 0 : return false;
467 : }
468 0 : _sport_push_buffer.packet.sensor = sensor;
469 0 : _sport_push_buffer.packet.frame = frame;
470 0 : _sport_push_buffer.packet.appid = appid;
471 0 : _sport_push_buffer.packet.data = static_cast<uint32_t>(data);
472 0 : _sport_push_buffer.pending = true;
473 0 : return true;
474 0 : }
475 :
476 : namespace AP {
477 0 : AP_Frsky_SPort *frsky_sport() {
478 0 : return AP_Frsky_SPort::get_singleton();
479 : }
480 : };
481 :
482 : #endif // AP_FRSKY_SPORT_TELEM_ENABLED
|
