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