?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Add Winbond W25Q512J support (#14036)
[betaflight.git] / src / main / telemetry / smartport.c
blobb2de692a0561630c9dedae5561ab16de1f120553
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 // On SmartPort, SERIAL_INVERTED is default
491 const portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : 0)
492 | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
493
494 smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
495 }
496 }
497
498 void checkSmartPortTelemetryState(void)
499 {
500 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
501 bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
502
503 if (enableSerialTelemetry && !smartPortSerialPort) {
504 configureSmartPortTelemetryPort();
505 } else if (!enableSerialTelemetry && smartPortSerialPort) {
506 freeSmartPortTelemetryPort();
507 }
508 }
509 }
510
511 #if defined(USE_MSP_OVER_TELEMETRY)
512 static void smartPortSendMspResponse(uint8_t *data, const uint8_t dataSize)
513 {
514 smartPortPayload_t payload;
515 payload.frameId = FSSP_MSPS_FRAME;
516 memcpy(&payload.valueId, data, MIN(dataSize,SMARTPORT_MSP_PAYLOAD_SIZE));
517
518 smartPortWriteFrame(&payload);
519 }
520 #endif
521
522 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const timeUs_t *requestTimeout)
523 {
524 static uint8_t smartPortIdCycleCnt = 0;
525 static uint8_t t1Cnt = 0;
526 static uint8_t t2Cnt = 0;
527 static uint8_t skipRequests = 0;
528 #ifdef USE_ESC_SENSOR_TELEMETRY
529 static uint8_t smartPortIdOffset = 0;
530 #endif
531
532 #if defined(USE_MSP_OVER_TELEMETRY)
533 if (skipRequests) {
534 skipRequests--;
535 } else if (payload && smartPortPayloadContainsMSP(payload)) {
536 // Do not check the physical ID here again
537 // unless we start receiving other sensors' packets
538 // Pass only the payload: skip frameId
539 uint8_t *frameStart = (uint8_t *)&payload->valueId;
540 smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
541
542 // Don't send MSP response after write to eeprom
543 // CPU just got out of suspended state after writeEEPROM()
544 // We don't know if the receiver is listening again
545 // Skip a few telemetry requests before sending response
546 if (skipRequests) {
547 *clearToSend = false;
548 }
549 }
550 #else
551 UNUSED(payload);
552 #endif
553
554 bool doRun = true;
555 while (doRun && *clearToSend && !skipRequests) {
556 // 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.
557 if (requestTimeout) {
558 if (cmpTimeUs(micros(), *requestTimeout) >= 0) {
559 *clearToSend = false;
560
561 return;
562 }
563 } else {
564 doRun = false;
565 }
566
567 #if defined(USE_MSP_OVER_TELEMETRY)
568 if (smartPortMspReplyPending) {
569 smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
570 *clearToSend = false;
571
572 return;
573 }
574 #endif
575
576 // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
577 frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
578
579 #ifdef USE_ESC_SENSOR_TELEMETRY
580 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
581 // send ESC sensors
582 tableInfo = &frSkyEscDataIdTableInfo;
583 if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
584 tableInfo->index = 0;
585 smartPortIdCycleCnt = 0;
586 smartPortIdOffset++;
587 if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
588 smartPortIdOffset = 0;
589 }
590 }
591 }
592 if (smartPortIdCycleCnt < ESC_SENSOR_PERIOD) {
593 // send other sensors
594 tableInfo = &frSkyDataIdTableInfo;
595 #endif
596 if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
597 tableInfo->index = 0;
598 }
599 #ifdef USE_ESC_SENSOR_TELEMETRY
600 }
601 #endif
602 uint16_t id = tableInfo->table[tableInfo->index];
603 #ifdef USE_ESC_SENSOR_TELEMETRY
604 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
605 id += smartPortIdOffset;
606 }
607 #endif
608 smartPortIdCycleCnt++;
609 tableInfo->index++;
610
611 int32_t tmpi;
612 uint32_t tmp2 = 0;
613 uint16_t vfasVoltage;
614
615 #ifdef USE_ESC_SENSOR_TELEMETRY
616 escSensorData_t *escData;
617 #endif
618
619 switch (id) {
620 case FSSP_DATAID_VFAS :
621 vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
622 smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec
623 *clearToSend = false;
624 break;
625 #ifdef USE_ESC_SENSOR_TELEMETRY
626 case FSSP_DATAID_VFAS1 :
627 case FSSP_DATAID_VFAS2 :
628 case FSSP_DATAID_VFAS3 :
629 case FSSP_DATAID_VFAS4 :
630 case FSSP_DATAID_VFAS5 :
631 case FSSP_DATAID_VFAS6 :
632 case FSSP_DATAID_VFAS7 :
633 case FSSP_DATAID_VFAS8 :
634 escData = getEscSensorData(id - FSSP_DATAID_VFAS1);
635 if (escData != NULL) {
636 smartPortSendPackage(id, escData->voltage);
637 *clearToSend = false;
638 }
639 break;
640 #endif
641 case FSSP_DATAID_CURRENT :
642 smartPortSendPackage(id, getAmperage() / 10); // in 0.1A according to SmartPort spec
643 *clearToSend = false;
644 break;
645 #ifdef USE_ESC_SENSOR_TELEMETRY
646 case FSSP_DATAID_CURRENT1 :
647 case FSSP_DATAID_CURRENT2 :
648 case FSSP_DATAID_CURRENT3 :
649 case FSSP_DATAID_CURRENT4 :
650 case FSSP_DATAID_CURRENT5 :
651 case FSSP_DATAID_CURRENT6 :
652 case FSSP_DATAID_CURRENT7 :
653 case FSSP_DATAID_CURRENT8 :
654 escData = getEscSensorData(id - FSSP_DATAID_CURRENT1);
655 if (escData != NULL) {
656 smartPortSendPackage(id, escData->current);
657 *clearToSend = false;
658 }
659 break;
660 case FSSP_DATAID_RPM :
661 escData = getEscSensorData(ESC_SENSOR_COMBINED);
662 if (escData != NULL) {
663 smartPortSendPackage(id, lrintf(erpmToRpm(escData->rpm)));
664 *clearToSend = false;
665 }
666 break;
667 case FSSP_DATAID_RPM1 :
668 case FSSP_DATAID_RPM2 :
669 case FSSP_DATAID_RPM3 :
670 case FSSP_DATAID_RPM4 :
671 case FSSP_DATAID_RPM5 :
672 case FSSP_DATAID_RPM6 :
673 case FSSP_DATAID_RPM7 :
674 case FSSP_DATAID_RPM8 :
675 escData = getEscSensorData(id - FSSP_DATAID_RPM1);
676 if (escData != NULL) {
677 smartPortSendPackage(id, lrintf(erpmToRpm(escData->rpm)));
678 *clearToSend = false;
679 }
680 break;
681 case FSSP_DATAID_TEMP :
682 escData = getEscSensorData(ESC_SENSOR_COMBINED);
683 if (escData != NULL) {
684 smartPortSendPackage(id, escData->temperature);
685 *clearToSend = false;
686 }
687 break;
688 case FSSP_DATAID_TEMP1 :
689 case FSSP_DATAID_TEMP2 :
690 case FSSP_DATAID_TEMP3 :
691 case FSSP_DATAID_TEMP4 :
692 case FSSP_DATAID_TEMP5 :
693 case FSSP_DATAID_TEMP6 :
694 case FSSP_DATAID_TEMP7 :
695 case FSSP_DATAID_TEMP8 :
696 escData = getEscSensorData(id - FSSP_DATAID_TEMP1);
697 if (escData != NULL) {
698 smartPortSendPackage(id, escData->temperature);
699 *clearToSend = false;
700 }
701 break;
702 #endif
703 case FSSP_DATAID_ALTITUDE :
704 smartPortSendPackage(id, getEstimatedAltitudeCm()); // in cm according to SmartPort spec
705 *clearToSend = false;
706 break;
707 case FSSP_DATAID_FUEL :
708 {
709 uint32_t data;
710 if (batteryConfig()->batteryCapacity > 0) {
711 data = calculateBatteryPercentageRemaining();
712 } else {
713 data = getMAhDrawn();
714 }
715 smartPortSendPackage(id, data);
716 *clearToSend = false;
717 }
718 break;
719 case FSSP_DATAID_CAP_USED :
720 smartPortSendPackage(id, getMAhDrawn()); // given in mAh, should be in percent according to SmartPort spec
721 *clearToSend = false;
722 break;
723 #if defined(USE_VARIO)
724 case FSSP_DATAID_VARIO :
725 smartPortSendPackage(id, getEstimatedVario()); // in cm/s according to SmartPort spec
726 *clearToSend = false;
727 break;
728 #endif
729 case FSSP_DATAID_HEADING :
730 smartPortSendPackage(id, attitude.values.yaw * 10); // in degrees * 100 according to SmartPort spec
731 *clearToSend = false;
732 break;
733 #if defined(USE_ACC)
734 case FSSP_DATAID_PITCH :
735 smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
736 *clearToSend = false;
737 break;
738 case FSSP_DATAID_ROLL :
739 smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
740 *clearToSend = false;
741 break;
742 case FSSP_DATAID_ACCX :
743 smartPortSendPackage(id, lrintf(100 * acc.accADC.x * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
744 *clearToSend = false;
745 break;
746 case FSSP_DATAID_ACCY :
747 smartPortSendPackage(id, lrintf(100 * acc.accADC.y * acc.dev.acc_1G_rec));
748 *clearToSend = false;
749 break;
750 case FSSP_DATAID_ACCZ :
751 smartPortSendPackage(id, lrintf(100 * acc.accADC.z * acc.dev.acc_1G_rec));
752 *clearToSend = false;
753 break;
754 #endif
755 case FSSP_DATAID_T1 :
756 // we send all the flags as decimal digits for easy reading
757
758 // the t1Cnt simply allows the telemetry view to show at least some changes
759 t1Cnt++;
760 if (t1Cnt == 4) {
761 t1Cnt = 1;
762 }
763 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
764 // the Taranis seems to be able to fit 5 digits on the screen
765 // the Taranis seems to consider this number a signed 16 bit integer
766
767 if (!isArmingDisabled()) {
768 tmpi += 1;
769 } else {
770 tmpi += 2;
771 }
772 if (ARMING_FLAG(ARMED)) {
773 tmpi += 4;
774 }
775
776 if (FLIGHT_MODE(ANGLE_MODE | ALT_HOLD_MODE)) {
777 tmpi += 10;
778 }
779 if (FLIGHT_MODE(HORIZON_MODE)) {
780 tmpi += 20;
781 }
782 if (FLIGHT_MODE(PASSTHRU_MODE)) {
783 tmpi += 40;
784 }
785
786 if (FLIGHT_MODE(MAG_MODE)) {
787 tmpi += 100;
788 }
789
790 if (FLIGHT_MODE(HEADFREE_MODE)) {
791 tmpi += 4000;
792 }
793
794 smartPortSendPackage(id, (uint32_t)tmpi);
795 *clearToSend = false;
796 break;
797 case FSSP_DATAID_T2 :
798 #ifdef USE_GPS
799 if (sensors(SENSOR_GPS)) {
800 // satellite accuracy PDOP: 0 = worst [PDOP > 5.5m], 9 = best [PDOP <= 1.0m]
801 // the above comment isn't entirely right. DOP is accuracy relative to specified accuracy of the module, not a value in meters
802 // eg a value of 1.0 means 1.0 times specified accuracy (typically 2m)
803 uint16_t pdop = constrain(scaleRange(gpsSol.dop.pdop, 100, 550, 9, 0), 0, 9) * 100;
804 smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + pdop + gpsSol.numSat);
805 *clearToSend = false;
806 } else if (featureIsEnabled(FEATURE_GPS)) {
807 smartPortSendPackage(id, 0);
808 *clearToSend = false;
809 } else
810 #endif
811 if (telemetryConfig()->pidValuesAsTelemetry) {
812 switch (t2Cnt) {
813 case 0:
814 tmp2 = currentPidProfile->pid[PID_ROLL].P;
815 tmp2 += (currentPidProfile->pid[PID_PITCH].P<<8);
816 tmp2 += (currentPidProfile->pid[PID_YAW].P<<16);
817 break;
818 case 1:
819 tmp2 = currentPidProfile->pid[PID_ROLL].I;
820 tmp2 += (currentPidProfile->pid[PID_PITCH].I<<8);
821 tmp2 += (currentPidProfile->pid[PID_YAW].I<<16);
822 break;
823 case 2:
824 tmp2 = currentPidProfile->pid[PID_ROLL].D;
825 tmp2 += (currentPidProfile->pid[PID_PITCH].D<<8);
826 tmp2 += (currentPidProfile->pid[PID_YAW].D<<16);
827 break;
828 case 3:
829 tmp2 = currentControlRateProfile->rates[FD_ROLL];
830 tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8);
831 tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16);
832 break;
833 }
834 tmp2 += t2Cnt<<24;
835 t2Cnt++;
836 if (t2Cnt == 4) {
837 t2Cnt = 0;
838 }
839 smartPortSendPackage(id, tmp2);
840 *clearToSend = false;
841 }
842
843 break;
844 #if defined(USE_ADC_INTERNAL)
845 case FSSP_DATAID_T11 :
846 smartPortSendPackage(id, getCoreTemperatureCelsius());
847 *clearToSend = false;
848 break;
849 #endif
850 #ifdef USE_GPS
851 case FSSP_DATAID_SPEED :
852 if (STATE(GPS_FIX)) {
853 //convert to knots: 1cm/s = 0.0194384449 knots
854 //Speed should be sent in knots/1000 (GPS speed is in cm/s)
855 uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
856 smartPortSendPackage(id, tmpui);
857 *clearToSend = false;
858 }
859 break;
860 case FSSP_DATAID_LATLONG :
861 if (STATE(GPS_FIX)) {
862 uint32_t tmpui = 0;
863 // the same ID is sent twice, one for longitude, one for latitude
864 // the MSB of the sent uint32_t helps FrSky keep track
865 // the even/odd bit of our counter helps us keep track
866 if (tableInfo->index & 1) {
867 tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
868 tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
869 if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
870 }
871 else {
872 tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
873 tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
874 if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
875 }
876 smartPortSendPackage(id, tmpui);
877 *clearToSend = false;
878 }
879 break;
880 case FSSP_DATAID_HOME_DIST :
881 if (STATE(GPS_FIX)) {
882 smartPortSendPackage(id, GPS_distanceToHome);
883 *clearToSend = false;
884 }
885 break;
886 case FSSP_DATAID_GPS_ALT :
887 if (STATE(GPS_FIX)) {
888 smartPortSendPackage(id, gpsSol.llh.altCm); // in cm according to SmartPort spec
889 *clearToSend = false;
890 }
891 break;
892 #endif
893 case FSSP_DATAID_A4 :
894 vfasVoltage = getBatteryAverageCellVoltage(); // in 0.01V according to SmartPort spec
895 smartPortSendPackage(id, vfasVoltage);
896 *clearToSend = false;
897 break;
898 default:
899 break;
900 // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
901 }
902 }
903 }
904
905 static bool serialReadyToSend(void)
906 {
907 return (serialRxBytesWaiting(smartPortSerialPort) == 0);
908 }
909
910 void handleSmartPortTelemetry(void)
911 {
912 const timeUs_t requestTimeout = micros() + SMARTPORT_SERVICE_TIMEOUT_US;
913
914 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
915 smartPortPayload_t *payload = NULL;
916 bool clearToSend = false;
917 while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
918 uint8_t c = serialRead(smartPortSerialPort);
919 payload = smartPortDataReceive(c, &clearToSend, serialReadyToSend, true);
920 }
921
922 processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
923 }
924 }
925 #endif