Paparazzi UAS  v5.17_devel-24-g2ae834f
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
cc2500_smartport.c
Go to the documentation of this file.
1 #include "cc2500_compat.h"
2 
3 #include "cc2500_rx.h"
4 #include "cc2500_settings.h"
5 #include "cc2500_smartport.h"
6 
8 
9 #ifdef UNUSED
10 #undef UNUSED
11 #define UNUSED(x) (void)(x)
12 #endif
13 
14 #ifdef USE_GPS
15 #undef USE_GPS
16 #endif
17 
18 //#define FSSP_DATAID_DOWNLINK 0x0B71 // DEBUG
19 #define FSSP_DATAID_DOWNLINK 0x5015
20 #define FSSP_DATAID_UPLINK 0x5016
21 
24 
25 // CAUTION: added DOWNLINK sensor to code below!
26 // CAUTION: added payload handling to processSmartPortTelemetry
27 // CAUTION: LARGE PARTS OF THIS FILE ARE COMMENTED OUT!
28 // betaflight/src/main/telemetry/smartport.c @ 443869e
29 /*
30  * This file is part of Cleanflight and Betaflight.
31  *
32  * Cleanflight and Betaflight are free software. You can redistribute
33  * this software and/or modify this software under the terms of the
34  * GNU General Public License as published by the Free Software
35  * Foundation, either version 3 of the License, or (at your option)
36  * any later version.
37  *
38  * Cleanflight and Betaflight are distributed in the hope that they
39  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
40  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
41  * See the GNU General Public License for more details.
42  *
43  * You should have received a copy of the GNU General Public License
44  * along with this software.
45  *
46  * If not, see <http://www.gnu.org/licenses/>.
47  */
48 
49 /*
50  * SmartPort Telemetry implementation by frank26080115
51  * see https://github.com/frank26080115/cleanflight/wiki/Using-Smart-Port
52  */
53 #include <stdbool.h>
54 #include <stdint.h>
55 #include <stdlib.h>
56 #include <string.h>
57 #include <math.h>
58 
59 //#include "platform.h"
60 //
61 //#if defined(USE_TELEMETRY_SMARTPORT)
62 //
63 //#include "common/axis.h"
64 //#include "common/color.h"
65 //#include "common/maths.h"
66 //#include "common/utils.h"
67 //
68 //#include "config/feature.h"
69 //
70 //#include "drivers/accgyro/accgyro.h"
71 //#include "drivers/compass/compass.h"
72 //#include "drivers/sensor.h"
73 //#include "drivers/time.h"
74 //
75 //#include "fc/config.h"
76 //#include "fc/controlrate_profile.h"
77 //#include "fc/rc_controls.h"
78 //#include "fc/runtime_config.h"
79 //
80 //#include "flight/failsafe.h"
81 //#include "flight/imu.h"
82 //#include "flight/mixer.h"
83 //#include "flight/pid.h"
84 //#include "flight/position.h"
85 //
86 //#include "io/beeper.h"
87 //#include "io/gps.h"
88 //#include "io/motors.h"
89 //#include "io/serial.h"
90 //
91 //#include "msp/msp.h"
92 //
93 //#include "rx/rx.h"
94 //
95 //#include "pg/pg.h"
96 //#include "pg/pg_ids.h"
97 //#include "pg/rx.h"
98 //
99 //#include "sensors/acceleration.h"
100 //#include "sensors/adcinternal.h"
101 //#include "sensors/barometer.h"
102 //#include "sensors/battery.h"
103 //#include "sensors/boardalignment.h"
104 //#include "sensors/compass.h"
105 //#include "sensors/esc_sensor.h"
106 //#include "sensors/gyro.h"
107 //#include "sensors/sensors.h"
108 //
109 //#include "telemetry/msp_shared.h"
110 //#include "telemetry/smartport.h"
111 //#include "telemetry/telemetry.h"
112 //
113 //#define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
114 
115 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
116 enum
117 {
118  FSSP_DATAID_SPEED = 0x0830 ,
119  FSSP_DATAID_VFAS = 0x0210 ,
120  FSSP_DATAID_VFAS1 = 0x0211 ,
121  FSSP_DATAID_VFAS2 = 0x0212 ,
122  FSSP_DATAID_VFAS3 = 0x0213 ,
123  FSSP_DATAID_VFAS4 = 0x0214 ,
124  FSSP_DATAID_VFAS5 = 0x0215 ,
125  FSSP_DATAID_VFAS6 = 0x0216 ,
126  FSSP_DATAID_VFAS7 = 0x0217 ,
127  FSSP_DATAID_VFAS8 = 0x0218 ,
137  FSSP_DATAID_RPM = 0x0500 ,
138  FSSP_DATAID_RPM1 = 0x0501 ,
139  FSSP_DATAID_RPM2 = 0x0502 ,
140  FSSP_DATAID_RPM3 = 0x0503 ,
141  FSSP_DATAID_RPM4 = 0x0504 ,
142  FSSP_DATAID_RPM5 = 0x0505 ,
143  FSSP_DATAID_RPM6 = 0x0506 ,
144  FSSP_DATAID_RPM7 = 0x0507 ,
145  FSSP_DATAID_RPM8 = 0x0508 ,
147  FSSP_DATAID_FUEL = 0x0600 ,
148  FSSP_DATAID_ADC1 = 0xF102 ,
149  FSSP_DATAID_ADC2 = 0xF103 ,
152  FSSP_DATAID_VARIO = 0x0110 ,
153  FSSP_DATAID_CELLS = 0x0300 ,
156 #if defined(USE_ACC)
157  FSSP_DATAID_ACCX = 0x0700 ,
158  FSSP_DATAID_ACCY = 0x0710 ,
159  FSSP_DATAID_ACCZ = 0x0720 ,
160 #endif
161  FSSP_DATAID_T1 = 0x0400 ,
162  FSSP_DATAID_T11 = 0x0401 ,
163  FSSP_DATAID_T2 = 0x0410 ,
166  FSSP_DATAID_ASPD = 0x0A00 ,
167  FSSP_DATAID_TEMP = 0x0B70 ,
168  FSSP_DATAID_TEMP1 = 0x0B71 ,
169  FSSP_DATAID_TEMP2 = 0x0B72 ,
170  FSSP_DATAID_TEMP3 = 0x0B73 ,
171  FSSP_DATAID_TEMP4 = 0x0B74 ,
172  FSSP_DATAID_TEMP5 = 0x0B75 ,
173  FSSP_DATAID_TEMP6 = 0x0B76 ,
174  FSSP_DATAID_TEMP7 = 0x0B77 ,
175  FSSP_DATAID_TEMP8 = 0x0B78 ,
176  FSSP_DATAID_A3 = 0x0900 ,
177  FSSP_DATAID_A4 = 0x0910
178 };
179 
180 // if adding more sensors then increase this value
181 #define MAX_DATAIDS 18
182 
184 
185 //#ifdef USE_ESC_SENSOR_TELEMETRY
187 //#define ESC_SENSOR_PERIOD 7
188 //
190 //#define MAX_ESC_DATAIDS 4
191 //
192 //static uint16_t frSkyEscDataIdTable[MAX_ESC_DATAIDS];
193 //#endif
194 
195 typedef struct frSkyTableInfo_s {
200 
202 //#ifdef USE_ESC_SENSOR_TELEMETRY
203 //static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, 0, 0};
204 //#endif
205 //
206 //#define SMARTPORT_BAUD 57600
207 //#define SMARTPORT_UART_MODE MODE_RXTX
208 //#define SMARTPORT_SERVICE_TIMEOUT_US 1000 // max allowed time to find a value to send
209 //
210 //static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
211 //static serialPortConfig_t *portConfig;
212 //
213 //static portSharing_e smartPortPortSharing;
214 
215 enum
216 {
220 };
221 
223 
224 //typedef struct smartPortFrame_s {
225 // uint8_t sensorId;
226 // smartPortPayload_t payload;
227 // uint8_t crc;
228 //} __attribute__((packed)) smartPortFrame_t;
229 //
230 //#define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
231 
233 
234 //#if defined(USE_MSP_OVER_TELEMETRY)
235 //static bool smartPortMspReplyPending = false;
236 //#endif
237 
238 smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
239 {
240  static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
241  static uint8_t smartPortRxBytes = 0;
242  static bool skipUntilStart = true;
243  static bool awaitingSensorId = false;
244  static bool byteStuffing = false;
245  static uint16_t checksum = 0;
246 
247  if (c == FSSP_START_STOP) {
248  *clearToSend = false;
249  smartPortRxBytes = 0;
250  awaitingSensorId = true;
251  skipUntilStart = false;
252 
253  return NULL;
254  } else if (skipUntilStart) {
255  return NULL;
256  }
257 
258  if (awaitingSensorId) {
259  awaitingSensorId = false;
260  if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
261  // our slot is starting, no need to decode more
262  *clearToSend = true;
263  skipUntilStart = true;
264  } else if (c == FSSP_SENSOR_ID2) {
265  checksum = 0;
266  } else {
267  skipUntilStart = true;
268  }
269  } else {
270  if (c == FSSP_DLE) {
271  byteStuffing = true;
272 
273  return NULL;
274  } else if (byteStuffing) {
275  c ^= FSSP_DLE_XOR;
276  byteStuffing = false;
277  }
278 
279  if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
280  rxBuffer[smartPortRxBytes++] = (uint8_t)c;
281  checksum += c;
282 
283  if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
284  skipUntilStart = true;
285 
286  return (smartPortPayload_t *)&rxBuffer;
287  }
288  } else {
289  skipUntilStart = true;
290 
291  checksum += c;
292  checksum = (checksum & 0xFF) + (checksum >> 8);
293  if (checksum == 0xFF) {
294  return (smartPortPayload_t *)&rxBuffer;
295  }
296  }
297  }
298 
299  return NULL;
300 }
301 
302 //void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
303 //{
304 // // smart port escape sequence
305 // if (c == FSSP_DLE || c == FSSP_START_STOP) {
306 // serialWrite(port, FSSP_DLE);
307 // serialWrite(port, c ^ FSSP_DLE_XOR);
308 // } else {
309 // serialWrite(port, c);
310 // }
311 //
312 // if (checksum != NULL) {
313 // *checksum += c;
314 // }
315 //}
316 //
317 //bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
318 //{
319 // return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
320 //}
321 //
322 //void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
323 //{
324 // uint8_t *data = (uint8_t *)payload;
325 // for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
326 // smartPortSendByte(*data++, &checksum, port);
327 // }
328 // checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
329 // smartPortSendByte((uint8_t)checksum, NULL, port);
330 //}
331 //
332 //static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
333 //{
334 // smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
335 //}
336 
338 {
339  smartPortPayload_t payload;
340  payload.frameId = FSSP_DATA_FRAME;
341  payload.valueId = id;
342  payload.data = val;
343 
344  smartPortWriteFrame(&payload);
345 }
346 
347 #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId
348 #define ADD_ESC_SENSOR(dataId) frSkyEscDataIdTableInfo.table[frSkyEscDataIdTableInfo.index++] = dataId
349 
350 static void initSmartPortSensors(void)
351 {
352  frSkyDataIdTableInfo.index = 0;
353 
357  }
358 
359 #if defined(USE_ADC_INTERNAL)
362  }
363 #endif
364 
366 #ifdef USE_ESC_SENSOR_TELEMETRY
368 #endif
369  {
371  }
372 
374  }
375 
377 #ifdef USE_ESC_SENSOR_TELEMETRY
379 #endif
380  {
382  }
383 
386  }
387  }
388 
391  }
392 
393 #if defined(USE_ACC)
394  if (sensors(SENSOR_ACC)) {
396  ADD_SENSOR(FSSP_DATAID_ACCX);
397  }
399  ADD_SENSOR(FSSP_DATAID_ACCY);
400  }
402  ADD_SENSOR(FSSP_DATAID_ACCZ);
403  }
404  }
405 #endif
406 
407  if (sensors(SENSOR_BARO)) {
410  }
413  }
414  }
415 
416 #ifdef USE_GPS
417  if (featureIsEnabled(FEATURE_GPS)) {
420  }
423  ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long)
424  }
427  }
430  }
431  }
432 #endif
433 
435 
436  frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index;
437  frSkyDataIdTableInfo.index = 0;
438 
439 #ifdef USE_ESC_SENSOR_TELEMETRY
440  frSkyEscDataIdTableInfo.index = 0;
441 
444  }
447  }
450  }
453  }
454 
455  frSkyEscDataIdTableInfo.size = frSkyEscDataIdTableInfo.index;
456  frSkyEscDataIdTableInfo.index = 0;
457 #endif
458 }
459 
460 //bool initSmartPortTelemetry(void)
461 //{
462 // if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
463 // portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
464 // if (portConfig) {
465 // smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
466 //
467 // smartPortWriteFrame = smartPortWriteFrameInternal;
468 //
469 // initSmartPortSensors();
470 //
471 // telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
472 // }
473 //
474 // return true;
475 // }
476 //
477 // return false;
478 //}
479 
480 bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
481 {
483  smartPortWriteFrame = smartPortWriteFrameExternal;
484 
486 
488 
489  return true;
490  }
491 
492  return false;
493 }
494 
495 //static void freeSmartPortTelemetryPort(void)
496 //{
497 // closeSerialPort(smartPortSerialPort);
498 // smartPortSerialPort = NULL;
499 //}
500 //
501 //static void configureSmartPortTelemetryPort(void)
502 //{
503 // if (portConfig) {
504 // portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
505 //
506 // smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
507 // }
508 //}
509 //
510 //void checkSmartPortTelemetryState(void)
511 //{
512 // if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
513 // bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
514 //
515 // if (enableSerialTelemetry && !smartPortSerialPort) {
516 // configureSmartPortTelemetryPort();
517 // } else if (!enableSerialTelemetry && smartPortSerialPort) {
518 // freeSmartPortTelemetryPort();
519 // }
520 // }
521 //}
522 //
523 //#if defined(USE_MSP_OVER_TELEMETRY)
524 //static void smartPortSendMspResponse(uint8_t *data) {
525 // smartPortPayload_t payload;
526 // payload.frameId = FSSP_MSPS_FRAME;
527 // memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
528 //
529 // smartPortWriteFrame(&payload);
530 //}
531 //#endif
532 
533 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const timeUs_t *requestTimeout)
534 {
535  static uint8_t smartPortIdCycleCnt = 0;
536  static uint8_t t1Cnt = 0;
537  static uint8_t t2Cnt = 0;
538  static uint8_t skipRequests = 0;
539 #ifdef USE_ESC_SENSOR_TELEMETRY
540  static uint8_t smartPortIdOffset = 0;
541 #endif
542 
543 #if defined(USE_MSP_OVER_TELEMETRY)
544  if (skipRequests) {
545  skipRequests--;
546  } else if (payload && smartPortPayloadContainsMSP(payload)) {
547  // Do not check the physical ID here again
548  // unless we start receiving other sensors' packets
549  // Pass only the payload: skip frameId
550  uint8_t *frameStart = (uint8_t *)&payload->valueId;
551  smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
552 
553  // Don't send MSP response after write to eeprom
554  // CPU just got out of suspended state after writeEEPROM()
555  // We don't know if the receiver is listening again
556  // Skip a few telemetry requests before sending response
557  if (skipRequests) {
558  *clearToSend = false;
559  }
560  }
561 #else
562  UNUSED(payload);
563 #endif
564 
565  if (smartPortUplink && payload && payload->valueId == FSSP_DATAID_UPLINK) {
566  smartPortUplink(payload);
567  }
568 
569  bool doRun = true;
570  while (doRun && *clearToSend && !skipRequests) {
571  // 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.
572  if (requestTimeout) {
573  if (cmpTimeUs(micros(), *requestTimeout) >= 0) {
574  *clearToSend = false;
575 
576  return;
577  }
578  } else {
579  doRun = false;
580  }
581 
582 #if defined(USE_MSP_OVER_TELEMETRY)
583  if (smartPortMspReplyPending) {
584  smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
585  *clearToSend = false;
586 
587  return;
588  }
589 #endif
590 
591  // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
592  frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
593 
594 #ifdef USE_ESC_SENSOR_TELEMETRY
595  if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
596  // send ESC sensors
597  tableInfo = &frSkyEscDataIdTableInfo;
598  if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
599  tableInfo->index = 0;
600  smartPortIdCycleCnt = 0;
601  smartPortIdOffset++;
602  if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
603  smartPortIdOffset = 0;
604  }
605  }
606  }
607  if (smartPortIdCycleCnt < ESC_SENSOR_PERIOD) {
608  // send other sensors
609  tableInfo = &frSkyDataIdTableInfo;
610 #endif
611  if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
612  tableInfo->index = 0;
613  }
614 #ifdef USE_ESC_SENSOR_TELEMETRY
615  }
616 #endif
617  uint16_t id = tableInfo->table[tableInfo->index];
618 #ifdef USE_ESC_SENSOR_TELEMETRY
619  if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
620  id += smartPortIdOffset;
621  }
622 #endif
623  smartPortIdCycleCnt++;
624  tableInfo->index++;
625 
626  int32_t tmpi;
627  uint32_t tmp2 = 0;
628  uint16_t vfasVoltage;
629  uint8_t cellCount;
630 
631 #ifdef USE_ESC_SENSOR_TELEMETRY
632  escSensorData_t *escData;
633 #endif
634 
635  switch (id) {
636  case FSSP_DATAID_VFAS :
637  vfasVoltage = getBatteryVoltage();
638  if (telemetryConfig()->report_cell_voltage) {
639  cellCount = getBatteryCellCount();
640  vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0;
641  }
642  smartPortSendPackage(id, vfasVoltage); // given in 0.01V, convert to volts
643  *clearToSend = false;
644  break;
645 #ifdef USE_ESC_SENSOR_TELEMETRY
646  case FSSP_DATAID_VFAS1 :
647  case FSSP_DATAID_VFAS2 :
648  case FSSP_DATAID_VFAS3 :
649  case FSSP_DATAID_VFAS4 :
650  case FSSP_DATAID_VFAS5 :
651  case FSSP_DATAID_VFAS6 :
652  case FSSP_DATAID_VFAS7 :
653  case FSSP_DATAID_VFAS8 :
654  escData = getEscSensorData(id - FSSP_DATAID_VFAS1);
655  if (escData != NULL) {
656  smartPortSendPackage(id, escData->voltage);
657  *clearToSend = false;
658  }
659  break;
660 #endif
661  case FSSP_DATAID_CURRENT :
662  smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
663  *clearToSend = false;
664  break;
665 #ifdef USE_ESC_SENSOR_TELEMETRY
666  case FSSP_DATAID_CURRENT1 :
667  case FSSP_DATAID_CURRENT2 :
668  case FSSP_DATAID_CURRENT3 :
669  case FSSP_DATAID_CURRENT4 :
670  case FSSP_DATAID_CURRENT5 :
671  case FSSP_DATAID_CURRENT6 :
672  case FSSP_DATAID_CURRENT7 :
673  case FSSP_DATAID_CURRENT8 :
674  escData = getEscSensorData(id - FSSP_DATAID_CURRENT1);
675  if (escData != NULL) {
676  smartPortSendPackage(id, escData->current);
677  *clearToSend = false;
678  }
679  break;
680  case FSSP_DATAID_RPM :
681  escData = getEscSensorData(ESC_SENSOR_COMBINED);
682  if (escData != NULL) {
683  smartPortSendPackage(id, calcEscRpm(escData->rpm));
684  *clearToSend = false;
685  }
686  break;
687  case FSSP_DATAID_RPM1 :
688  case FSSP_DATAID_RPM2 :
689  case FSSP_DATAID_RPM3 :
690  case FSSP_DATAID_RPM4 :
691  case FSSP_DATAID_RPM5 :
692  case FSSP_DATAID_RPM6 :
693  case FSSP_DATAID_RPM7 :
694  case FSSP_DATAID_RPM8 :
695  escData = getEscSensorData(id - FSSP_DATAID_RPM1);
696  if (escData != NULL) {
697  smartPortSendPackage(id, calcEscRpm(escData->rpm));
698  *clearToSend = false;
699  }
700  break;
701  case FSSP_DATAID_TEMP :
702  escData = getEscSensorData(ESC_SENSOR_COMBINED);
703  if (escData != NULL) {
704  smartPortSendPackage(id, escData->temperature);
705  *clearToSend = false;
706  }
707  break;
708  case FSSP_DATAID_TEMP1 :
709  case FSSP_DATAID_TEMP2 :
710  case FSSP_DATAID_TEMP3 :
711  case FSSP_DATAID_TEMP4 :
712  case FSSP_DATAID_TEMP5 :
713  case FSSP_DATAID_TEMP6 :
714  case FSSP_DATAID_TEMP7 :
715  case FSSP_DATAID_TEMP8 :
716  escData = getEscSensorData(id - FSSP_DATAID_TEMP1);
717  if (escData != NULL) {
718  smartPortSendPackage(id, escData->temperature);
719  *clearToSend = false;
720  }
721  break;
722 #endif
723  case FSSP_DATAID_ALTITUDE :
724  smartPortSendPackage(id, getEstimatedAltitudeCm()); // unknown given unit, requested 100 = 1 meter
725  *clearToSend = false;
726  break;
727  case FSSP_DATAID_FUEL :
728  smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
729  *clearToSend = false;
730  break;
731  case FSSP_DATAID_VARIO :
732  smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
733  *clearToSend = false;
734  break;
735  case FSSP_DATAID_HEADING :
736  smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
737  *clearToSend = false;
738  break;
739 #if defined(USE_ACC)
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.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:
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); // given in 0.01m
885  *clearToSend = false;
886  }
887  break;
888 #endif
889  case FSSP_DATAID_A4 :
890  cellCount = getBatteryCellCount();
891  vfasVoltage = cellCount ? (getBatteryVoltage() / cellCount) : 0; // given in 0.01V, convert to volts
892  smartPortSendPackage(id, vfasVoltage);
893  *clearToSend = false;
894  break;
895  case FSSP_DATAID_DOWNLINK :
896  if (smartPortDownlink) {
897  uint32_t data;
898  if (smartPortDownlink(&data)) {
899  smartPortSendPackage(id, data);
900  *clearToSend = false;
901  }
902  }
903  break;
904  default:
905  break;
906  // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
907  }
908  }
909 }
910 
911 //static bool serialCheckQueueEmpty(void)
912 //{
913 // return (serialRxBytesWaiting(smartPortSerialPort) == 0);
914 //}
915 //
916 //void handleSmartPortTelemetry(void)
917 //{
918 // const timeUs_t requestTimeout = micros() + SMARTPORT_SERVICE_TIMEOUT_US;
919 //
920 // if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
921 // smartPortPayload_t *payload = NULL;
922 // bool clearToSend = false;
923 // while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
924 // uint8_t c = serialRead(smartPortSerialPort);
925 // payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
926 // }
927 //
928 // processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
929 // }
930 //}
931 //#endif
#define isArmingDisabled()
#define FD_PITCH
unsigned short uint16_t
Definition: types.h:16
#define getAmperage()
struct pidProfile_s * currentPidProfile
Definition: cc2500_compat.c:43
#define PID_ROLL
#define FD_YAW
#define isAmperageConfigured()
bool smartPortCheckQueueEmptyFn(void)
smartPortPayload_t * smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
smartPortDownlinkFn * smartPortDownlink
controlRateConfig_t * currentControlRateProfile
#define getBatteryCellCount()
static frSkyTableInfo_t frSkyDataIdTableInfo
uint32_t timeUs_t
Definition: cc2500_compat.h:93
#define FSSP_DATAID_UPLINK
bool smartPortDownlinkFn(uint32_t *data)
void smartPortWriteFrameFn(const smartPortPayload_t *payload)
uint8_t last_wp UNUSED
Definition: navigation.c:96
#define featureIsEnabled(mask)
static void initSmartPortSensors(void)
#define isBatteryVoltageConfigured()
#define MAX_DATAIDS
bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
static uint16_t frSkyDataIdTable[MAX_DATAIDS]
static uint8_t telemetryState
static uint8_t checksum
Definition: airspeed_uADC.c:60
#define FSSP_DATAID_DOWNLINK
void smartPortUplinkFn(struct smartPortPayload_s *payload)
bool telemetryIsSensorEnabled(sensor_e sensor)
static smartPortWriteFrameFn * smartPortWriteFrame
#define getEstimatedAltitudeCm()
#define getBatteryVoltage()
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const timeUs_t *requestTimeout)
struct smartPortPayload_s smartPortPayload_t
uint16_t val[TCOUPLE_NB]
unsigned long uint32_t
Definition: types.h:18
#define getEstimatedVario()
struct frSkyTableInfo_s frSkyTableInfo_t
#define sensors(...)
Definition: cc2500_compat.h:68
#define attitude
Definition: cc2500_compat.h:79
#define ADD_ESC_SENSOR(dataId)
signed long int32_t
Definition: types.h:19
#define FD_ROLL
#define FLIGHT_MODE(...)
unsigned char uint8_t
Definition: types.h:14
const telemetryConfig_t * telemetryConfig(void)
static void smartPortSendPackage(uint16_t id, uint32_t val)
#define micros()
#define PID_YAW
#define ARMING_FLAG(...)
#define ADD_SENSOR(dataId)
#define PID_PITCH
static timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b)
Definition: cc2500_compat.h:96
struct pidGains_s pid[1]
#define getMAhDrawn()
smartPortUplinkFn * smartPortUplink