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 : /*
17 : Spektrum Telemetry library, based on AP_Frsky_Telem.cpp
18 : See https://www.spektrumrc.com/ProdInfo/Files/SPM_Telemetry_Developers_Specs.pdf
19 : */
20 :
21 : #include "AP_Spektrum_Telem.h"
22 : #include <AP_HAL/utility/sparse-endian.h>
23 : #include <AP_AHRS/AP_AHRS.h>
24 : #include <AP_RPM/AP_RPM.h>
25 : #include <AP_Airspeed/AP_Airspeed.h>
26 : #include <AP_Compass/AP_Compass.h>
27 : #include <AP_BattMonitor/AP_BattMonitor.h>
28 : #include <AP_RangeFinder/AP_RangeFinder.h>
29 : #include <AP_Common/AP_FWVersion.h>
30 : #include <GCS_MAVLink/GCS.h>
31 : #include <AP_Common/Location.h>
32 : #include <AP_GPS/AP_GPS.h>
33 : #include <AP_Baro/AP_Baro.h>
34 : #include <AP_RTC/AP_RTC.h>
35 : #include <AP_SerialManager/AP_SerialManager.h>
36 : #include <AP_ESC_Telem/AP_ESC_Telem.h>
37 : #include <math.h>
38 :
39 : #if HAL_SPEKTRUM_TELEM_ENABLED
40 :
41 : #define MICROSEC_PER_MINUTE 60000000
42 : #define MAX_TEXTGEN_LEN 13
43 :
44 : //#define SPKT_DEBUG
45 : #ifdef SPKT_DEBUG
46 : # define debug(fmt, args...) hal.console->printf("SPKT:" fmt "\n", ##args)
47 : #else
48 : # define debug(fmt, args...) do {} while(0)
49 : #endif
50 :
51 : extern const AP_HAL::HAL& hal;
52 :
53 : AP_Spektrum_Telem *AP_Spektrum_Telem::singleton;
54 :
55 0 : AP_Spektrum_Telem::AP_Spektrum_Telem() : AP_RCTelemetry(0)
56 : {
57 0 : singleton = this;
58 0 : }
59 :
60 0 : AP_Spektrum_Telem::~AP_Spektrum_Telem(void)
61 : {
62 0 : singleton = nullptr;
63 0 : }
64 :
65 0 : bool AP_Spektrum_Telem::init(void)
66 : {
67 : // sanity check that we are using a UART for RC input
68 0 : if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0)) {
69 0 : return false;
70 : }
71 0 : return AP_RCTelemetry::init();
72 : }
73 :
74 : /*
75 : setup ready for passthrough telem
76 : */
77 0 : void AP_Spektrum_Telem::setup_wfq_scheduler(void)
78 : {
79 : // initialize packet weights for the WFQ scheduler
80 : // priority[i] = 1/_scheduler.packet_weight[i]
81 : // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
82 :
83 : // Spektrum telemetry rate is 46Hz, so these rates must fit
84 0 : add_scheduler_entry(50, 100); // qos 10Hz
85 0 : add_scheduler_entry(50, 100); // rpm 10Hz
86 0 : add_scheduler_entry(50, 100); // text, 10Hz
87 0 : add_scheduler_entry(50, 120); // Attitude and compass 8Hz
88 0 : add_scheduler_entry(550, 280); // GPS 3Hz
89 0 : add_scheduler_entry(550, 280); // ESC 3Hz
90 0 : add_scheduler_entry(400, 250); // altitude 4Hz
91 0 : add_scheduler_entry(400, 250); // airspeed 4Hz
92 0 : add_scheduler_entry(700, 500); // GPS status 2Hz
93 0 : add_scheduler_entry(1300, 500); // batt volt 2Hz
94 0 : add_scheduler_entry(1300, 500); // batt curr 2Hz
95 0 : add_scheduler_entry(1300, 500); // batt mah 2Hz
96 0 : add_scheduler_entry(1300, 500); // temp 2Hz
97 0 : }
98 :
99 0 : void AP_Spektrum_Telem::adjust_packet_weight(bool queue_empty)
100 : {
101 0 : if (!queue_empty) {
102 0 : _scheduler.packet_weight[TEXT] = 50; // messages
103 : } else {
104 0 : _scheduler.packet_weight[TEXT] = 5000; // messages
105 : }
106 0 : }
107 :
108 : // WFQ scheduler
109 0 : bool AP_Spektrum_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
110 : {
111 0 : bool packet_ready = false;
112 0 : switch (idx) {
113 0 : case TEXT:
114 0 : packet_ready = !queue_empty;
115 0 : break;
116 0 : default:
117 0 : packet_ready = true;
118 0 : break;
119 : }
120 :
121 0 : return packet_ready;
122 : }
123 :
124 : // WFQ scheduler
125 0 : void AP_Spektrum_Telem::process_packet(uint8_t idx)
126 : {
127 : // send packet
128 0 : switch (idx) {
129 0 : case QOS: // QOS
130 0 : calc_qos();
131 0 : break;
132 0 : case RPM: // RPM
133 0 : calc_rpm();
134 0 : break;
135 0 : case TEXT: // status text
136 0 : if (repeat_msg_chunk() || get_next_msg_chunk()) {
137 0 : send_msg_chunk(_msg_chunk);
138 : }
139 0 : break;
140 0 : case ATTITUDE: // Attitude and compass
141 0 : calc_attandmag();
142 0 : break;
143 0 : case GPS_LOC: // GPS location
144 0 : calc_gps_location();
145 0 : break;
146 0 : case ESC: // ESC
147 0 : calc_esc();
148 0 : break;
149 0 : case ALTITUDE: // altitude
150 0 : calc_altitude();
151 0 : break;
152 0 : case AIRSPEED: // airspeed
153 0 : calc_airspeed();
154 0 : break;
155 0 : case GPS_STATUS: // GPS status
156 0 : calc_gps_status();
157 0 : break;
158 0 : case VOLTAGE: // Battery volts
159 0 : calc_batt_volts(0);
160 0 : break;
161 0 : case AMPS: // Battery current
162 0 : calc_batt_amps(0);
163 0 : break;
164 0 : case MAH: // Battery current & mah
165 0 : calc_batt_mah();
166 0 : break;
167 0 : case TEMP: // temperature
168 0 : calc_temperature(0);
169 0 : break;
170 0 : default:
171 0 : break;
172 : }
173 0 : }
174 :
175 : // whether to repeat the last texgen output
176 0 : bool AP_Spektrum_Telem::repeat_msg_chunk(void)
177 : {
178 0 : if (_msg_chunk.repeats == 0) {
179 0 : return false;
180 : }
181 :
182 : // repeat each message chunk 3 times to ensure transmission
183 : // on slow links reduce the number of duplicate chunks
184 0 : uint8_t extra_chunks = 2;
185 :
186 0 : if (_scheduler.avg_packet_rate < 20) {
187 0 : extra_chunks = 0;
188 0 : } else if (_scheduler.avg_packet_rate < 30) {
189 0 : extra_chunks = 1;
190 : }
191 :
192 0 : if (_msg_chunk.repeats++ > extra_chunks) {
193 0 : _msg_chunk.repeats = 0;
194 0 : return false;
195 : }
196 0 : return true;
197 : }
198 :
199 :
200 : // grabs one "chunk" (13 bytes) of the queued message to be transmitted
201 0 : bool AP_Spektrum_Telem::get_next_msg_chunk(void)
202 : {
203 0 : _msg_chunk.repeats++;
204 :
205 0 : if (!_statustext.available) {
206 0 : WITH_SEMAPHORE(_statustext.sem);
207 0 : if (!_statustext.queue.pop(_statustext.next)) {
208 0 : return false;
209 : }
210 :
211 0 : _statustext.available = true;
212 :
213 : // We're going to display a new message so first clear the screen
214 0 : _msg_chunk.linenumber = 0xFF;
215 0 : _msg_chunk.char_index = 0;
216 0 : return true;
217 0 : }
218 :
219 0 : uint8_t character = 0;
220 0 : memset(_msg_chunk.chunk, 0, MAX_TEXTGEN_LEN);
221 :
222 0 : const uint8_t message_len = sizeof(_statustext.next.text);
223 : // the message fits in an entire line of text
224 : if (message_len < MAX_TEXTGEN_LEN) {
225 : memcpy(_msg_chunk.chunk, _statustext.next.text, message_len);
226 : _msg_chunk.linenumber = 0;
227 : _statustext.available = false;
228 : return true;
229 : }
230 :
231 : // a following part of multi-line text
232 0 : if (_msg_chunk.linenumber == 0xFF) {
233 0 : _msg_chunk.linenumber = 0;
234 0 : } else if (_msg_chunk.char_index > 0) {
235 0 : _msg_chunk.linenumber++;
236 : }
237 :
238 : // skip leading whitespace
239 0 : while (_statustext.next.text[_msg_chunk.char_index] == ' ' && _msg_chunk.char_index < message_len) {
240 0 : _msg_chunk.char_index++;
241 : }
242 :
243 0 : uint8_t space_idx = 0;
244 0 : const uint8_t begin_idx = _msg_chunk.char_index;
245 : // can't fit it all on one line so wrap at an appropriate place
246 0 : for (int i = 0; i < MAX_TEXTGEN_LEN && _msg_chunk.char_index < message_len; i++) {
247 0 : character = _statustext.next.text[_msg_chunk.char_index++];
248 : // split at the first ':'
249 0 : if (character == ':') {
250 0 : _msg_chunk.chunk[i] = 0;
251 0 : break;
252 : }
253 : // record the last space if we need to go back there
254 0 : if (character == ' ') {
255 0 : space_idx = _msg_chunk.char_index;
256 : }
257 :
258 0 : _msg_chunk.chunk[i] = character;
259 :
260 0 : if (!character) {
261 0 : break;
262 : }
263 : }
264 : // still not done, can we break at a word boundary?
265 0 : if (character != 0 && _msg_chunk.char_index < message_len && space_idx > 0) {
266 0 : _msg_chunk.char_index = space_idx;
267 0 : _msg_chunk.chunk[space_idx - begin_idx - 1] = 0;
268 : }
269 :
270 : // we've reached the end of the message (string terminated by '\0' or last character of the string has been processed)
271 0 : if (character == 0 || _msg_chunk.char_index == message_len) {
272 0 : _msg_chunk.char_index = 0; // reset index to get ready to process the next message
273 0 : _statustext.available = false;
274 : }
275 :
276 0 : return true;
277 : }
278 :
279 : // prepare qos data - mandatory frame that must be sent periodically
280 0 : void AP_Spektrum_Telem::calc_qos()
281 : {
282 0 : _telem.qos.identifier = TELE_DEVICE_QOS;
283 0 : _telem.qos.sID = 0;
284 0 : _telem.qos.A = 0xFFFF;
285 0 : _telem.qos.B = 0xFFFF;
286 0 : _telem.qos.L = 0xFFFF;
287 0 : _telem.qos.R = 0xFFFF;
288 0 : _telem.qos.F = 0xFFFF;
289 0 : _telem.qos.H = 0xFFFF;
290 0 : _telem.qos.rxVoltage = 0xFFFF;
291 0 : _telem_pending = true;
292 0 : }
293 :
294 : // prepare rpm data - B/E mandatory frame that must be sent periodically
295 0 : void AP_Spektrum_Telem::calc_rpm()
296 : {
297 : #if AP_BATTERY_ENABLED
298 0 : const AP_BattMonitor &_battery = AP::battery();
299 : #endif
300 :
301 0 : _telem.rpm.identifier = TELE_DEVICE_RPM;
302 0 : _telem.rpm.sID = 0;
303 : // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
304 : #if AP_BATTERY_ENABLED
305 0 : _telem.rpm.volts = htobe16(((uint16_t)roundf(_battery.voltage(0) * 100.0f)));
306 : #endif
307 0 : _telem.rpm.temperature = htobe16(int16_t(roundf(32.0f + AP::baro().get_temperature(0) * 9.0f / 5.0f)));
308 : #if AP_RPM_ENABLED
309 0 : const AP_RPM *rpm = AP::rpm();
310 : float rpm_value;
311 0 : if (!rpm || !rpm->get_rpm(0, rpm_value) || rpm_value < 999.0f) {
312 0 : rpm_value = 999.0f;
313 : }
314 0 : _telem.rpm.microseconds = htobe16(uint16_t(roundf(MICROSEC_PER_MINUTE / rpm_value)));
315 0 : _telem.rpm.dBm_A = 0x7F;
316 0 : _telem.rpm.dBm_B = 0x7F;
317 : #endif
318 0 : _telem_pending = true;
319 0 : }
320 :
321 0 : void AP_Spektrum_Telem::send_msg_chunk(const MessageChunk& chunk)
322 : {
323 0 : memcpy(_telem.textgen.text, chunk.chunk, 13);
324 0 : _telem.textgen.identifier = TELE_DEVICE_TEXTGEN;
325 0 : _telem.textgen.lineNumber = chunk.linenumber;
326 0 : _telem.textgen.sID = 0;
327 0 : _telem_pending = true;
328 0 : }
329 :
330 : // prepare battery data - B/E but not supported by Spektrum
331 0 : void AP_Spektrum_Telem::calc_batt_volts(uint8_t instance)
332 : {
333 : #if AP_BATTERY_ENABLED
334 0 : const AP_BattMonitor &_battery = AP::battery();
335 :
336 : // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
337 0 : _telem.hv.volts = htobe16(uint16_t(roundf(_battery.voltage(instance) * 100.0f)));
338 : #endif
339 0 : _telem.hv.identifier = TELE_DEVICE_VOLTAGE;
340 0 : _telem.hv.sID = 0;
341 0 : _telem_pending = true;
342 0 : }
343 :
344 : // prepare battery data - B/E but not supported by Spektrum
345 0 : void AP_Spektrum_Telem::calc_batt_amps(uint8_t instance)
346 : {
347 : #if AP_BATTERY_ENABLED
348 0 : const AP_BattMonitor &_battery = AP::battery();
349 :
350 : float current;
351 0 : if (!_battery.current_amps(current, instance)) {
352 0 : current = 0;
353 : }
354 :
355 : // Range: +/- 150A Resolution: 300A / 2048 = 0.196791 A/count
356 0 : _telem.amps.current = htobe16(int16_t(roundf(current * 2048.0f / 300.0f)));
357 : #endif
358 0 : _telem.amps.identifier = TELE_DEVICE_AMPS;
359 0 : _telem.amps.sID = 0;
360 0 : _telem_pending = true;
361 0 : }
362 :
363 : // prepare battery data - L/E
364 0 : void AP_Spektrum_Telem::calc_batt_mah()
365 : {
366 : #if AP_BATTERY_ENABLED
367 0 : const AP_BattMonitor &_battery = AP::battery();
368 : #endif
369 :
370 0 : _telem.fpMAH.identifier = TELE_DEVICE_FP_MAH;
371 0 : _telem.fpMAH.sID = 0;
372 :
373 : #if AP_BATTERY_ENABLED
374 : float current;
375 0 : if (!_battery.current_amps(current, 0)) {
376 0 : current = 0;
377 : }
378 0 : _telem.fpMAH.current_A = int16_t(roundf(current * 10.0f)); // Instantaneous current, 0.1A (0-3276.6A)
379 :
380 : float used_mah;
381 0 : if (!_battery.consumed_mah(used_mah, 0)) {
382 0 : used_mah = 0;
383 : }
384 0 : _telem.fpMAH.chargeUsed_A = int16_t(roundf(used_mah)); // Integrated mAh used, 1mAh (0-32.766Ah)
385 :
386 : float temp;
387 0 : if (_battery.get_temperature(temp, 0)) {
388 0 : _telem.fpMAH.temp_A = uint16_t(roundf(temp * 10.0f)); // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
389 : } else {
390 0 : _telem.fpMAH.temp_A = 0x7FFF;
391 : }
392 :
393 0 : if (!_battery.current_amps(current, 1)) {
394 0 : current = 0;
395 : }
396 0 : _telem.fpMAH.current_B = int16_t(roundf(current * 10.0f)); // Instantaneous current, 0.1A (0-3276.6A)
397 :
398 0 : if (!_battery.consumed_mah(used_mah, 1)) {
399 0 : used_mah = 0;
400 : }
401 0 : _telem.fpMAH.chargeUsed_B = int16_t(roundf(used_mah)); // Integrated mAh used, 1mAh (0-32.766Ah)
402 :
403 0 : if (_battery.get_temperature(temp, 1)) {
404 0 : _telem.fpMAH.temp_B = uint16_t(roundf(temp * 10.0f)); // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
405 : } else {
406 0 : _telem.fpMAH.temp_B = 0x7FFF;
407 : }
408 : #else
409 : _telem.fpMAH.temp_A = 0x7FFF;
410 : _telem.fpMAH.temp_B = 0x7FFF;
411 : #endif
412 :
413 0 : _telem_pending = true;
414 0 : }
415 :
416 : // prepare temperature data - B/E but not supported by Spektrum
417 0 : void AP_Spektrum_Telem::calc_temperature(uint8_t instance)
418 : {
419 0 : _telem.temp.temperature = htobe16(int16_t(roundf(32.0f + AP::baro().get_temperature(instance) * 9.0f / 5.0f)));
420 0 : _telem.temp.identifier = TELE_DEVICE_TEMPERATURE;
421 0 : _telem.temp.sID = 0;
422 0 : _telem_pending = true;
423 0 : }
424 :
425 : // prepare altitude data - B/E
426 0 : void AP_Spektrum_Telem::calc_altitude()
427 : {
428 0 : _telem.alt.identifier = TELE_DEVICE_ALTITUDE;
429 0 : _telem.alt.sID = 0;
430 :
431 0 : AP_AHRS &ahrs = AP::ahrs();
432 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
433 :
434 0 : float alt = 0;
435 0 : ahrs.get_relative_position_D_home(alt);
436 0 : alt = roundf(-alt * 10.0f);
437 0 : _telem.alt.altitude = htobe16(uint16_t(alt)); // .1m increments
438 0 : _max_alt = MAX(alt, _max_alt);
439 0 : _telem.alt.maxAltitude = htobe16(uint16_t(_max_alt)); // .1m increments
440 0 : _telem_pending = true;
441 0 : }
442 :
443 : // prepare airspeed data - B/E
444 0 : void AP_Spektrum_Telem::calc_airspeed()
445 : {
446 0 : _telem.speed.identifier = TELE_DEVICE_AIRSPEED;
447 0 : _telem.speed.sID = 0;
448 :
449 0 : AP_AHRS &ahrs = AP::ahrs();
450 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
451 :
452 0 : float speed = 0.0f;
453 : #if AP_AIRSPEED_ENABLED
454 0 : const AP_Airspeed *airspeed = AP::airspeed();
455 0 : if (airspeed && airspeed->healthy()) {
456 0 : speed = roundf(airspeed->get_airspeed() * 3.6);
457 : } else {
458 0 : speed = roundf(AP::ahrs().groundspeed() * 3.6);
459 : }
460 : #else
461 : speed = roundf(AP::ahrs().groundspeed() * 3.6);
462 : #endif
463 :
464 0 : _telem.speed.airspeed = htobe16(uint16_t(speed)); // 1 km/h increments
465 0 : _max_speed = MAX(speed, _max_speed);
466 0 : _telem.speed.maxAirspeed = htobe16(uint16_t(_max_speed)); // 1 km/h increments
467 0 : _telem_pending = true;
468 0 : }
469 :
470 : // prepare attitude and compass data - L/E
471 0 : void AP_Spektrum_Telem::calc_attandmag(void)
472 : {
473 0 : _telem.attMag.identifier = TELE_DEVICE_ATTMAG;
474 0 : _telem.attMag.sID = 0;
475 :
476 0 : AP_AHRS &_ahrs = AP::ahrs();
477 0 : WITH_SEMAPHORE(_ahrs.get_semaphore());
478 :
479 : // Attitude, 3 axes. Roll is a rotation about the X Axis of the vehicle using the RHR.
480 : // Units are 0.1 deg - Pitch is a rotation about the Y Axis of the vehicle using the RHR.
481 : // Yaw is a rotation about the Z Axis of the vehicle using the RHR.
482 0 : _telem.attMag.attRoll = _ahrs.roll_sensor / 10;
483 0 : _telem.attMag.attPitch = _ahrs.pitch_sensor / 10;
484 0 : _telem.attMag.attYaw = _ahrs.yaw_sensor / 10;
485 0 : _telem.attMag.heading = (_ahrs.yaw_sensor / 10) % 3600; // Heading, 0.1deg
486 :
487 0 : const Vector3f& field = AP::compass().get_field();
488 :
489 0 : _telem.attMag.magX = int16_t(roundf(field.x * 10.0f)); // Units are 0.1mG
490 0 : _telem.attMag.magY = int16_t(roundf(field.y * 10.0f));
491 0 : _telem.attMag.magZ = int16_t(roundf(field.z * 10.0f));
492 0 : _telem_pending = true;
493 0 : }
494 :
495 : // prepare gps location - L/E
496 0 : void AP_Spektrum_Telem::calc_gps_location()
497 : {
498 0 : const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
499 0 : const uint32_t u1e8 = 100000000, u1e7 = 10000000, u1e6 = 1000000, u1e5 = 100000, u1e4 = 10000;
500 :
501 0 : _telem.gpsloc.identifier = TELE_DEVICE_GPS_LOC; // Source device = 0x16
502 0 : _telem.gpsloc.sID = 0; // Secondary ID
503 0 : uint32_t alt = (abs(loc.alt) / 10) % u1e6;
504 0 : _telem.gpsloc.altitudeLow = ((alt % u1e4 / 1000) << 12) | ((alt % 1000 / 100) << 8)
505 0 : | ((alt % 100 / 10) << 4) | (alt % 100); // BCD, meters, format 3.1 (Low order of altitude)
506 :
507 0 : const float lat = fabsf(loc.lat / 1.0e7f); // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
508 0 : const float lng = fabsf(loc.lng / 1.0e7f); // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
509 :
510 0 : const uint32_t ulat = roundf((int32_t(lat) * 100.0f + (lat - int32_t(lat)) * 60.0f) * 10000.0f);
511 0 : const uint32_t ulng = roundf((int32_t(lng) * 100.0f + (lng - int32_t(lng)) * 60.0f) * 10000.0f);
512 :
513 0 : _telem.gpsloc.latitude = ((ulat % u1e8 / u1e7) << 28) | ((ulat % u1e7 / u1e6) << 24) | ((ulat % u1e6 / u1e5) << 20) | ((ulat % u1e5 / u1e4) << 16)
514 0 : | ((ulat % u1e4 / 1000) << 12) | ((ulat % 1000 / 100) << 8) | ((ulat % 100 / 10) << 4) | (ulat % 10);
515 0 : _telem.gpsloc.longitude = ((ulng % u1e8 / u1e7) << 28) | ((ulng % u1e7 / u1e6) << 24) | ((ulng % u1e6 / u1e5) << 20) | ((ulng % u1e5 / u1e4) << 16)
516 0 : | ((ulng % u1e4 / 1000) << 12) | ((ulng % 1000 / 100) << 8) | ((ulng % 100 / 10) << 4) | (ulng % 10);
517 :
518 0 : uint16_t course = uint16_t(roundf(AP::gps().ground_course() * 10.0f));
519 0 : _telem.gpsloc.course = ((course % u1e5 / u1e4) << 12) | ((course % u1e4 / 1000) << 8) | ((course % 1000 / 100) << 4) | (course % 100 / 10); // BCD, 3.1
520 0 : uint16_t hdop = AP::gps().get_hdop(0);
521 0 : _telem.gpsloc.HDOP = ((hdop % 1000 / 100) << 4) | (hdop % 100 / 10); // BCD, format 1.1
522 0 : _telem.gpsloc.GPSflags = 0;
523 :
524 0 : if (AP::gps().status(0) >= AP_GPS_FixType::FIX_3D) {
525 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_3D_FIX;
526 : }
527 0 : if (loc.alt < 0) {
528 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_NEGATIVE_ALT;
529 : }
530 0 : if ((loc.lng / 1e7) > 99) {
531 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_LONGITUDE_GREATER_99;
532 : }
533 0 : if (loc.lat >= 0) {
534 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_IS_NORTH;
535 : }
536 0 : if (loc.lng >= 0) {
537 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_IS_EAST;
538 : }
539 0 : if (AP::gps().status(0) > AP_GPS_FixType::NONE) {
540 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_GPS_FIX_VALID;
541 : }
542 0 : if (AP::gps().status(0) >= AP_GPS_FixType::NONE) {
543 0 : _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_GPS_DATA_RECEIVED;
544 : }
545 0 : _telem_pending = true;
546 0 : }
547 :
548 : // prepare gps status - L/E
549 0 : void AP_Spektrum_Telem::calc_gps_status()
550 : {
551 0 : const Location &loc = AP::gps().location(0);
552 :
553 0 : _telem.gpsstat.identifier = TELE_DEVICE_GPS_STATS; // Source device = 0x17
554 0 : _telem.gpsstat.sID = 0; // Secondary ID
555 :
556 0 : uint16_t knots = roundf(AP::gps().ground_speed() * 1.94384f * 10.0f);
557 0 : _telem.gpsstat.speed = ((knots % 10000 / 1000) << 12) | ((knots % 1000 / 100) << 8) | ((knots % 100 / 10) << 4) | (knots % 10); // BCD, knots, format 3.1
558 : uint16_t ms;
559 : uint8_t h, m, s;
560 : #if AP_RTC_ENABLED
561 0 : AP::rtc().get_system_clock_utc(h, m, s, ms); // BCD, format HH:MM:SS.S, format 6.1
562 : // FIXME: the above call can fail!
563 : #else
564 : h = 0;
565 : m = 0;
566 : s = 0;
567 : ms = 0;
568 : #endif
569 0 : _telem.gpsstat.UTC = ((((h / 10) << 4) | (h % 10)) << 20) | ((((m / 10) << 4) | (m % 10)) << 12) | ((((s / 10) << 4) | (s % 10)) << 4) | (ms / 100) ;
570 0 : uint8_t nsats = AP::gps().num_sats();
571 0 : _telem.gpsstat.numSats = ((nsats / 10) << 4) | (nsats % 10); // BCD, 0-99
572 0 : uint32_t alt = (abs(loc.alt) / 100000);
573 0 : _telem.gpsstat.altitudeHigh = ((alt / 10) << 4) | (alt % 10); // BCD, meters, format 2.0 (High order of altitude)
574 0 : _telem_pending = true;
575 0 : }
576 :
577 : // prepare ESC information - B/E
578 0 : void AP_Spektrum_Telem::calc_esc()
579 : {
580 : #if HAL_WITH_ESC_TELEM
581 0 : uint8_t esc = AP::esc_telem().get_max_rpm_esc();
582 0 : const volatile AP_ESC_Telem_Backend::TelemetryData& td = AP::esc_telem().get_telem_data(esc); // ideally should rotate between ESCs
583 0 : float rpm = 0.0f;
584 0 : uint16_t rpmdata = 0xFFFFU;
585 0 : if (AP::esc_telem().get_rpm(esc, rpm)) {
586 0 : rpmdata = uint16_t(roundf(rpm));
587 : }
588 :
589 0 : _telem.esc.identifier = TELE_DEVICE_ESC; // Source device = 0x20
590 0 : _telem.esc.sID = 0; // Secondary ID
591 0 : _telem.esc.RPM = htobe16(rpmdata); // Electrical RPM, 10RPM (0-655340 RPM) 0xFFFF --> "No data"
592 0 : _telem.esc.voltsInput = htobe16(td.voltage * 100); // Volts, 0.01v (0-655.34V) 0xFFFF --> "No data"
593 0 : _telem.esc.tempFET = htobe16(td.temperature_cdeg * 10); // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
594 0 : _telem.esc.currentMotor = htobe16(td.current * 100); // Current, 10mA (0-655.34A) 0xFFFF --> "No data"
595 0 : _telem.esc.tempBEC = 0xFFFF; // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
596 0 : _telem.esc.currentBEC = 0xFF; // BEC Current, 100mA (0-25.4A) 0xFF ----> "No data"
597 0 : _telem.esc.voltsBEC = 0xFF; // BEC Volts, 0.05V (0-12.70V) 0xFF ----> "No data"
598 0 : _telem.esc.throttle = 0xFF; // 0.5% (0-100%) 0xFF ----> "No data"
599 0 : _telem.esc.powerOut = 0xFF; // Power Output, 0.5% (0-127%) 0xFF ----> "No data"
600 0 : _telem_pending = true;
601 : #endif
602 0 : }
603 :
604 : /*
605 : fetch Spektrum data for an external transport, such as SRXL2
606 : */
607 0 : bool AP_Spektrum_Telem::_get_telem_data(uint8_t* data)
608 : {
609 0 : memset(&_telem, 0, 16);
610 0 : run_wfq_scheduler();
611 0 : if (!_telem_pending) {
612 0 : return false;
613 : }
614 0 : memcpy(data, &_telem, 16);
615 0 : _telem_pending = false;
616 0 : return true;
617 : }
618 :
619 : /*
620 : fetch data for an external transport, such as SRXL2
621 : */
622 0 : bool AP_Spektrum_Telem::get_telem_data(uint8_t* data)
623 : {
624 0 : if (!singleton && !hal.util->get_soft_armed()) {
625 : // if telem data is requested when we are disarmed and don't
626 : // yet have a AP_Spektrum_Telem object then try to allocate one
627 0 : NEW_NOTHROW AP_Spektrum_Telem();
628 : // initialize the passthrough scheduler
629 0 : if (singleton) {
630 0 : singleton->init();
631 : }
632 : }
633 0 : if (!singleton) {
634 0 : return false;
635 : }
636 0 : return singleton->_get_telem_data(data);
637 : }
638 :
639 : namespace AP {
640 262927 : AP_Spektrum_Telem *spektrum_telem() {
641 262927 : return AP_Spektrum_Telem::get_singleton();
642 : }
643 : };
644 :
645 : #endif
|
