?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge pull request #11299 from daleckystepan/vtx-start-bit
[betaflight.git] / src / main / telemetry / smartport.c
blob16b2e8ed310471c2f423de0a9eaf1b53dbc4e46b
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/serial.h"
63
64 #include "msp/msp.h"
65
66 #include "rx/rx.h"
67
68 #include "pg/pg.h"
69 #include "pg/pg_ids.h"
70 #include "pg/rx.h"
71
72 #include "sensors/acceleration.h"
73 #include "sensors/adcinternal.h"
74 #include "sensors/barometer.h"
75 #include "sensors/battery.h"
76 #include "sensors/boardalignment.h"
77 #include "sensors/compass.h"
78 #include "sensors/esc_sensor.h"
79 #include "sensors/gyro.h"
80 #include "sensors/sensors.h"
81
82 #include "telemetry/msp_shared.h"
83 #include "telemetry/smartport.h"
84 #include "telemetry/telemetry.h"
85
86 #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
87
88 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
89 enum
90 {
91 FSSP_DATAID_SPEED = 0x0830 ,
92 FSSP_DATAID_VFAS = 0x0210 ,
93 FSSP_DATAID_VFAS1 = 0x0211 ,
94 FSSP_DATAID_VFAS2 = 0x0212 ,
95 FSSP_DATAID_VFAS3 = 0x0213 ,
96 FSSP_DATAID_VFAS4 = 0x0214 ,
97 FSSP_DATAID_VFAS5 = 0x0215 ,
98 FSSP_DATAID_VFAS6 = 0x0216 ,
99 FSSP_DATAID_VFAS7 = 0x0217 ,
100 FSSP_DATAID_VFAS8 = 0x0218 ,
101 FSSP_DATAID_CURRENT = 0x0200 ,
102 FSSP_DATAID_CURRENT1 = 0x0201 ,
103 FSSP_DATAID_CURRENT2 = 0x0202 ,
104 FSSP_DATAID_CURRENT3 = 0x0203 ,
105 FSSP_DATAID_CURRENT4 = 0x0204 ,
106 FSSP_DATAID_CURRENT5 = 0x0205 ,
107 FSSP_DATAID_CURRENT6 = 0x0206 ,
108 FSSP_DATAID_CURRENT7 = 0x0207 ,
109 FSSP_DATAID_CURRENT8 = 0x0208 ,
110 FSSP_DATAID_RPM = 0x0500 ,
111 FSSP_DATAID_RPM1 = 0x0501 ,
112 FSSP_DATAID_RPM2 = 0x0502 ,
113 FSSP_DATAID_RPM3 = 0x0503 ,
114 FSSP_DATAID_RPM4 = 0x0504 ,
115 FSSP_DATAID_RPM5 = 0x0505 ,
116 FSSP_DATAID_RPM6 = 0x0506 ,
117 FSSP_DATAID_RPM7 = 0x0507 ,
118 FSSP_DATAID_RPM8 = 0x0508 ,
119 FSSP_DATAID_ALTITUDE = 0x0100 ,
120 FSSP_DATAID_FUEL = 0x0600 ,
121 FSSP_DATAID_ADC1 = 0xF102 ,
122 FSSP_DATAID_ADC2 = 0xF103 ,
123 FSSP_DATAID_LATLONG = 0x0800 ,
124 FSSP_DATAID_VARIO = 0x0110 ,
125 FSSP_DATAID_CELLS = 0x0300 ,
126 FSSP_DATAID_CELLS_LAST = 0x030F ,
127 FSSP_DATAID_HEADING = 0x0840 ,
128 // DIY range 0x5100 to 0x52FF
129 FSSP_DATAID_CAP_USED = 0x5250 ,
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 if (telemetryIsSensorEnabled(SENSOR_CAP_USED)) {
366 ADD_SENSOR(FSSP_DATAID_CAP_USED);
367 }
368 }
369
370 if (telemetryIsSensorEnabled(SENSOR_HEADING)) {
371 ADD_SENSOR(FSSP_DATAID_HEADING);
372 }
373
374 #if defined(USE_ACC)
375 if (sensors(SENSOR_ACC)) {
376 if (telemetryIsSensorEnabled(SENSOR_PITCH)) {
377 ADD_SENSOR(FSSP_DATAID_PITCH);
378 }
379 if (telemetryIsSensorEnabled(SENSOR_ROLL)) {
380 ADD_SENSOR(FSSP_DATAID_ROLL);
381 }
382 if (telemetryIsSensorEnabled(SENSOR_ACC_X)) {
383 ADD_SENSOR(FSSP_DATAID_ACCX);
384 }
385 if (telemetryIsSensorEnabled(SENSOR_ACC_Y)) {
386 ADD_SENSOR(FSSP_DATAID_ACCY);
387 }
388 if (telemetryIsSensorEnabled(SENSOR_ACC_Z)) {
389 ADD_SENSOR(FSSP_DATAID_ACCZ);
390 }
391 }
392 #endif
393
394 if (sensors(SENSOR_BARO)) {
395 if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
396 ADD_SENSOR(FSSP_DATAID_ALTITUDE);
397 }
398 if (telemetryIsSensorEnabled(SENSOR_VARIO)) {
399 ADD_SENSOR(FSSP_DATAID_VARIO);
400 }
401 }
402
403 #ifdef USE_GPS
404 if (featureIsEnabled(FEATURE_GPS)) {
405 if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) {
406 ADD_SENSOR(FSSP_DATAID_SPEED);
407 }
408 if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) {
409 ADD_SENSOR(FSSP_DATAID_LATLONG);
410 ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long)
411 }
412 if (telemetryIsSensorEnabled(SENSOR_DISTANCE)) {
413 ADD_SENSOR(FSSP_DATAID_HOME_DIST);
414 }
415 if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
416 ADD_SENSOR(FSSP_DATAID_GPS_ALT);
417 }
418 }
419 #endif
420
421 frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index;
422 frSkyDataIdTableInfo.index = 0;
423
424 #ifdef USE_ESC_SENSOR_TELEMETRY
425 frSkyEscDataIdTableInfo.index = 0;
426
427 if (telemetryIsSensorEnabled(ESC_SENSOR_VOLTAGE)) {
428 ADD_ESC_SENSOR(FSSP_DATAID_VFAS);
429 }
430 if (telemetryIsSensorEnabled(ESC_SENSOR_CURRENT)) {
431 ADD_ESC_SENSOR(FSSP_DATAID_CURRENT);
432 }
433 if (telemetryIsSensorEnabled(ESC_SENSOR_RPM)) {
434 ADD_ESC_SENSOR(FSSP_DATAID_RPM);
435 }
436 if (telemetryIsSensorEnabled(ESC_SENSOR_TEMPERATURE)) {
437 ADD_ESC_SENSOR(FSSP_DATAID_TEMP);
438 }
439
440 frSkyEscDataIdTableInfo.size = frSkyEscDataIdTableInfo.index;
441 frSkyEscDataIdTableInfo.index = 0;
442 #endif
443 }
444
445 bool initSmartPortTelemetry(void)
446 {
447 if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
448 portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
449 if (portConfig) {
450 smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
451
452 smartPortWriteFrame = smartPortWriteFrameInternal;
453
454 initSmartPortSensors();
455
456 telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
457 }
458
459 return true;
460 }
461
462 return false;
463 }
464
465 bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
466 {
467 if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
468 smartPortWriteFrame = smartPortWriteFrameExternal;
469
470 initSmartPortSensors();
471
472 telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
473
474 return true;
475 }
476
477 return false;
478 }
479
480 static void freeSmartPortTelemetryPort(void)
481 {
482 closeSerialPort(smartPortSerialPort);
483 smartPortSerialPort = NULL;
484 }
485
486 static void configureSmartPortTelemetryPort(void)
487 {
488 if (portConfig) {
489 portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
490
491 smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
492 }
493 }
494
495 void checkSmartPortTelemetryState(void)
496 {
497 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
498 bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
499
500 if (enableSerialTelemetry && !smartPortSerialPort) {
501 configureSmartPortTelemetryPort();
502 } else if (!enableSerialTelemetry && smartPortSerialPort) {
503 freeSmartPortTelemetryPort();
504 }
505 }
506 }
507
508 #if defined(USE_MSP_OVER_TELEMETRY)
509 static void smartPortSendMspResponse(uint8_t *data, const uint8_t dataSize) {
510 smartPortPayload_t payload;
511 payload.frameId = FSSP_MSPS_FRAME;
512 memcpy(&payload.valueId, data, MIN(dataSize,SMARTPORT_MSP_PAYLOAD_SIZE));
513
514 smartPortWriteFrame(&payload);
515 }
516 #endif
517
518 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const timeUs_t *requestTimeout)
519 {
520 static uint8_t smartPortIdCycleCnt = 0;
521 static uint8_t t1Cnt = 0;
522 static uint8_t t2Cnt = 0;
523 static uint8_t skipRequests = 0;
524 #ifdef USE_ESC_SENSOR_TELEMETRY
525 static uint8_t smartPortIdOffset = 0;
526 #endif
527
528 #if defined(USE_MSP_OVER_TELEMETRY)
529 if (skipRequests) {
530 skipRequests--;
531 } else if (payload && smartPortPayloadContainsMSP(payload)) {
532 // Do not check the physical ID here again
533 // unless we start receiving other sensors' packets
534 // Pass only the payload: skip frameId
535 uint8_t *frameStart = (uint8_t *)&payload->valueId;
536 smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
537
538 // Don't send MSP response after write to eeprom
539 // CPU just got out of suspended state after writeEEPROM()
540 // We don't know if the receiver is listening again
541 // Skip a few telemetry requests before sending response
542 if (skipRequests) {
543 *clearToSend = false;
544 }
545 }
546 #else
547 UNUSED(payload);
548 #endif
549
550 bool doRun = true;
551 while (doRun && *clearToSend && !skipRequests) {
552 // 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.
553 if (requestTimeout) {
554 if (cmpTimeUs(micros(), *requestTimeout) >= 0) {
555 *clearToSend = false;
556
557 return;
558 }
559 } else {
560 doRun = false;
561 }
562
563 #if defined(USE_MSP_OVER_TELEMETRY)
564 if (smartPortMspReplyPending) {
565 smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
566 *clearToSend = false;
567
568 return;
569 }
570 #endif
571
572 // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
573 frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
574
575 #ifdef USE_ESC_SENSOR_TELEMETRY
576 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
577 // send ESC sensors
578 tableInfo = &frSkyEscDataIdTableInfo;
579 if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
580 tableInfo->index = 0;
581 smartPortIdCycleCnt = 0;
582 smartPortIdOffset++;
583 if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
584 smartPortIdOffset = 0;
585 }
586 }
587 }
588 if (smartPortIdCycleCnt < ESC_SENSOR_PERIOD) {
589 // send other sensors
590 tableInfo = &frSkyDataIdTableInfo;
591 #endif
592 if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
593 tableInfo->index = 0;
594 }
595 #ifdef USE_ESC_SENSOR_TELEMETRY
596 }
597 #endif
598 uint16_t id = tableInfo->table[tableInfo->index];
599 #ifdef USE_ESC_SENSOR_TELEMETRY
600 if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
601 id += smartPortIdOffset;
602 }
603 #endif
604 smartPortIdCycleCnt++;
605 tableInfo->index++;
606
607 int32_t tmpi;
608 uint32_t tmp2 = 0;
609 uint16_t vfasVoltage;
610
611 #ifdef USE_ESC_SENSOR_TELEMETRY
612 escSensorData_t *escData;
613 #endif
614
615 switch (id) {
616 case FSSP_DATAID_VFAS :
617 vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
618 smartPortSendPackage(id, vfasVoltage); // in 0.01V according to SmartPort spec
619 *clearToSend = false;
620 break;
621 #ifdef USE_ESC_SENSOR_TELEMETRY
622 case FSSP_DATAID_VFAS1 :
623 case FSSP_DATAID_VFAS2 :
624 case FSSP_DATAID_VFAS3 :
625 case FSSP_DATAID_VFAS4 :
626 case FSSP_DATAID_VFAS5 :
627 case FSSP_DATAID_VFAS6 :
628 case FSSP_DATAID_VFAS7 :
629 case FSSP_DATAID_VFAS8 :
630 escData = getEscSensorData(id - FSSP_DATAID_VFAS1);
631 if (escData != NULL) {
632 smartPortSendPackage(id, escData->voltage);
633 *clearToSend = false;
634 }
635 break;
636 #endif
637 case FSSP_DATAID_CURRENT :
638 smartPortSendPackage(id, getAmperage() / 10); // in 0.1A according to SmartPort spec
639 *clearToSend = false;
640 break;
641 #ifdef USE_ESC_SENSOR_TELEMETRY
642 case FSSP_DATAID_CURRENT1 :
643 case FSSP_DATAID_CURRENT2 :
644 case FSSP_DATAID_CURRENT3 :
645 case FSSP_DATAID_CURRENT4 :
646 case FSSP_DATAID_CURRENT5 :
647 case FSSP_DATAID_CURRENT6 :
648 case FSSP_DATAID_CURRENT7 :
649 case FSSP_DATAID_CURRENT8 :
650 escData = getEscSensorData(id - FSSP_DATAID_CURRENT1);
651 if (escData != NULL) {
652 smartPortSendPackage(id, escData->current);
653 *clearToSend = false;
654 }
655 break;
656 case FSSP_DATAID_RPM :
657 escData = getEscSensorData(ESC_SENSOR_COMBINED);
658 if (escData != NULL) {
659 smartPortSendPackage(id, calcEscRpm(escData->rpm));
660 *clearToSend = false;
661 }
662 break;
663 case FSSP_DATAID_RPM1 :
664 case FSSP_DATAID_RPM2 :
665 case FSSP_DATAID_RPM3 :
666 case FSSP_DATAID_RPM4 :
667 case FSSP_DATAID_RPM5 :
668 case FSSP_DATAID_RPM6 :
669 case FSSP_DATAID_RPM7 :
670 case FSSP_DATAID_RPM8 :
671 escData = getEscSensorData(id - FSSP_DATAID_RPM1);
672 if (escData != NULL) {
673 smartPortSendPackage(id, calcEscRpm(escData->rpm));
674 *clearToSend = false;
675 }
676 break;
677 case FSSP_DATAID_TEMP :
678 escData = getEscSensorData(ESC_SENSOR_COMBINED);
679 if (escData != NULL) {
680 smartPortSendPackage(id, escData->temperature);
681 *clearToSend = false;
682 }
683 break;
684 case FSSP_DATAID_TEMP1 :
685 case FSSP_DATAID_TEMP2 :
686 case FSSP_DATAID_TEMP3 :
687 case FSSP_DATAID_TEMP4 :
688 case FSSP_DATAID_TEMP5 :
689 case FSSP_DATAID_TEMP6 :
690 case FSSP_DATAID_TEMP7 :
691 case FSSP_DATAID_TEMP8 :
692 escData = getEscSensorData(id - FSSP_DATAID_TEMP1);
693 if (escData != NULL) {
694 smartPortSendPackage(id, escData->temperature);
695 *clearToSend = false;
696 }
697 break;
698 #endif
699 case FSSP_DATAID_ALTITUDE :
700 smartPortSendPackage(id, getEstimatedAltitudeCm()); // in cm according to SmartPort spec
701 *clearToSend = false;
702 break;
703 case FSSP_DATAID_FUEL :
704 {
705 uint32_t data;
706 if (batteryConfig()->batteryCapacity > 0) {
707 data = calculateBatteryPercentageRemaining();
708 } else {
709 data = getMAhDrawn();
710 }
711 smartPortSendPackage(id, data);
712 *clearToSend = false;
713 }
714 break;
715 case FSSP_DATAID_CAP_USED :
716 smartPortSendPackage(id, getMAhDrawn()); // given in mAh, should be in percent according to SmartPort spec
717 *clearToSend = false;
718 break;
719 #if defined(USE_VARIO)
720 case FSSP_DATAID_VARIO :
721 smartPortSendPackage(id, getEstimatedVario()); // in cm/s according to SmartPort spec
722 *clearToSend = false;
723 break;
724 #endif
725 case FSSP_DATAID_HEADING :
726 smartPortSendPackage(id, attitude.values.yaw * 10); // in degrees * 100 according to SmartPort spec
727 *clearToSend = false;
728 break;
729 #if defined(USE_ACC)
730 case FSSP_DATAID_PITCH :
731 smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
732 *clearToSend = false;
733 break;
734 case FSSP_DATAID_ROLL :
735 smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
736 *clearToSend = false;
737 break;
738 case FSSP_DATAID_ACCX :
739 smartPortSendPackage(id, lrintf(100 * acc.accADC[X] * acc.dev.acc_1G_rec)); // Multiply by 100 to show as x.xx g on Taranis
740 *clearToSend = false;
741 break;
742 case FSSP_DATAID_ACCY :
743 smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] * acc.dev.acc_1G_rec));
744 *clearToSend = false;
745 break;
746 case FSSP_DATAID_ACCZ :
747 smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] * acc.dev.acc_1G_rec));
748 *clearToSend = false;
749 break;
750 #endif
751 case FSSP_DATAID_T1 :
752 // we send all the flags as decimal digits for easy reading
753
754 // the t1Cnt simply allows the telemetry view to show at least some changes
755 t1Cnt++;
756 if (t1Cnt == 4) {
757 t1Cnt = 1;
758 }
759 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
760 // the Taranis seems to be able to fit 5 digits on the screen
761 // the Taranis seems to consider this number a signed 16 bit integer
762
763 if (!isArmingDisabled()) {
764 tmpi += 1;
765 } else {
766 tmpi += 2;
767 }
768 if (ARMING_FLAG(ARMED)) {
769 tmpi += 4;
770 }
771
772 if (FLIGHT_MODE(ANGLE_MODE)) {
773 tmpi += 10;
774 }
775 if (FLIGHT_MODE(HORIZON_MODE)) {
776 tmpi += 20;
777 }
778 if (FLIGHT_MODE(PASSTHRU_MODE)) {
779 tmpi += 40;
780 }
781
782 if (FLIGHT_MODE(MAG_MODE)) {
783 tmpi += 100;
784 }
785
786 if (FLIGHT_MODE(HEADFREE_MODE)) {
787 tmpi += 4000;
788 }
789
790 smartPortSendPackage(id, (uint32_t)tmpi);
791 *clearToSend = false;
792 break;
793 case FSSP_DATAID_T2 :
794 #ifdef USE_GPS
795 if (sensors(SENSOR_GPS)) {
796 // satellite accuracy HDOP: 0 = worst [HDOP > 5.5m], 9 = best [HDOP <= 1.0m]
797 uint16_t hdop = constrain(scaleRange(gpsSol.hdop, 100, 550, 9, 0), 0, 9) * 100;
798 smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + hdop + gpsSol.numSat);
799 *clearToSend = false;
800 } else if (featureIsEnabled(FEATURE_GPS)) {
801 smartPortSendPackage(id, 0);
802 *clearToSend = false;
803 } else
804 #endif
805 if (telemetryConfig()->pidValuesAsTelemetry) {
806 switch (t2Cnt) {
807 case 0:
808 tmp2 = currentPidProfile->pid[PID_ROLL].P;
809 tmp2 += (currentPidProfile->pid[PID_PITCH].P<<8);
810 tmp2 += (currentPidProfile->pid[PID_YAW].P<<16);
811 break;
812 case 1:
813 tmp2 = currentPidProfile->pid[PID_ROLL].I;
814 tmp2 += (currentPidProfile->pid[PID_PITCH].I<<8);
815 tmp2 += (currentPidProfile->pid[PID_YAW].I<<16);
816 break;
817 case 2:
818 tmp2 = currentPidProfile->pid[PID_ROLL].D;
819 tmp2 += (currentPidProfile->pid[PID_PITCH].D<<8);
820 tmp2 += (currentPidProfile->pid[PID_YAW].D<<16);
821 break;
822 case 3:
823 tmp2 = currentControlRateProfile->rates[FD_ROLL];
824 tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8);
825 tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16);
826 break;
827 }
828 tmp2 += t2Cnt<<24;
829 t2Cnt++;
830 if (t2Cnt == 4) {
831 t2Cnt = 0;
832 }
833 smartPortSendPackage(id, tmp2);
834 *clearToSend = false;
835 }
836
837 break;
838 #if defined(USE_ADC_INTERNAL)
839 case FSSP_DATAID_T11 :
840 smartPortSendPackage(id, getCoreTemperatureCelsius());
841 *clearToSend = false;
842 break;
843 #endif
844 #ifdef USE_GPS
845 case FSSP_DATAID_SPEED :
846 if (STATE(GPS_FIX)) {
847 //convert to knots: 1cm/s = 0.0194384449 knots
848 //Speed should be sent in knots/1000 (GPS speed is in cm/s)
849 uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
850 smartPortSendPackage(id, tmpui);
851 *clearToSend = false;
852 }
853 break;
854 case FSSP_DATAID_LATLONG :
855 if (STATE(GPS_FIX)) {
856 uint32_t tmpui = 0;
857 // the same ID is sent twice, one for longitude, one for latitude
858 // the MSB of the sent uint32_t helps FrSky keep track
859 // the even/odd bit of our counter helps us keep track
860 if (tableInfo->index & 1) {
861 tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
862 tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
863 if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
864 }
865 else {
866 tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
867 tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
868 if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
869 }
870 smartPortSendPackage(id, tmpui);
871 *clearToSend = false;
872 }
873 break;
874 case FSSP_DATAID_HOME_DIST :
875 if (STATE(GPS_FIX)) {
876 smartPortSendPackage(id, GPS_distanceToHome);
877 *clearToSend = false;
878 }
879 break;
880 case FSSP_DATAID_GPS_ALT :
881 if (STATE(GPS_FIX)) {
882 smartPortSendPackage(id, gpsSol.llh.altCm); // in cm according to SmartPort spec
883 *clearToSend = false;
884 }
885 break;
886 #endif
887 case FSSP_DATAID_A4 :
888 vfasVoltage = getBatteryAverageCellVoltage(); // in 0.01V according to SmartPort spec
889 smartPortSendPackage(id, vfasVoltage);
890 *clearToSend = false;
891 break;
892 default:
893 break;
894 // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
895 }
896 }
897 }
898
899 static bool serialReadyToSend(void)
900 {
901 return (serialRxBytesWaiting(smartPortSerialPort) == 0);
902 }
903
904 void handleSmartPortTelemetry(void)
905 {
906 const timeUs_t requestTimeout = micros() + SMARTPORT_SERVICE_TIMEOUT_US;
907
908 if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
909 smartPortPayload_t *payload = NULL;
910 bool clearToSend = false;
911 while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
912 uint8_t c = serialRead(smartPortSerialPort);
913 payload = smartPortDataReceive(c, &clearToSend, serialReadyToSend, true);
914 }
915
916 processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
917 }
918 }
919 #endif