Line data Source code
1 : #include "AP_Frsky_SPort_Passthrough.h"
2 :
3 : #if AP_FRSKY_SPORT_PASSTHROUGH_ENABLED
4 :
5 : #include <AP_AHRS/AP_AHRS.h>
6 : #include <AP_BattMonitor/AP_BattMonitor.h>
7 : #include <AP_GPS/AP_GPS.h>
8 : #include <AP_HAL/utility/RingBuffer.h>
9 : #include <AP_InertialSensor/AP_InertialSensor.h>
10 : #include <AP_Notify/AP_Notify.h>
11 : #include <AP_RangeFinder/AP_RangeFinder.h>
12 : #include <AP_RPM/AP_RPM.h>
13 : #include <AP_Terrain/AP_Terrain.h>
14 : #include <AC_Fence/AC_Fence.h>
15 : #include <AP_Vehicle/AP_Vehicle.h>
16 : #include <GCS_MAVLink/GCS.h>
17 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
18 : #include <AP_WindVane/AP_WindVane.h>
19 : #endif
20 :
21 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
22 : #include "AP_Frsky_MAVlite.h"
23 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
24 : #include "AP_Frsky_Parameters.h"
25 :
26 : /*
27 : for FrSky SPort Passthrough
28 : */
29 : // data bits preparation
30 : // for parameter data
31 : #define PARAM_ID_OFFSET 24
32 : #define PARAM_VALUE_LIMIT 0xFFFFFF
33 : // for gps status data
34 : #define GPS_SATS_LIMIT 0xF
35 : #define GPS_STATUS_LIMIT 0x3
36 : #define GPS_STATUS_OFFSET 4
37 : #define GPS_HDOP_OFFSET 6
38 : #define GPS_ADVSTATUS_OFFSET 14
39 : #define GPS_ALTMSL_OFFSET 22
40 : // for battery data
41 : #define BATT_VOLTAGE_LIMIT 0x1FF
42 : #define BATT_CURRENT_OFFSET 9
43 : #define BATT_TOTALMAH_LIMIT 0x7FFF
44 : #define BATT_TOTALMAH_OFFSET 17
45 : // for autopilot status data
46 : #define AP_CONTROL_MODE_LIMIT 0x1F
47 : #define AP_SIMPLE_OFFSET 5
48 : #define AP_SSIMPLE_OFFSET 6
49 : #define AP_FLYING_OFFSET 7
50 : #define AP_ARMED_OFFSET 8
51 : #define AP_BATT_FS_OFFSET 9
52 : #define AP_EKF_FS_OFFSET 10
53 : #define AP_FS_OFFSET 12
54 : #define AP_FENCE_PRESENT_OFFSET 13
55 : #define AP_FENCE_BREACH_OFFSET 14
56 : #define AP_THROTTLE_OFFSET 19
57 : #define AP_IMU_TEMP_MIN 19.0f
58 : #define AP_IMU_TEMP_MAX 82.0f
59 : #define AP_IMU_TEMP_OFFSET 26
60 : // for home position related data
61 : #define HOME_ALT_OFFSET 12
62 : #define HOME_BEARING_LIMIT 0x7F
63 : #define HOME_BEARING_OFFSET 25
64 : // for velocity and yaw data
65 : #define VELANDYAW_XYVEL_OFFSET 9
66 : #define VELANDYAW_YAW_LIMIT 0x7FF
67 : #define VELANDYAW_YAW_OFFSET 17
68 : #define VELANDYAW_ARSPD_OFFSET 28
69 : // for attitude (roll, pitch) and range data
70 : #define ATTIANDRNG_ROLL_LIMIT 0x7FF
71 : #define ATTIANDRNG_PITCH_LIMIT 0x3FF
72 : #define ATTIANDRNG_PITCH_OFFSET 11
73 : #define ATTIANDRNG_RNGFND_OFFSET 21
74 : // for terrain data
75 : #define TERRAIN_UNHEALTHY_OFFSET 13
76 : // for wind data
77 : #define WIND_ANGLE_LIMIT 0x7F
78 : #define WIND_SPEED_OFFSET 7
79 : #define WIND_APPARENT_ANGLE_OFFSET 15
80 : #define WIND_APPARENT_SPEED_OFFSET 22
81 : // for waypoint data
82 : #define WP_NUMBER_LIMIT 2047
83 : #define WP_DISTANCE_LIMIT 1023000
84 : #define WP_DISTANCE_OFFSET 11
85 : #define WP_BEARING_OFFSET 23
86 :
87 : extern const AP_HAL::HAL& hal;
88 :
89 : AP_Frsky_SPort_Passthrough *AP_Frsky_SPort_Passthrough::singleton;
90 :
91 3 : bool AP_Frsky_SPort_Passthrough::init()
92 : {
93 3 : if (!AP_RCTelemetry::init()) {
94 0 : return false;
95 : }
96 3 : return AP_Frsky_SPort::init();
97 : }
98 :
99 3 : bool AP_Frsky_SPort_Passthrough::init_serial_port()
100 : {
101 3 : if (_use_external_data) {
102 0 : return true;
103 : }
104 3 : return AP_Frsky_SPort::init_serial_port();
105 : }
106 :
107 329 : void AP_Frsky_SPort_Passthrough::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
108 : {
109 329 : if (_use_external_data) {
110 0 : external_data.packet.frame = frame;
111 0 : external_data.packet.appid = appid;
112 0 : external_data.packet.data = data;
113 0 : external_data.pending = true;
114 0 : return;
115 : }
116 :
117 329 : return AP_Frsky_SPort::send_sport_frame(frame, appid, data);
118 : }
119 :
120 : /*
121 : setup ready for passthrough telem
122 : */
123 3 : void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
124 : {
125 : // initialize packet weights for the WFQ scheduler
126 : // priority[i] = 1/_scheduler.packet_weight[i]
127 : // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
128 3 : set_scheduler_entry(TEXT, 35, 28); // 0x5000 status text (dynamic)
129 3 : set_scheduler_entry(ATTITUDE, 50, 38); // 0x5006 Attitude and range (dynamic)
130 3 : set_scheduler_entry(GPS_LAT, 550, 280); // 0x800 GPS lat
131 3 : set_scheduler_entry(GPS_LON, 550, 280); // 0x800 GPS lon
132 3 : set_scheduler_entry(VEL_YAW, 400, 250); // 0x5005 Vel and Yaw
133 3 : set_scheduler_entry(AP_STATUS, 700, 500); // 0x5001 AP status
134 3 : set_scheduler_entry(GPS_STATUS, 700, 500); // 0x5002 GPS status
135 3 : set_scheduler_entry(HOME, 400, 500); // 0x5004 Home
136 3 : set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
137 3 : set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status
138 3 : set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
139 3 : set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2
140 3 : set_scheduler_entry(TERRAIN, 700, 500); // 0x500B terrain data
141 3 : set_scheduler_entry(WIND, 700, 500); // 0x500C wind data
142 : #if AP_MISSION_ENABLED
143 3 : set_scheduler_entry(WAYPOINT, 750, 500); // 0x500D waypoint data
144 : #endif // AP_MISSION_ENABLED
145 3 : set_scheduler_entry(UDATA, 5000, 200); // user data
146 :
147 : // initialize default sport sensor ID
148 3 : set_sensor_id(_frsky_parameters->_dnlink_id, downlink_sensor_id);
149 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
150 3 : set_scheduler_entry(MAV, 35, 25); // mavlite
151 : // initialize sport sensor IDs
152 3 : set_sensor_id(_frsky_parameters->_uplink_id, _SPort_bidir.uplink_sensor_id);
153 3 : set_sensor_id(_frsky_parameters->_dnlink1_id, _SPort_bidir.downlink1_sensor_id);
154 3 : set_sensor_id(_frsky_parameters->_dnlink2_id, _SPort_bidir.downlink2_sensor_id);
155 : // initialize sport
156 3 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::process_rx_queue, void));
157 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
158 3 : }
159 :
160 : /*
161 : dynamically change scheduler priorities based on queue sizes
162 : */
163 344 : void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
164 : {
165 : /*
166 : When queues are empty set a low priority (high weight), when queues
167 : are not empty set a higher priority (low weight) based on the following
168 : relative priority order: mavlite > status text > attitude.
169 : */
170 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
171 344 : if (!_SPort_bidir.tx_packet_queue.is_empty()) {
172 98 : _scheduler.packet_weight[MAV] = 30; // mavlite
173 98 : if (!queue_empty) {
174 98 : _scheduler.packet_weight[TEXT] = 45; // messages
175 98 : _scheduler.packet_weight[ATTITUDE] = 80; // attitude
176 : } else {
177 0 : _scheduler.packet_weight[TEXT] = 5000; // messages
178 0 : _scheduler.packet_weight[ATTITUDE] = 80; // attitude
179 : }
180 : } else {
181 246 : _scheduler.packet_weight[MAV] = 5000; // mavlite
182 246 : if (!queue_empty) {
183 246 : _scheduler.packet_weight[TEXT] = 45; // messages
184 246 : _scheduler.packet_weight[ATTITUDE] = 80; // attitude
185 : } else {
186 0 : _scheduler.packet_weight[TEXT] = 5000; // messages
187 0 : _scheduler.packet_weight[ATTITUDE] = 45; // attitude
188 : }
189 : }
190 : #else //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
191 : if (!queue_empty) {
192 : _scheduler.packet_weight[TEXT] = 45; // messages
193 : _scheduler.packet_weight[ATTITUDE] = 80; // attitude
194 : } else {
195 : _scheduler.packet_weight[TEXT] = 5000; // messages
196 : _scheduler.packet_weight[ATTITUDE] = 45; // attitude
197 : }
198 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
199 : // when using fport raise user data priority if any packets are pending
200 344 : if (_use_external_data && _sport_push_buffer.pending) {
201 0 : _scheduler.packet_weight[UDATA] = 250;
202 : } else {
203 344 : _scheduler.packet_weight[UDATA] = 5000; // user data
204 : }
205 344 : }
206 :
207 : // WFQ scheduler
208 1771 : bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
209 : {
210 1771 : bool packet_ready = false;
211 1771 : switch (idx) {
212 235 : case TEXT:
213 235 : packet_ready = !queue_empty;
214 235 : break;
215 126 : case GPS_LAT:
216 : case GPS_LON:
217 : // force gps coords to use default sensor ID, always send when used with external data
218 126 : packet_ready = _use_external_data || (_passthrough.new_byte == downlink_sensor_id);
219 126 : break;
220 71 : case AP_STATUS:
221 71 : packet_ready = gcs().vehicle_initialised();
222 71 : break;
223 257 : case BATT_2:
224 257 : packet_ready = AP::battery().num_instances() > 1;
225 257 : break;
226 107 : case RPM:
227 : {
228 107 : packet_ready = false;
229 : #if AP_RPM_ENABLED
230 107 : const AP_RPM *rpm = AP::rpm();
231 107 : if (rpm == nullptr) {
232 0 : break;
233 : }
234 107 : packet_ready = rpm->num_sensors() > 0;
235 : #endif
236 : }
237 107 : break;
238 158 : case TERRAIN:
239 : {
240 : #if AP_TERRAIN_AVAILABLE
241 158 : const AP_Terrain *terrain = AP::terrain();
242 158 : packet_ready = terrain && terrain->enabled();
243 : #endif
244 : }
245 158 : break;
246 21 : case WIND:
247 : #if !APM_BUILD_TYPE(APM_BUILD_Rover)
248 : {
249 : float a;
250 21 : WITH_SEMAPHORE(AP::ahrs().get_semaphore());
251 21 : if (AP::ahrs().airspeed_TAS(a)) {
252 : // if we have an airspeed estimate then we have a valid wind estimate
253 21 : packet_ready = true;
254 : }
255 21 : }
256 : #else
257 : {
258 0 : const AP_WindVane* windvane = AP_WindVane::get_singleton();
259 0 : packet_ready = windvane != nullptr && windvane->enabled();
260 : }
261 : #endif
262 21 : break;
263 : #if AP_MISSION_ENABLED
264 280 : case WAYPOINT:
265 280 : packet_ready = AP::mission().get_current_nav_index() > 0;
266 280 : break;
267 : #endif // AP_MISSION_ENABLED
268 148 : case UDATA:
269 : // when using fport user data is sent by scheduler
270 : // when using sport user data is sent responding to custom polling
271 148 : packet_ready = _use_external_data && _sport_push_buffer.pending;
272 148 : break;
273 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
274 130 : case MAV:
275 130 : packet_ready = !_SPort_bidir.tx_packet_queue.is_empty();
276 130 : break;
277 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
278 238 : default:
279 238 : packet_ready = true;
280 238 : break;
281 : }
282 :
283 1771 : return packet_ready;
284 : }
285 :
286 : /*
287 : * WFQ scheduler
288 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
289 : */
290 329 : void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
291 : {
292 : // send packet
293 329 : switch (idx) {
294 51 : case TEXT: // 0x5000 status text
295 51 : if (get_next_msg_chunk()) {
296 51 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
297 : }
298 51 : break;
299 45 : case ATTITUDE: // 0x5006 Attitude and range
300 45 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
301 45 : break;
302 22 : case GPS_LAT: // 0x800 GPS lat
303 : // sample both lat and lon at the same time
304 22 : send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(_passthrough.send_latitude)); // gps latitude or longitude
305 22 : _passthrough.gps_lng_sample = calc_gps_latlng(_passthrough.send_latitude);
306 : // force the scheduler to select GPS lon as packet that's been waiting the most
307 : // this guarantees that lat and lon are sent as consecutive packets
308 22 : _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
309 22 : break;
310 25 : case GPS_LON: // 0x800 GPS lon
311 25 : send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
312 25 : break;
313 22 : case VEL_YAW: // 0x5005 Vel and Yaw
314 22 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
315 22 : break;
316 20 : case AP_STATUS: // 0x5001 AP status
317 20 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
318 20 : break;
319 20 : case GPS_STATUS: // 0x5002 GPS Status
320 20 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
321 20 : break;
322 22 : case HOME: // 0x5004 Home
323 22 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
324 22 : break;
325 0 : case BATT_2: // 0x5008 Battery 2 status
326 0 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
327 0 : break;
328 19 : case BATT_1: // 0x5003 Battery 1 status
329 19 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
330 19 : break;
331 18 : case PARAM: // 0x5007 parameters
332 18 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
333 18 : break;
334 16 : case RPM: // 0x500A rpm sensors 1 and 2
335 16 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
336 16 : break;
337 10 : case TERRAIN: // 0x500B terrain data
338 10 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0B, calc_terrain());
339 10 : break;
340 21 : case WIND: // 0x500C terrain data
341 21 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0C, calc_wind());
342 21 : break;
343 : #if AP_MISSION_ENABLED
344 0 : case WAYPOINT: // 0x500D waypoint data
345 0 : send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0D, calc_waypoint());
346 0 : break;
347 : #endif // AP_MISSION_ENABLED
348 0 : case UDATA: // user data
349 : {
350 0 : WITH_SEMAPHORE(_sport_push_buffer.sem);
351 0 : if (_use_external_data && _sport_push_buffer.pending) {
352 0 : send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
353 0 : _sport_push_buffer.pending = false;
354 : }
355 0 : }
356 0 : break;
357 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
358 18 : case MAV: // mavlite
359 18 : process_tx_queue();
360 18 : break;
361 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
362 : }
363 329 : }
364 :
365 : /*
366 : * send telemetry data
367 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
368 : */
369 31052 : void AP_Frsky_SPort_Passthrough::send(void)
370 : {
371 31052 : const uint16_t numc = MIN(_port->available(), 1024U);
372 :
373 : // this is the constant for hub data frame
374 31052 : if (_port->txspace() < 19) {
375 10517 : return;
376 : }
377 : // keep only the last two bytes of the data found in the serial buffer, as we shouldn't respond to old poll requests
378 20535 : uint8_t prev_byte = 0;
379 21413 : for (uint16_t i = 0; i < numc; i++) {
380 878 : prev_byte = _passthrough.new_byte;
381 878 : _passthrough.new_byte = _port->read();
382 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
383 : AP_Frsky_SPort::sport_packet_t sp;
384 :
385 878 : if (_sport_handler.process_byte(sp, _passthrough.new_byte)) {
386 19 : queue_rx_packet(sp);
387 : }
388 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
389 : }
390 : // check if we should respond to this polling byte
391 20535 : if (prev_byte == FRAME_HEAD) {
392 344 : if (is_passthrough_byte(_passthrough.new_byte)) {
393 344 : run_wfq_scheduler();
394 : } else {
395 : // respond to custom user data polling
396 0 : WITH_SEMAPHORE(_sport_push_buffer.sem);
397 0 : if (_sport_push_buffer.pending && _passthrough.new_byte == _sport_push_buffer.packet.sensor) {
398 0 : send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
399 0 : _sport_push_buffer.pending = false;
400 : }
401 0 : }
402 : }
403 : }
404 :
405 : /*
406 : * grabs one "chunk" (4 bytes) of the queued message to be transmitted
407 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
408 : */
409 51 : bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void)
410 : {
411 51 : if (!_statustext.available) {
412 5 : WITH_SEMAPHORE(_statustext.sem);
413 5 : if (!_statustext.queue.pop(_statustext.next)) {
414 0 : return false;
415 : }
416 5 : _statustext.available = true;
417 5 : }
418 :
419 51 : if (_msg_chunk.repeats == 0) { // if it's the first time get_next_msg_chunk is called for a given chunk
420 26 : uint8_t character = 0;
421 26 : _msg_chunk.chunk = 0; // clear the 4 bytes of the chunk buffer
422 :
423 121 : for (uint8_t i = 0; i < 4 && _msg_chunk.char_index < sizeof(_statustext.next.text); i++) {
424 98 : character = _statustext.next.text[_msg_chunk.char_index++];
425 :
426 98 : if (!character) {
427 3 : break;
428 : }
429 :
430 95 : _msg_chunk.chunk |= character << (3-i) * 8;
431 : }
432 :
433 26 : if (!character || (_msg_chunk.char_index == sizeof(_statustext.next.text))) { // we've reached the end of the message (string terminated by '\0' or last character of the string has been processed)
434 3 : _msg_chunk.char_index = 0; // reset index to get ready to process the next message
435 : // add severity which is sent as the MSB of the last three bytes of the last chunk (bits 24, 16, and 8) since a character is on 7 bits
436 3 : _msg_chunk.chunk |= (_statustext.next.severity & 0x4)<<21;
437 3 : _msg_chunk.chunk |= (_statustext.next.severity & 0x2)<<14;
438 3 : _msg_chunk.chunk |= (_statustext.next.severity & 0x1)<<7;
439 : }
440 : }
441 :
442 : // repeat each message chunk 3 times to ensure transmission
443 : // on slow links reduce the number of duplicate chunks
444 51 : uint8_t extra_chunks = 2;
445 :
446 51 : if (_scheduler.avg_packet_rate < 20) {
447 : // with 3 or more extra frsky sensors on the bus
448 : // send messages only once
449 51 : extra_chunks = 0;
450 0 : } else if (_scheduler.avg_packet_rate < 30) {
451 : // with 1 or 2 extra frsky sensors on the bus
452 : // send messages twice
453 0 : extra_chunks = 1;
454 : }
455 :
456 51 : if (_msg_chunk.repeats++ > extra_chunks ) {
457 25 : _msg_chunk.repeats = 0;
458 25 : if (_msg_chunk.char_index == 0) {
459 : // we're ready for the next message
460 2 : _statustext.available = false;
461 : }
462 : }
463 51 : return true;
464 : }
465 :
466 : /*
467 : * prepare parameter data
468 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
469 : */
470 18 : uint32_t AP_Frsky_SPort_Passthrough::calc_param(void)
471 : {
472 18 : uint8_t param_id = _paramID; //cache it because it gets changed inside the switch
473 18 : uint32_t param_value = 0;
474 :
475 18 : switch (_paramID) {
476 8 : case NONE:
477 : case FRAME_TYPE:
478 8 : param_value = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h
479 8 : _paramID = BATT_CAPACITY_1;
480 8 : break;
481 5 : case BATT_CAPACITY_1:
482 5 : param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(0)); // battery pack capacity in mAh
483 5 : _paramID = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : TELEMETRY_FEATURES;
484 5 : break;
485 0 : case BATT_CAPACITY_2:
486 0 : param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(1)); // battery pack capacity in mAh
487 0 : _paramID = TELEMETRY_FEATURES;
488 0 : break;
489 5 : case TELEMETRY_FEATURES:
490 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
491 5 : BIT_SET(param_value,PassthroughFeatures::BIDIR);
492 : #endif
493 : #if AP_SCRIPTING_ENABLED
494 5 : BIT_SET(param_value,PassthroughFeatures::SCRIPTING);
495 : #endif
496 5 : _paramID = FRAME_TYPE;
497 5 : break;
498 : }
499 : //Reserve first 8 bits for param ID, use other 24 bits to store parameter value
500 18 : return (param_id << PARAM_ID_OFFSET) | (param_value & PARAM_VALUE_LIMIT);
501 : }
502 :
503 : /*
504 : * prepare gps status data
505 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
506 : */
507 20 : uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void)
508 : {
509 20 : const AP_GPS &gps = AP::gps();
510 :
511 : // number of GPS satellites visible (limit to 15 (0xF) since the value is stored on 4 bits)
512 20 : uint32_t gps_status = (gps.num_sats() < GPS_SATS_LIMIT) ? gps.num_sats() : GPS_SATS_LIMIT;
513 : // GPS receiver status (limit to 0-3 (0x3) since the value is stored on 2 bits: NO_GPS = 0, NO_FIX = 1, GPS_OK_FIX_2D = 2, GPS_OK_FIX_3D or GPS_OK_FIX_3D_DGPS or GPS_OK_FIX_3D_RTK_FLOAT or GPS_OK_FIX_3D_RTK_FIXED = 3)
514 20 : gps_status |= (((uint8_t)gps.status() < GPS_STATUS_LIMIT) ? (uint8_t)gps.status() : GPS_STATUS_LIMIT)<<GPS_STATUS_OFFSET;
515 : // GPS horizontal dilution of precision in dm
516 20 : gps_status |= prep_number(roundf(gps.get_hdop() * 0.1f),2,1)<<GPS_HDOP_OFFSET;
517 : // GPS receiver advanced status (0: no advanced fix, 1: GPS_OK_FIX_3D_DGPS, 2: GPS_OK_FIX_3D_RTK_FLOAT, 3: GPS_OK_FIX_3D_RTK_FIXED)
518 20 : gps_status |= (((uint8_t)gps.status() > GPS_STATUS_LIMIT) ? (uint8_t)gps.status()-GPS_STATUS_LIMIT : 0)<<GPS_ADVSTATUS_OFFSET;
519 : // Altitude MSL in dm
520 20 : const Location &loc = gps.location();
521 20 : gps_status |= prep_number(roundf(loc.alt * 0.1f),2,2)<<GPS_ALTMSL_OFFSET;
522 20 : return gps_status;
523 : }
524 :
525 : /*
526 : * prepare battery data
527 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
528 : */
529 19 : uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance)
530 : {
531 19 : const AP_BattMonitor &_battery = AP::battery();
532 :
533 : float current, consumed_mah;
534 19 : if (!_battery.current_amps(current, instance)) {
535 0 : current = 0;
536 : }
537 19 : if (!_battery.consumed_mah(consumed_mah, instance)) {
538 0 : consumed_mah = 0;
539 : }
540 :
541 : // battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
542 19 : uint32_t batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
543 : // battery current draw in deciamps
544 19 : batt |= prep_number(roundf(current * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET;
545 : // battery current drawn since power on in mAh (limit to 32767 (0x7FFF) since value is stored on 15 bits)
546 19 : batt |= ((consumed_mah < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(consumed_mah) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
547 19 : return batt;
548 : }
549 :
550 : /*
551 : * true if we need to respond to the last polling byte
552 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
553 : */
554 344 : bool AP_Frsky_SPort_Passthrough::is_passthrough_byte(const uint8_t byte) const
555 : {
556 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
557 344 : if( byte == _SPort_bidir.downlink1_sensor_id || byte == _SPort_bidir.downlink2_sensor_id ) {
558 0 : return true;
559 : }
560 : #endif
561 344 : return byte == downlink_sensor_id;
562 : }
563 :
564 : /*
565 : * prepare various autopilot status data
566 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
567 : */
568 20 : uint32_t AP_Frsky_SPort_Passthrough::calc_ap_status(void)
569 : {
570 : // IMU temperature: offset -19, 0 means temp =< 19°, 63 means temp => 82°
571 20 : uint8_t imu_temp = 0;
572 : #if AP_INERTIALSENSOR_ENABLED
573 20 : imu_temp = (uint8_t) roundf(constrain_float(AP::ins().get_temperature(0), AP_IMU_TEMP_MIN, AP_IMU_TEMP_MAX) - AP_IMU_TEMP_MIN);
574 : #endif
575 :
576 : // control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits)
577 20 : uint32_t ap_status = (uint8_t)((gcs().custom_mode()+1) & AP_CONTROL_MODE_LIMIT);
578 : // simple/super simple modes flags
579 20 : ap_status |= (uint8_t)(gcs().simple_input_active())<<AP_SIMPLE_OFFSET;
580 20 : ap_status |= (uint8_t)(gcs().supersimple_input_active())<<AP_SSIMPLE_OFFSET;
581 : // is_flying flag
582 20 : ap_status |= (uint8_t)(AP_Notify::flags.flying) << AP_FLYING_OFFSET;
583 : // armed flag
584 20 : ap_status |= (uint8_t)(AP_Notify::flags.armed)<<AP_ARMED_OFFSET;
585 : // battery failsafe flag
586 20 : ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery)<<AP_BATT_FS_OFFSET;
587 : // bad ekf flag
588 20 : ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
589 : // generic failsafe
590 20 : ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery||AP_Notify::flags.failsafe_ekf||AP_Notify::flags.failsafe_gcs||AP_Notify::flags.failsafe_radio)<<AP_FS_OFFSET;
591 : #if AP_FENCE_ENABLED
592 : // fence status
593 20 : AC_Fence *fence = AP::fence();
594 20 : if (fence != nullptr) {
595 20 : ap_status |= (uint8_t)(fence->enabled() && fence->present()) << AP_FENCE_PRESENT_OFFSET;
596 20 : ap_status |= (uint8_t)(fence->get_breaches()>0) << AP_FENCE_BREACH_OFFSET;
597 : }
598 : #endif
599 : // signed throttle [-100,100] scaled down to [-63,63] on 7 bits, MSB for sign + 6 bits for 0-63
600 20 : ap_status |= prep_number(gcs().get_hud_throttle()*0.63, 2, 0)<<AP_THROTTLE_OFFSET;
601 : // IMU temperature
602 20 : ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
603 20 : return ap_status;
604 : }
605 :
606 : /*
607 : * prepare home position related data
608 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
609 : */
610 22 : uint32_t AP_Frsky_SPort_Passthrough::calc_home(void)
611 : {
612 22 : uint32_t home = 0;
613 22 : Location loc;
614 22 : Location home_loc;
615 22 : bool got_position = false;
616 22 : float _relative_home_altitude = 0;
617 :
618 : {
619 22 : AP_AHRS &_ahrs = AP::ahrs();
620 22 : WITH_SEMAPHORE(_ahrs.get_semaphore());
621 22 : got_position = _ahrs.get_location(loc);
622 22 : home_loc = _ahrs.get_home();
623 22 : }
624 :
625 22 : if (got_position) {
626 : // check home_loc is valid
627 21 : if (home_loc.lat != 0 || home_loc.lng != 0) {
628 : // distance between vehicle and home_loc in meters
629 21 : home = prep_number(roundf(home_loc.get_distance(loc)), 3, 2);
630 : // angle from front of vehicle to the direction of home_loc in 3 degree increments (just in case, limit to 127 (0x7F) since the value is stored on 7 bits)
631 21 : home |= (((uint8_t)roundf(loc.get_bearing_to(home_loc) * 0.00333f)) & HOME_BEARING_LIMIT)<<HOME_BEARING_OFFSET;
632 : }
633 : // altitude between vehicle and home_loc
634 21 : _relative_home_altitude = loc.alt;
635 21 : if (!loc.relative_alt) {
636 : // loc.alt has home altitude added, remove it
637 21 : _relative_home_altitude -= home_loc.alt;
638 : }
639 : }
640 : // altitude above home in decimeters
641 22 : home |= prep_number(roundf(_relative_home_altitude * 0.1f), 3, 2)<<HOME_ALT_OFFSET;
642 22 : return home;
643 : }
644 :
645 : /*
646 : * prepare velocity and yaw data
647 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
648 : */
649 22 : uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
650 : {
651 22 : float vspd = AP_RCTelemetry::get_vspeed_ms();
652 : // vertical velocity in dm/s
653 22 : uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1);
654 : float airspeed_m; // m/s
655 : float hspeed_m; // m/s
656 : bool airspeed_estimate_true;
657 22 : AP_AHRS &_ahrs = AP::ahrs();
658 : {
659 22 : WITH_SEMAPHORE(_ahrs.get_semaphore());
660 22 : hspeed_m = _ahrs.groundspeed(); // default is to use groundspeed
661 22 : airspeed_estimate_true = AP::ahrs().airspeed_TAS(airspeed_m);
662 22 : }
663 22 : bool option_airspeed_enabled = (_frsky_parameters->_options & frsky_options_e::OPTION_AIRSPEED_AND_GROUNDSPEED) != 0;
664 : // airspeed estimate + airspeed option disabled (default) => send airspeed (we give priority to airspeed over groundspeed)
665 : // airspeed estimate + airspeed option enabled => alternate airspeed/groundspeed, i.e send airspeed only when _passthrough.send_airspeed==true
666 22 : if (airspeed_estimate_true && (!option_airspeed_enabled || _passthrough.send_airspeed)) {
667 22 : hspeed_m = airspeed_m;
668 : }
669 : // horizontal velocity in dm/s
670 22 : velandyaw |= prep_number(roundf(hspeed_m * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
671 : // yaw from [0;36000] centidegrees to 0.2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
672 22 : velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
673 : // flag the airspeed bit if required
674 22 : if (airspeed_estimate_true && option_airspeed_enabled && _passthrough.send_airspeed) {
675 0 : velandyaw |= 1U<<VELANDYAW_ARSPD_OFFSET;
676 : }
677 : // toggle air/ground speed selector
678 22 : _passthrough.send_airspeed = !_passthrough.send_airspeed;
679 22 : return velandyaw;
680 : }
681 :
682 : /*
683 : * prepare attitude (roll, pitch) and range data
684 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
685 : */
686 45 : uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
687 : {
688 : #if AP_RANGEFINDER_ENABLED
689 45 : const RangeFinder *_rng = RangeFinder::get_singleton();
690 : #endif
691 :
692 : float roll;
693 : float pitch;
694 : float yaw;
695 45 : AP::vehicle()->get_osd_attitude_rad(roll, pitch, yaw);
696 : // roll from [-18000;18000] centidegrees to unsigned 0.2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
697 45 : uint32_t attiandrng = ((uint16_t)roundf((roll * RAD_TO_DEG * 100 + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT);
698 : // pitch from [-9000;9000] centidegrees to unsigned 0.2 degree increments [0;900] (just in case, limit to 1023 (0x3FF) since the value is stored on 10 bits)
699 45 : attiandrng |= ((uint16_t)roundf((pitch * RAD_TO_DEG * 100 + 9000) * 0.05f) & ATTIANDRNG_PITCH_LIMIT)<<ATTIANDRNG_PITCH_OFFSET;
700 : // rangefinder measurement in cm
701 : #if AP_RANGEFINDER_ENABLED
702 45 : attiandrng |= prep_number(_rng ? _rng->distance_orient(ROTATION_PITCH_270)*100 : 0, 3, 1)<<ATTIANDRNG_RNGFND_OFFSET;
703 : #else
704 : attiandrng |= prep_number(0, 3, 1)<<ATTIANDRNG_RNGFND_OFFSET;
705 : #endif
706 45 : return attiandrng;
707 : }
708 :
709 : /*
710 : * prepare rpm for sensors 1 and 2
711 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
712 : */
713 16 : uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void)
714 : {
715 : #if AP_RPM_ENABLED
716 16 : const AP_RPM *ap_rpm = AP::rpm();
717 16 : if (ap_rpm == nullptr) {
718 0 : return 0;
719 : }
720 16 : uint32_t value = 0;
721 : // we send: rpm_value*0.1 as 16 bits signed
722 : float rpm;
723 : // bits 0-15 for rpm 0
724 16 : if (ap_rpm->get_rpm(0,rpm)) {
725 16 : value |= (int16_t)roundf(rpm * 0.1);
726 : }
727 : // bits 16-31 for rpm 1
728 16 : if (ap_rpm->get_rpm(1,rpm)) {
729 0 : value |= (int16_t)roundf(rpm * 0.1) << 16;
730 : }
731 16 : return value;
732 : #else
733 : return 0;
734 : #endif
735 : }
736 :
737 : /*
738 : * prepare terrain data
739 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
740 : */
741 10 : uint32_t AP_Frsky_SPort_Passthrough::calc_terrain(void)
742 : {
743 10 : uint32_t value = 0;
744 : #if AP_TERRAIN_AVAILABLE
745 10 : AP_Terrain *terrain = AP::terrain();
746 10 : if (terrain == nullptr || !terrain->enabled()) {
747 0 : return value;
748 : }
749 : float height_above_terrain;
750 10 : if (terrain->height_above_terrain(height_above_terrain, true)) {
751 : // vehicle height above terrain
752 9 : value |= prep_number(roundf(height_above_terrain * 10), 3, 2);
753 : }
754 : // terrain unhealthy flag
755 10 : value |= (uint8_t)(terrain->status() == AP_Terrain::TerrainStatus::TerrainStatusUnhealthy) << TERRAIN_UNHEALTHY_OFFSET;
756 : #endif
757 10 : return value;
758 : }
759 :
760 : /*
761 : * prepare wind data
762 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
763 : * wind direction = 0 means North
764 : */
765 21 : uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void)
766 : {
767 : #if !APM_BUILD_TYPE(APM_BUILD_Rover)
768 21 : Vector3f v;
769 : {
770 21 : AP_AHRS &ahrs = AP::ahrs();
771 21 : WITH_SEMAPHORE(ahrs.get_semaphore());
772 : // use the estimate even if it is not marked valid, to preserve
773 : // existing behaviour
774 21 : IGNORE_RETURN(ahrs.get_wind(v));
775 21 : }
776 : // wind angle in 3 degree increments 0,360 (unsigned)
777 21 : uint32_t value = prep_number(roundf(wrap_360(degrees(atan2f(-v.y, -v.x))) * (1.0f/3.0f)), 2, 0);
778 : // wind speed in dm/s
779 21 : value |= prep_number(roundf(v.length() * 10), 2, 1) << WIND_SPEED_OFFSET;
780 : #else
781 0 : const AP_WindVane* windvane = AP_WindVane::get_singleton();
782 0 : uint32_t value = 0;
783 0 : if (windvane != nullptr && windvane->enabled()) {
784 : // true wind angle in 3 degree increments 0,360 (unsigned)
785 0 : value = prep_number(roundf(wrap_360(degrees(windvane->get_true_wind_direction_rad())) * (1.0f/3.0f)), 2, 0);
786 : // true wind speed in dm/s
787 0 : value |= prep_number(roundf(windvane->get_true_wind_speed() * 10), 2, 1) << WIND_SPEED_OFFSET;
788 : // apparent wind angle in 3 degree increments -180,180 (signed)
789 0 : value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0) << WIND_APPARENT_ANGLE_OFFSET;
790 : // apparent wind speed in dm/s
791 0 : value |= prep_number(roundf(windvane->get_apparent_wind_speed() * 10), 2, 1) << WIND_APPARENT_SPEED_OFFSET;
792 : }
793 : #endif
794 21 : return value;
795 : }
796 : /*
797 : * prepare waypoint data
798 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
799 : */
800 : #if AP_MISSION_ENABLED
801 0 : uint32_t AP_Frsky_SPort_Passthrough::calc_waypoint(void)
802 : {
803 0 : const AP_Vehicle *vehicle = AP::vehicle();
804 0 : if (vehicle == nullptr) {
805 0 : return 0U;
806 : }
807 : float wp_distance;
808 0 : if (!vehicle->get_wp_distance_m(wp_distance)) {
809 0 : return 0U;
810 : }
811 : float angle;
812 0 : if (!vehicle->get_wp_bearing_deg(angle)) {
813 0 : return 0U;
814 : }
815 : // waypoint current nav index
816 0 : uint32_t value = MIN(AP::mission().get_current_nav_index(), WP_NUMBER_LIMIT);
817 : // distance to next waypoint
818 0 : value |= prep_number(wp_distance, 3, 2) << WP_DISTANCE_OFFSET;
819 : // bearing encoded in 3 degrees increments
820 0 : value |= ((uint8_t)roundf(wrap_360(angle) * 0.333f)) << WP_BEARING_OFFSET;
821 0 : return value;
822 : }
823 : #endif // AP_MISSION_ENABLED
824 :
825 : /*
826 : fetch Sport data for an external transport, such as FPort or crossfire
827 : Note: we need to create a packet array with unique packet types
828 : For very big frames we might have to relax the "unique packet type per frame"
829 : constraint in order to maximize bandwidth usage
830 : */
831 0 : bool AP_Frsky_SPort_Passthrough::get_telem_data(sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
832 : {
833 0 : if (!_use_external_data) {
834 0 : return false;
835 : }
836 :
837 0 : uint8_t idx = 0;
838 :
839 : // max_size >= WFQ_LAST_ITEM
840 : // get a packet per enabled type
841 0 : if (max_size >= WFQ_LAST_ITEM) {
842 0 : for (uint8_t i=0; i<WFQ_LAST_ITEM; i++) {
843 0 : if (process_scheduler_entry(i)) {
844 0 : if (external_data.pending) {
845 0 : packet_array[idx].frame = external_data.packet.frame;
846 0 : packet_array[idx].appid = external_data.packet.appid;
847 0 : packet_array[idx].data = external_data.packet.data;
848 0 : idx++;
849 0 : external_data.pending = false;
850 : }
851 : }
852 : }
853 : } else {
854 : // max_size < WFQ_LAST_ITEM
855 : // call run_wfq_scheduler(false) enough times to create a packet of up to max_size unique elements
856 0 : uint32_t item_mask = 0;
857 0 : for (uint8_t i=0; i<max_size; i++) {
858 : // call the scheduler with the shaper "disabled"
859 0 : const uint8_t item = run_wfq_scheduler(false);
860 0 : if (!BIT_IS_SET(item_mask, item) && external_data.pending) {
861 : // ok got some data, flip the bitmask bit to prevent adding the same packet type more than once
862 0 : BIT_SET(item_mask, item);
863 0 : packet_array[idx].frame = external_data.packet.frame;
864 0 : packet_array[idx].appid = external_data.packet.appid;
865 0 : packet_array[idx].data = external_data.packet.data;
866 0 : idx++;
867 0 : external_data.pending = false;
868 : }
869 : }
870 : }
871 0 : packet_count = idx;
872 0 : return idx > 0;
873 : }
874 :
875 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
876 : /*
877 : allow external transports (e.g. FPort), to supply telemetry data
878 : */
879 0 : bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
880 : {
881 : // queue only Uplink packets
882 0 : if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) {
883 0 : const AP_Frsky_SPort::sport_packet_t sp {
884 : { 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID
885 : frame,
886 : appid,
887 : data }
888 0 : };
889 :
890 0 : _SPort_bidir.rx_packet_queue.push_force(sp);
891 0 : return true;
892 : }
893 0 : return false;
894 : }
895 :
896 : /*
897 : * Queue uplink packets in the sport rx queue
898 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
899 : */
900 19 : void AP_Frsky_SPort_Passthrough::queue_rx_packet(const AP_Frsky_SPort::sport_packet_t packet)
901 : {
902 : // queue only Uplink packets
903 19 : if (packet.sensor == _SPort_bidir.uplink_sensor_id && packet.frame == SPORT_UPLINK_FRAME) {
904 19 : _SPort_bidir.rx_packet_queue.push_force(packet);
905 : }
906 19 : }
907 :
908 : /*
909 : * Extract up to 1 mavlite message from the sport rx packet queue
910 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
911 : */
912 189206 : void AP_Frsky_SPort_Passthrough::process_rx_queue()
913 : {
914 : AP_Frsky_SPort::sport_packet_t packet;
915 189206 : uint8_t loop_count = 0; // prevent looping forever
916 189219 : while (_SPort_bidir.rx_packet_queue.pop(packet) && loop_count++ < MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN)) {
917 19 : AP_Frsky_MAVlite_Message rxmsg;
918 :
919 19 : if (sport_to_mavlite.process(rxmsg, packet)) {
920 6 : mavlite.process_message(rxmsg);
921 6 : break; // process only 1 mavlite message each call
922 : }
923 : }
924 189206 : }
925 :
926 : /*
927 : * Process the sport tx queue
928 : * pop and send 1 sport packet
929 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
930 : */
931 18 : void AP_Frsky_SPort_Passthrough::process_tx_queue()
932 : {
933 : AP_Frsky_SPort::sport_packet_t packet;
934 :
935 18 : if (!_SPort_bidir.tx_packet_queue.peek(packet)) {
936 0 : return;
937 : }
938 :
939 : // when using fport repeat each packet to account for
940 : // fport packet loss (around 15%)
941 18 : if (!_use_external_data || _SPort_bidir.tx_packet_duplicates++ == SPORT_TX_PACKET_DUPLICATES) {
942 18 : _SPort_bidir.tx_packet_queue.pop();
943 18 : _SPort_bidir.tx_packet_duplicates = 0;
944 : }
945 :
946 18 : send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data);
947 : }
948 :
949 : /*
950 : * Send a mavlite message
951 : * Message is chunked in sport packets pushed in the tx queue
952 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
953 : */
954 6 : bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg)
955 : {
956 6 : return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
957 : }
958 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
959 : /*
960 : * Utility method to apply constraints in changing sensor id values
961 : * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
962 : */
963 12 : void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &sensor)
964 : {
965 12 : int8_t idx = param_idx.get();
966 :
967 12 : if (idx == -1) {
968 : // disable this sensor
969 0 : sensor = 0xFF;
970 0 : return;
971 : }
972 12 : sensor = calc_sensor_id(idx);
973 : }
974 :
975 : namespace AP
976 : {
977 0 : AP_Frsky_SPort_Passthrough *frsky_passthrough_telem()
978 : {
979 0 : return AP_Frsky_SPort_Passthrough::get_singleton();
980 : }
981 : };
982 :
983 : #endif // AP_FRSKY_SPORT_PASSTHROUGH_ENABLED
|
