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