?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Updated comments in 'smartport.c' to reflect the specification.
[betaflight.git] / src / main / telemetry / smartport.c
blob7485268b9cb17ccbad73525c957e25a43be43a66
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 /*
22 * SmartPort Telemetry implementation by frank26080115
23 * see https://github.com/frank26080115/cleanflight/wiki/Using-Smart-Port
24 */
25 #include <stdbool.h>
26 #include <stdint.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <math.h>
30
31 #include "platform.h"
32
33 #if defined(USE_TELEMETRY_SMARTPORT)
34
35 #include "common/axis.h"
36 #include "common/color.h"
37 #include "common/maths.h"
38 #include "common/utils.h"
39
40 #include "config/feature.h"
41
42 #include "rx/frsky_crc.h"
43
44 #include "drivers/accgyro/accgyro.h"
45 #include "drivers/compass/compass.h"
46 #include "drivers/sensor.h"
47 #include "drivers/time.h"
48
49 #include "config/config.h"
50 #include "fc/controlrate_profile.h"
51 #include "fc/rc_controls.h"
52 #include "fc/runtime_config.h"
53
54 #include "flight/failsafe.h"
55 #include "flight/imu.h"
56 #include "flight/mixer.h"
57 #include "flight/pid.h"
58 #include "flight/position.h"
59
60 #include "io/beeper.h"
61 #include "io/gps.h"
62 #include "io/motors.h"
63 #include "io/serial.h"
64
65 #include "msp/msp.h"
66
67 #include "rx/rx.h"
68
69 #include "pg/pg.h"
70 #include "pg/pg_ids.h"
71 #include "pg/rx.h"
72
73 #include "sensors/acceleration.h"
74 #include "sensors/adcinternal.h"
75 #include "sensors/barometer.h"
76 #include "sensors/battery.h"
77 #include "sensors/boardalignment.h"
78 #include "sensors/compass.h"
79 #include "sensors/esc_sensor.h"
80 #include "sensors/gyro.h"
81 #include "sensors/sensors.h"
82
83 #include "telemetry/msp_shared.h"
84 #include "telemetry/smartport.h"
85 #include "telemetry/telemetry.h"
86
87 #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
88
89 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
90 enum
91 {
92 FSSP_DATAID_SPEED = 0x0830 ,
93 FSSP_DATAID_VFAS = 0x0210 ,
94 FSSP_DATAID_VFAS1 = 0x0211 ,
95 FSSP_DATAID_VFAS2 = 0x0212 ,
96 FSSP_DATAID_VFAS3 = 0x0213 ,
97 FSSP_DATAID_VFAS4 = 0x0214 ,
98 FSSP_DATAID_VFAS5 = 0x0215 ,
99 FSSP_DATAID_VFAS6 = 0x0216 ,
100 FSSP_DATAID_VFAS7 = 0x0217 ,
101 FSSP_DATAID_VFAS8 = 0x0218 ,
102 FSSP_DATAID_CURRENT = 0x0200 ,
103 FSSP_DATAID_CURRENT1 = 0x0201 ,
104 FSSP_DATAID_CURRENT2 = 0x0202 ,
105 FSSP_DATAID_CURRENT3 = 0x0203 ,
106 FSSP_DATAID_CURRENT4 = 0x0204 ,
107 FSSP_DATAID_CURRENT5 = 0x0205 ,
108 FSSP_DATAID_CURRENT6 = 0x0206 ,
109 FSSP_DATAID_CURRENT7 = 0x0207 ,
110 FSSP_DATAID_CURRENT8 = 0x0208 ,
111 FSSP_DATAID_RPM = 0x0500 ,
112 FSSP_DATAID_RPM1 = 0x0501 ,
113 FSSP_DATAID_RPM2 = 0x0502 ,
114 FSSP_DATAID_RPM3 = 0x0503 ,
115 FSSP_DATAID_RPM4 = 0x0504 ,
116 FSSP_DATAID_RPM5 = 0x0505 ,
117 FSSP_DATAID_RPM6 = 0x0506 ,
118 FSSP_DATAID_RPM7 = 0x0507 ,
119 FSSP_DATAID_RPM8 = 0x0508 ,
120 FSSP_DATAID_ALTITUDE = 0x0100 ,
121 FSSP_DATAID_FUEL = 0x0600 ,
122 FSSP_DATAID_ADC1 = 0xF102 ,
123 FSSP_DATAID_ADC2 = 0xF103 ,
124 FSSP_DATAID_LATLONG = 0x0800 ,
125 FSSP_DATAID_CAP_USED = 0x0600 ,
126 FSSP_DATAID_VARIO = 0x0110 ,
127 FSSP_DATAID_CELLS = 0x0300 ,
128 FSSP_DATAID_CELLS_LAST = 0x030F ,
129 FSSP_DATAID_HEADING = 0x0840 ,
130 #if defined(USE_ACC)
131 FSSP_DATAID_PITCH = 0x5230 , // custom
132 FSSP_DATAID_ROLL = 0x5240 , // custom
133 FSSP_DATAID_ACCX = 0x0700 ,
134 FSSP_DATAID_ACCY = 0x0710 ,
135 FSSP_DATAID_ACCZ = 0x0720 ,
136 #endif
137 FSSP_DATAID_T1 = 0x0400 ,
138 FSSP_DATAID_T11 = 0x0401 ,
139 FSSP_DATAID_T2 = 0x0410 ,
140 FSSP_DATAID_HOME_DIST = 0x0420 ,
141 FSSP_DATAID_GPS_ALT = 0x0820 ,
142 FSSP_DATAID_ASPD = 0x0A00 ,
143 FSSP_DATAID_TEMP = 0x0B70 ,
144 FSSP_DATAID_TEMP1 = 0x0B71 ,
145 FSSP_DATAID_TEMP2 = 0x0B72 ,
146 FSSP_DATAID_TEMP3 = 0x0B73 ,
147 FSSP_DATAID_TEMP4 = 0x0B74 ,
148 FSSP_DATAID_TEMP5 = 0x0B75 ,
149 FSSP_DATAID_TEMP6 = 0x0B76 ,
150 FSSP_DATAID_TEMP7 = 0x0B77 ,
151 FSSP_DATAID_TEMP8 = 0x0B78 ,
152 FSSP_DATAID_A3 = 0x0900 ,
153 FSSP_DATAID_A4 = 0x0910
154 };
155
156 // if adding more sensors then increase this value (should be equal to the maximum number of ADD_SENSOR calls)
157 #define MAX_DATAIDS 20
158
159 static uint16_t frSkyDataIdTable[MAX_DATAIDS];
160
161 #ifdef USE_ESC_SENSOR_TELEMETRY
162 // number of sensors to send between sending the ESC sensors
163 #define ESC_SENSOR_PERIOD 7
164
165 // if adding more esc sensors then increase this value
166 #define MAX_ESC_DATAIDS 4
167
168 static uint16_t frSkyEscDataIdTable[MAX_ESC_DATAIDS];
169 #endif
170
171 typedef struct frSkyTableInfo_s {
172 uint16_t * table;
173 uint8_t size;
174 uint8_t index;
175 } frSkyTableInfo_t;
176
177 static frSkyTableInfo_t frSkyDataIdTableInfo = { frSkyDataIdTable, 0, 0 };
178 #ifdef USE_ESC_SENSOR_TELEMETRY
179 static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, 0, 0};
180 #endif
181
182 #define SMARTPORT_BAUD 57600
183 #define SMARTPORT_UART_MODE MODE_RXTX
184 #define SMARTPORT_SERVICE_TIMEOUT_US 1000 // max allowed time to find a value to send
185
186 static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
187 static const serialPortConfig_t *portConfig;
188
189 static portSharing_e smartPortPortSharing;
190
191 enum
192 {
193 TELEMETRY_STATE_UNINITIALIZED,
194 TELEMETRY_STATE_INITIALIZED_SERIAL,
195 TELEMETRY_STATE_INITIALIZED_EXTERNAL,
196 };
197
198 static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
199
200 typedef struct smartPortFrame_s {
201 uint8_t sensorId;
202 smartPortPayload_t payload;
203 uint8_t crc;
204 } __attribute__((packed)) smartPortFrame_t;
205
206 #define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
207
208 static smartPortWriteFrameFn *smartPortWriteFrame;
209
210 #if defined(USE_MSP_OVER_TELEMETRY)
211 static bool smartPortMspReplyPending = false;
212 #endif
213
214 smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortReadyToSendFn *readyToSend, bool useChecksum)
215 {
216 static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
217 static uint8_t smartPortRxBytes = 0;
218 static bool skipUntilStart = true;
219 static bool awaitingSensorId = false;
220 static bool byteStuffing = false;
221 static uint16_t checksum = 0;
222
223 if (c == FSSP_START_STOP) {
224 *clearToSend = false;
225 smartPortRxBytes = 0;
226 awaitingSensorId = true;
227 skipUntilStart = false;
228
229 return NULL;
230 } else if (skipUntilStart) {
231 return NULL;
232 }
233
234 if (awaitingSensorId) {
235 awaitingSensorId = false;
236 if ((c == FSSP_SENSOR_ID1) && readyToSend()) {
237 // our slot is starting, start sending
238 *clearToSend = true;
239 // no need to decode more
240 skipUntilStart = true;
241 } else if (c == FSSP_SENSOR_ID2) {
242 checksum = 0;
243 } else {
244 skipUntilStart = true;
245 }
246 } else {
247 if (c == FSSP_DLE) {
248 byteStuffing = true;
249
250 return NULL;
251 } else if (byteStuffing) {
252 c ^= FSSP_DLE_XOR;
253 byteStuffing = false;
254 }
255
256 if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
257 rxBuffer[smartPortRxBytes++] = (uint8_t)c;
258 checksum += c;
259
260 if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
261 skipUntilStart = true;
262
263 return (smartPortPayload_t *)&rxBuffer;
264 }
265 } else {
266 skipUntilStart = true;
267
268 checksum += c;
269 checksum = (checksum & 0xFF) + (checksum >> 8);
270 if (checksum == 0xFF) {
271 return (smartPortPayload_t *)&rxBuffer;
272 }
273 }
274 }
275
276 return NULL;
277 }
278
279 void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
280 {
281 // smart port escape sequence
282 if (c == FSSP_DLE || c == FSSP_START_STOP) {
283 serialWrite(port, FSSP_DLE);
284 serialWrite(port, c ^ FSSP_DLE_XOR);
285 } else {
286 serialWrite(port, c);
287 }
288
289 if (checksum != NULL) {
290 frskyCheckSumStep(checksum, c);
291 }
292 }
293
294 bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
295 {
296 return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
297 }
298
299 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
300 {
301 uint8_t *data = (uint8_t *)payload;
302 for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
303 smartPortSendByte(*data++, &checksum, port);
304 }
305 frskyCheckSumFini(&checksum);
306 smartPortSendByte(checksum, NULL, port);
307 }
308
309 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
310 {
311 smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
312 }
313
314 static void smartPortSendPackage(uint16_t id, uint32_t val)
315 {
316 smartPortPayload_t payload;
317 payload.frameId = FSSP_DATA_FRAME;
318 payload.valueId = id;
319 payload.data = val;
320
321 smartPortWriteFrame(&payload);
322 }
323
324 #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId
325 #define ADD_ESC_SENSOR(dataId) frSkyEscDataIdTableInfo.table[frSkyEscDataIdTableInfo.index++] = dataId
326
327 static void initSmartPortSensors(void)
328 {
329 frSkyDataIdTableInfo.index = 0;
330
331 if (telemetryIsSensorEnabled(SENSOR_MODE)) {
332 ADD_SENSOR(FSSP_DATAID_T1);
333 ADD_SENSOR(FSSP_DATAID_T2);
334 }
335
336 #if defined(USE_ADC_INTERNAL)
337 if (telemetryIsSensorEnabled(SENSOR_TEMPERATURE)) {
338 ADD_SENSOR(FSSP_DATAID_T11);
339 }
340 #endif
341
342 if (isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) {
343 #ifdef USE_ESC_SENSOR_TELEMETRY
344 if (!telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE))
345 #endif
346 {
347 ADD_SENSOR(FSSP_DATAID_VFAS);
348 }
349
350 ADD_SENSOR(FSSP_DATAID_A4);
351 }
352
353 if (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT)) {
354 #ifdef USE_ESC_SENSOR_TELEMETRY
355 if (!telemetryIsSensorEnabled(ESC_SENSOR_CURRENT))
356 #endif
357 {
358 ADD_SENSOR(FSSP_DATAID_CURRENT);
359 }
360
361 if (telemetryIsSensorEnabled(SENSOR_FUEL)) {
362 ADD_SENSOR(FSSP_DATAID_FUEL);
363 }
364 }
365
366 if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
367 ADD_SENSOR(FSSP_DATAID_HEADING);
368 }
369
370 #if defined(USE_ACC)
371 if (sensors(SENSOR_ACC)) {
372 if (telemetryIsSensorEnabled(SENSOR_PITCH)) {
373 ADD_SENSOR(FSSP_DATAID_PITCH);
374 }
375 if (telemetryIsSensorEnabled(SENSOR_ROLL)) {
376 ADD_SENSOR(FSSP_DATAID_ROLL);
377 }
378 if (telemetryIsSensorEnabled(SENSOR_ACC_X)) {
379 ADD_SENSOR(FSSP_DATAID_ACCX);
380 }
381 if (telemetryIsSensorEnabled(SENSOR_ACC_Y)) {
382 ADD_SENSOR(FSSP_DATAID_ACCY);
383 }
384 if (telemetryIsSensorEnabled(SENSOR_ACC_Z)) {
385 ADD_SENSOR(FSSP_DATAID_ACCZ);
386 }
387 }
388 #endif
389
390 if (sensors(SENSOR_BARO)) {
391 if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
392 ADD_SENSOR(FSSP_DATAID_ALTITUDE);
393 }
394 if (telemetryIsSensorEnabled(SENSOR_VARIO)) {
395 ADD_SENSOR(FSSP_DATAID_VARIO);
396 }
397 }
398
399 #ifdef USE_GPS
400 if (featureIsEnabled(FEATURE_GPS)) {
401 if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) {
402 ADD_SENSOR(FSSP_DATAID_SPEED);
403 }
404 if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) {
405 ADD_SENSOR(FSSP_DATAID_LATLONG);
406 ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long)
407 }
408 if (telemetryIsSensorEnabled(SENSOR_DISTANCE)) {
409 ADD_SENSOR(FSSP_DATAID_HOME_DIST);
410 }
411 if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
412 ADD_SENSOR(FSSP_DATAID_GPS_ALT);
413 }
414 }
415 #endif
416
417 frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index;
418 frSkyDataIdTableInfo.index = 0;
419
420 #ifdef USE_ESC_SENSOR_TELEMETRY
421 frSkyEscDataIdTableInfo.index = 0;
422
423 if (telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) {
424 ADD_ESC_SENSOR(FSSP_DATAID_VFAS);
425 }
426 if (telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) {
427 ADD_ESC_SENSOR(FSSP_DATAID_CURRENT);
428 }
429 if (telemetryIsSensorEnabled(ESC_SENSOR_RPM)) {
430 ADD_ESC_SENSOR(FSSP_DATAID_RPM);
431 }
432 if (telemetryIsSensorEnabled(ESC_SENSOR_TEMPERATURE)) {
433 ADD_ESC_SENSOR(FSSP_DATAID_TEMP);
434 }
435
436 frSkyEscDataIdTableInfo.size = frSkyEscDataIdTableInfo.index;
437 frSkyEscDataIdTableInfo.index = 0;
438 #endif
439 }
440
441 bool initSmartPortTelemetry(void)
442 {
443 if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
444 portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
445 if (portConfig) {
446 smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
447
448 smartPortWriteFrame = smartPortWriteFrameInternal;
449
450 initSmartPortSensors();
451
452 telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
453 }
454
455 return true;
456 }
457
458 return false;
459 }
460
461 bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
462 {
463 if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
464 smartPortWriteFrame = smartPortWriteFrameExternal;
465
466 initSmartPortSensors();
467
468 telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
469
470 return true;
471 }
472
473 return false;
474 }
475
476 static void freeSmartPortTelemetryPort(void)
477 {
478 closeSerialPort(smartPortSerialPort);
479 smartPortSerialPort = NULL;
480 }
481
482 static void configureSmartPortTelemetryPort(void)
483 {
484 if (portConfig) {
485 portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
486
487 smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
488 }
489 }
490
491 void checkSmartPortTelemetryState(void)
492 {
493 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
494 bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
495
496 if (enableSerialTelemetry && !smartPortSerialPort) {
497 configureSmartPortTelemetryPort();
498 } else if (!enableSerialTelemetry && smartPortSerialPort) {
499 freeSmartPortTelemetryPort();
500 }
501 }
502 }
503
504 #if defined(USE_MSP_OVER_TELEMETRY)
505 static void smartPortSendMspResponse(uint8_t *data) {
506 smartPortPayload_t payload;
507 payload.frameId = FSSP_MSPS_FRAME;
508 memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
509
510 smartPortWriteFrame(&payload);
511 }
512 #endif
513
514 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const timeUs_t *requestTimeout)
515 {
516 static uint8_t smartPortIdCycleCnt = 0;
517 static uint8_t t1Cnt = 0;
518 static uint8_t t2Cnt = 0;
519 static uint8_t skipRequests = 0;
520 #ifdef USE_ESC_SENSOR_TELEMETRY
521 static uint8_t smartPortIdOffset = 0;
522 #endif
523
524 #if defined(USE_MSP_OVER_TELEMETRY)
525 if (skipRequests) {
526 skipRequests--;
527 } else if (payload && smartPortPayloadContainsMSP(payload)) {
528 // Do not check the physical ID here again
529 // unless we start receiving other sensors' packets
530 // Pass only the payload: skip frameId
531 uint8_t *frameStart = (uint8_t *)&payload->valueId;
532 smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
533
534 // Don't send MSP response after write to eeprom
535 // CPU just got out of suspended state after writeEEPROM()
536 // We don't know if the receiver is listening again
537 // Skip a few telemetry requests before sending response
538 if (skipRequests) {
539 *clearToSend = false;
540 }
541 }
542 #else
543 UNUSED(payload);
544 #endif
545
546 bool doRun = true;
547 while (doRun && *clearToSend && !skipRequests) {
548 // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
549 if (requestTimeout) {
550 if (cmpTimeUs(micros(), *requestTimeout) >= 0) {
551 *clearToSend = false;
552
553 return;
554 }
555 } else {
556 doRun = false;
557 }
558
559 #if defined(USE_MSP_OVER_TELEMETRY)
560 if (smartPortMspReplyPending) {
561 smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
562 *clearToSend = false;
563
564 return;
565 }
566 #endif
567
568 // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
569 frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
570
571 #ifdef USE_ESC_SENSOR_TELEMETRY
572 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
573 // send ESC sensors
574 tableInfo = &frSkyEscDataIdTableInfo;
575 if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
576 tableInfo->index = 0;
577 smartPortIdCycleCnt = 0;
578 smartPortIdOffset++;
579 if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
580 smartPortIdOffset = 0;
581 }
582 }
583 }
584 if (smartPortIdCycleCnt < ESC_SENSOR_PERIOD) {
585 // send other sensors
586 tableInfo = &frSkyDataIdTableInfo;
587 #endif
588 if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
589 tableInfo->index = 0;
590 }
591 #ifdef USE_ESC_SENSOR_TELEMETRY
592 }
593 #endif
594 uint16_t id = tableInfo->table[tableInfo->index];
595 #ifdef USE_ESC_SENSOR_TELEMETRY
596 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
597 id += smartPortIdOffset;
598 }
599 #endif
600 smartPortIdCycleCnt++;
601 tableInfo->index++;
602
603 int32_t tmpi;
604 uint32_t tmp2 = 0;
605 uint16_t vfasVoltage;
606 uint8_t cellCount;
607
608 #ifdef USE_ESC_SENSOR_TELEMETRY
609 escSensorData_t *escData;
610 #endif
611
612 switch (id) {
613 case FSSP_DATAID_VFAS :
614 vfasVoltage = getBatteryVoltage();
615 if (telemetryConfig()->report_cell_voltage) {
616 cellCount = getBatteryCellCount();
617 vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0;
618 }
619 smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec
620 *clearToSend = false;
621 break;
622 #ifdef USE_ESC_SENSOR_TELEMETRY
623 case FSSP_DATAID_VFAS1 :
624 case FSSP_DATAID_VFAS2 :
625 case FSSP_DATAID_VFAS3 :
626 case FSSP_DATAID_VFAS4 :
627 case FSSP_DATAID_VFAS5 :
628 case FSSP_DATAID_VFAS6 :
629 case FSSP_DATAID_VFAS7 :
630 case FSSP_DATAID_VFAS8 :
631 escData = getEscSensorData(id - FSSP_DATAID_VFAS1);
632 if (escData != NULL) {
633 smartPortSendPackage(id, escData->voltage);
634 *clearToSend = false;
635 }
636 break;
637 #endif
638 case FSSP_DATAID_CURRENT :
639 smartPortSendPackage(id, getAmperage() / 10); // in 0.1A according to SmartPort spec
640 *clearToSend = false;
641 break;
642 #ifdef USE_ESC_SENSOR_TELEMETRY
643 case FSSP_DATAID_CURRENT1 :
644 case FSSP_DATAID_CURRENT2 :
645 case FSSP_DATAID_CURRENT3 :
646 case FSSP_DATAID_CURRENT4 :
647 case FSSP_DATAID_CURRENT5 :
648 case FSSP_DATAID_CURRENT6 :
649 case FSSP_DATAID_CURRENT7 :
650 case FSSP_DATAID_CURRENT8 :
651 escData = getEscSensorData(id - FSSP_DATAID_CURRENT1);
652 if (escData != NULL) {
653 smartPortSendPackage(id, escData->current);
654 *clearToSend = false;
655 }
656 break;
657 case FSSP_DATAID_RPM :
658 escData = getEscSensorData(ESC_SENSOR_COMBINED);
659 if (escData != NULL) {
660 smartPortSendPackage(id, calcEscRpm(escData->rpm));
661 *clearToSend = false;
662 }
663 break;
664 case FSSP_DATAID_RPM1 :
665 case FSSP_DATAID_RPM2 :
666 case FSSP_DATAID_RPM3 :
667 case FSSP_DATAID_RPM4 :
668 case FSSP_DATAID_RPM5 :
669 case FSSP_DATAID_RPM6 :
670 case FSSP_DATAID_RPM7 :
671 case FSSP_DATAID_RPM8 :
672 escData = getEscSensorData(id - FSSP_DATAID_RPM1);
673 if (escData != NULL) {
674 smartPortSendPackage(id, calcEscRpm(escData->rpm));
675 *clearToSend = false;
676 }
677 break;
678 case FSSP_DATAID_TEMP :
679 escData = getEscSensorData(ESC_SENSOR_COMBINED);
680 if (escData != NULL) {
681 smartPortSendPackage(id, escData->temperature);
682 *clearToSend = false;
683 }
684 break;
685 case FSSP_DATAID_TEMP1 :
686 case FSSP_DATAID_TEMP2 :
687 case FSSP_DATAID_TEMP3 :
688 case FSSP_DATAID_TEMP4 :
689 case FSSP_DATAID_TEMP5 :
690 case FSSP_DATAID_TEMP6 :
691 case FSSP_DATAID_TEMP7 :
692 case FSSP_DATAID_TEMP8 :
693 escData = getEscSensorData(id - FSSP_DATAID_TEMP1);
694 if (escData != NULL) {
695 smartPortSendPackage(id, escData->temperature);
696 *clearToSend = false;
697 }
698 break;
699 #endif
700 case FSSP_DATAID_ALTITUDE :
701 smartPortSendPackage(id, getEstimatedAltitudeCm()); // in cm according to SmartPort spec
702 *clearToSend = false;
703 break;
704 case FSSP_DATAID_FUEL :
705 smartPortSendPackage(id, getMAhDrawn()); // given in mAh, should be in percent according to SmartPort spec
706 *clearToSend = false;
707 break;
708 case FSSP_DATAID_VARIO :
709 smartPortSendPackage(id, getEstimatedVario()); // in cm/s according to SmartPort spec
710 *clearToSend = false;
711 break;
712 case FSSP_DATAID_HEADING :
713 smartPortSendPackage(id, attitude.values.yaw * 10); // in degrees * 100 according to SmartPort spec
714 *clearToSend = false;
715 break;
716 #if defined(USE_ACC)
717 case FSSP_DATAID_PITCH :
718 smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
719 *clearToSend = false;
720 break;
721 case FSSP_DATAID_ROLL :
722 smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
723 *clearToSend = false;
724 break;
725 case FSSP_DATAID_ACCX :
726 smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
727 *clearToSend = false;
728 break;
729 case FSSP_DATAID_ACCY :
730 smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] * acc.dev.acc_1G_rec));
731 *clearToSend = false;
732 break;
733 case FSSP_DATAID_ACCZ :
734 smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec));
735 *clearToSend = false;
736 break;
737 #endif
738 case FSSP_DATAID_T1 :
739 // we send all the flags as decimal digits for easy reading
740
741 // the t1Cnt simply allows the telemetry view to show at least some changes
742 t1Cnt++;
743 if (t1Cnt == 4) {
744 t1Cnt = 1;
745 }
746 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
747 // the Taranis seems to be able to fit 5 digits on the screen
748 // the Taranis seems to consider this number a signed 16 bit integer
749
750 if (!isArmingDisabled()) {
751 tmpi += 1;
752 } else {
753 tmpi += 2;
754 }
755 if (ARMING_FLAG(ARMED)) {
756 tmpi += 4;
757 }
758
759 if (FLIGHT_MODE(ANGLE_MODE)) {
760 tmpi += 10;
761 }
762 if (FLIGHT_MODE(HORIZON_MODE)) {
763 tmpi += 20;
764 }
765 if (FLIGHT_MODE(PASSTHRU_MODE)) {
766 tmpi += 40;
767 }
768
769 if (FLIGHT_MODE(MAG_MODE)) {
770 tmpi += 100;
771 }
772
773 if (FLIGHT_MODE(HEADFREE_MODE)) {
774 tmpi += 4000;
775 }
776
777 smartPortSendPackage(id, (uint32_t)tmpi);
778 *clearToSend = false;
779 break;
780 case FSSP_DATAID_T2 :
781 #ifdef USE_GPS
782 if (sensors(SENSOR_GPS)) {
783 // satellite accuracy HDOP: 0 = worst [HDOP > 5.5m], 9 = best [HDOP <= 1.0m]
784 uint16_t hdop = constrain(scaleRange(gpsSol.hdop, 100, 550, 9, 0), 0, 9) * 100;
785 smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + hdop + gpsSol.numSat);
786 *clearToSend = false;
787 } else if (featureIsEnabled(FEATURE_GPS)) {
788 smartPortSendPackage(id, 0);
789 *clearToSend = false;
790 } else
791 #endif
792 if (telemetryConfig()->pidValuesAsTelemetry) {
793 switch (t2Cnt) {
794 case 0:
795 tmp2 = currentPidProfile->pid[PID_ROLL].P;
796 tmp2 += (currentPidProfile->pid[PID_PITCH].P<<8);
797 tmp2 += (currentPidProfile->pid[PID_YAW].P<<16);
798 break;
799 case 1:
800 tmp2 = currentPidProfile->pid[PID_ROLL].I;
801 tmp2 += (currentPidProfile->pid[PID_PITCH].I<<8);
802 tmp2 += (currentPidProfile->pid[PID_YAW].I<<16);
803 break;
804 case 2:
805 tmp2 = currentPidProfile->pid[PID_ROLL].D;
806 tmp2 += (currentPidProfile->pid[PID_PITCH].D<<8);
807 tmp2 += (currentPidProfile->pid[PID_YAW].D<<16);
808 break;
809 case 3:
810 tmp2 = currentControlRateProfile->rates[FD_ROLL];
811 tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8);
812 tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16);
813 break;
814 }
815 tmp2 += t2Cnt<<24;
816 t2Cnt++;
817 if (t2Cnt == 4) {
818 t2Cnt = 0;
819 }
820 smartPortSendPackage(id, tmp2);
821 *clearToSend = false;
822 }
823
824 break;
825 #if defined(USE_ADC_INTERNAL)
826 case FSSP_DATAID_T11 :
827 smartPortSendPackage(id, getCoreTemperatureCelsius());
828 *clearToSend = false;
829 break;
830 #endif
831 #ifdef USE_GPS
832 case FSSP_DATAID_SPEED :
833 if (STATE(GPS_FIX)) {
834 //convert to knots: 1cm/s = 0.0194384449 knots
835 //Speed should be sent in knots/1000 (GPS speed is in cm/s)
836 uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
837 smartPortSendPackage(id, tmpui);
838 *clearToSend = false;
839 }
840 break;
841 case FSSP_DATAID_LATLONG :
842 if (STATE(GPS_FIX)) {
843 uint32_t tmpui = 0;
844 // the same ID is sent twice, one for longitude, one for latitude
845 // the MSB of the sent uint32_t helps FrSky keep track
846 // the even/odd bit of our counter helps us keep track
847 if (tableInfo->index & 1) {
848 tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
849 tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
850 if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
851 }
852 else {
853 tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
854 tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
855 if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
856 }
857 smartPortSendPackage(id, tmpui);
858 *clearToSend = false;
859 }
860 break;
861 case FSSP_DATAID_HOME_DIST :
862 if (STATE(GPS_FIX)) {
863 smartPortSendPackage(id, GPS_distanceToHome);
864 *clearToSend = false;
865 }
866 break;
867 case FSSP_DATAID_GPS_ALT :
868 if (STATE(GPS_FIX)) {
869 smartPortSendPackage(id, gpsSol.llh.altCm); // in cm according to SmartPort spec
870 *clearToSend = false;
871 }
872 break;
873 #endif
874 case FSSP_DATAID_A4 :
875 cellCount = getBatteryCellCount();
876 vfasVoltage = cellCount ? (getBatteryVoltage() / cellCount) : 0; // in 0.01V according to SmartPort spec
877 smartPortSendPackage(id, vfasVoltage);
878 *clearToSend = false;
879 break;
880 default:
881 break;
882 // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
883 }
884 }
885 }
886
887 static bool serialReadyToSend(void)
888 {
889 return (serialRxBytesWaiting(smartPortSerialPort) == 0);
890 }
891
892 void handleSmartPortTelemetry(void)
893 {
894 const timeUs_t requestTimeout = micros() + SMARTPORT_SERVICE_TIMEOUT_US;
895
896 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
897 smartPortPayload_t *payload = NULL;
898 bool clearToSend = false;
899 while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
900 uint8_t c = serialRead(smartPortSerialPort);
901 payload = smartPortDataReceive(c, &clearToSend, serialReadyToSend, true);
902 }
903
904 processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
905 }
906 }
907 #endif