?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
check two CRC types
[u360gts.git] / src / main / tracker / frskyx.c
blob4632838025cf269eb8b43778a120e4f65ca391dd
1 /*
2 * This file is part of u360gts, aka amv-open360tracker 32bits:
3 * https://github.com/raul-ortega/amv-open360tracker-32bits
4 *
5 * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch:
6 * https://github.com/SamuelBrucksch/open360tracker
7 *
8 * u360gts is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation, either version 3 of the License, or
11 * (at your option) any later version.
12 *
13 * u360gts is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with u360gts. If not, see <http://www.gnu.org/licenses/>.
20 *
21 */
22
23 #include "config.h"
24 #include "telemetry.h"
25 #include <math.h>
26
27 void processFrskyPacket(uint8_t *packet);
28 void parseTelemHubByte(uint8_t c);
29 int32_t coordToLong(int8_t neg, uint16_t bp, uint16_t ap);
30
31 #define START_STOP 0x7e
32 #define BYTESTUFF 0x7d
33 #define STUFF_MASK 0x20
34
35 // FrSky PRIM IDs (1 byte)
36 #define DATA_FRAME 0x10
37
38 // FrSky old DATA IDs (1 byte)
39 #define GPS_ALT_BP_ID 0x01
40 #define GPS_ALT_AP_ID 0x09
41 #define BARO_ALT_BP_ID 0x10
42 #define BARO_ALT_AP_ID 0x21
43 #define GPS_LONG_BP_ID 0x12
44 #define GPS_LONG_AP_ID 0x1A
45 #define GPS_LAT_BP_ID 0x13
46 #define GPS_LAT_AP_ID 0x1B
47 #define GPS_LONG_EW_ID 0x22
48 #define GPS_LAT_NS_ID 0x23
49 #define FRSKY_LAST_ID 0x3f
50 //used for sats and fix type
51 #define TEMP2 0x05
52
53 // FrSky Passthrough DATA IDs from APM
54 #define GPS_STATUS 0x5002
55 #define AIRCRAFT_HOME 0X5004
56 #define bitmask(X,Y) (((1 << X) -1) << Y)
57
58 // FrSky new DATA IDs (2 bytes)
59 #define VFAS_FIRST_ID 0x0210
60 #define VFAS_LAST_ID 0x021f
61 #define ALT_FIRST_ID 0x0100
62 #define ALT_LAST_ID 0x010f
63 #define T2_FIRST_ID 0x0410
64 #define T2_LAST_ID 0x041f
65 #define GPS_LONG_LATI_FIRST_ID 0x0800
66 #define GPS_LONG_LATI_LAST_ID 0x080f
67 #define GPS_ALT_FIRST_ID 0x0820
68 #define GPS_ALT_LAST_ID 0x082f
69 #define GPS_SPEED_FIRST_ID 0x0830
70 #define GPS_SPEED_LAST_ID 0x083f
71 #define GPS_COURS_FIRST_ID 0x0840
72 #define GPS_COURS_LAST_ID 0x084f
73
74 #define TELEMETRY_INIT 0
75 #define TELEMETRY_OK 1
76 #define TELEMETRY_KO 2
77
78 #define FRSKY_RX_PACKET_SIZE 8
79
80 #define FRSKYX_LATLON_DIVIDER 1000000
81
82 uint8_t frskyx_RxBuffer[FRSKY_RX_PACKET_SIZE];
83 uint8_t telemetryState = TELEMETRY_INIT;
84
85 //alt in m
86 int16_t alt;
87 uint8_t sats;
88 uint8_t fix;
89
90 uint16_t NS;
91 uint16_t EW;
92
93 //lat lon in decimal degree dd.ddddd
94 uint16_t lat_bp;
95 uint16_t lon_bp;
96 uint16_t lat_ap;
97 uint16_t lon_ap;
98
99 //VFAS sensor = voltage
100 int16_t telemetry_voltage;
101
102 int32_t frskyx_setLat() {
103 int32_t value = coordToLong(NS == 'N' ? 1 : -1, lat_bp, lat_ap);
104 NS = 0;
105 return value;
106 }
107
108 int32_t frskyx_setLon() {
109 int32_t value = coordToLong(EW == 'E' ? 1 : -1, lon_bp, lon_ap);
110 EW = 0;
111 return value;
112 }
113
114 int32_t coordToLong(int8_t neg, uint16_t bp, uint16_t ap) {
115 uint32_t first = ((uint32_t)bp / 100) * FRSKYX_LATLON_DIVIDER;
116 uint32_t second = ((((uint32_t)bp % 100) * FRSKYX_LATLON_DIVIDER) + ((uint32_t)ap * 100)) / 60;
117 return ((int32_t)(first + second) * (uint32_t)neg);
118 }
119
120 void processHubPacket(uint8_t id, uint16_t value)
121 {
122 if (id > FRSKY_LAST_ID)
123 return;
124 switch (id) {
125 case GPS_ALT_BP_ID:
126 alt = value;
127 telemetry_alt = alt;
128 gotAlt = true;
129 break;
130 case GPS_LONG_BP_ID:
131 lon_bp = value;
132 break;
133 case GPS_LONG_AP_ID:
134 lon_ap = value;
135 break;
136 case GPS_LAT_BP_ID:
137 lat_bp = value;
138 break;
139 case GPS_LAT_AP_ID:
140 lat_ap = value;
141 break;
142 case GPS_LONG_EW_ID:
143 EW = value;
144 break;
145 case GPS_LAT_NS_ID:
146 NS = value;
147 break;
148 case TEMP2:
149 if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
150 sats = value / 10;
151 fix = value % 10;
152 } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
153 sats = value % 100;
154
155 if ((value >= 1000) && (value < 2000)) {
156 telemetry_fixtype = 1;
157 }
158 else if ((value >= 2000) && (value < 3000)) {
159 telemetry_fixtype = 2;
160 }
161 else if (value >= 3000) {
162 telemetry_fixtype = 3;
163 }
164 else {
165 telemetry_fixtype = 0;
166 }
167
168 } else
169 sats = value;
170 break;
171 }
172 if ((NS == 'N' || NS == 'S') && (EW == 'E' || EW == 'W')) {
173 telemetry_lat = frskyx_setLat();
174 telemetry_lon = frskyx_setLon();
175 telemetry_sats = sats;
176 gotFix = true;
177 }
178 }
179
180 bool checkSportPacket_V1(uint8_t *packet)
181 {
182 short crc = 0;
183 for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) {
184 crc += packet[i]; //0-1FF
185 crc += crc >> 8; //0-100
186 crc &= 0x00ff;
187 crc += crc >> 8; //0-0FF
188 crc &= 0x00ff;
189 }
190 return (crc == 0x00ff);
191 }
192
193 static uint8_t checkSportPacket_V2(uint8_t * packet)
194 {
195 short crc = 0;
196 for (int i=1; i<FRSKY_RX_PACKET_SIZE-1; ++i) {
197 crc += packet[i]; // 0-1FE
198 crc += crc >> 8; // 0-1FF
199 crc &= 0x00ff; // 0-FF
200 }
201 return 0x00ff - crc;
202 }
203
204 #define SPORT_DATA_U8(packet) (packet[4])
205 #define SPORT_DATA_S32(packet) (*((int32_t *)(packet+4)))
206 #define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4)))
207 #define HUB_DATA_U16(packet) (*((uint16_t *)(packet+4)))
208
209 void processSportPacket(uint8_t *packet)
210 {
211 uint8_t prim = packet[1];
212 uint16_t appId = *((uint16_t *)(packet + 2));
213
214 if (!checkSportPacket_V1(packet) && !checkSportPacket_V2(packet)){
215 telemetry_failed_cs++;
216 return;
217 }
218
219 if(appId == GPS_STATUS) telemetry_provider = TELEMETRY_PROVIDER_APM10;
220
221 switch (prim)
222 {
223 case DATA_FRAME:
224 if ((appId >> 8) == 0) {
225 // The old FrSky IDs
226 uint8_t id = (uint8_t)appId;
227 uint16_t value = HUB_DATA_U16(packet);
228 processHubPacket(id, value);
229 }
230 else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
231 telemetry_voltage = SPORT_DATA_S32(packet); //get voltage
232 }
233 else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
234 if(telemetry_provider == TELEMETRY_PROVIDER_DIY_GPS){
235 sats = SPORT_DATA_S32(packet) / 10;
236 //fix = SPORT_DATA_S32(packet) % 10;
237 } else if (telemetry_provider == TELEMETRY_PROVIDER_INAV){
238 sats = SPORT_DATA_S32(packet) % 100;
239 if ((SPORT_DATA_S32(packet) >= 1000) && (SPORT_DATA_S32(packet) < 2000)) {
240 telemetry_fixtype = 1;
241 }
242 else if ((SPORT_DATA_S32(packet) >= 2000) && (SPORT_DATA_S32(packet) < 3000)) {
243 telemetry_fixtype = 2;
244 }
245 else if (SPORT_DATA_S32(packet) >= 3000) {
246 telemetry_fixtype = 3;
247 }
248 else {
249 telemetry_fixtype = 0;
250 }
251 } else {
252 //we assume that other systems just send the sats over temp2 and fix over temp1
253 sats = SPORT_DATA_S32(packet);
254 }
255 }
256
257
258 else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
259 //frskyData.hub.gpsSpeed_bp = (uint16_t) (SPORT_DATA_U32(packet) / 1000);
260 }
261 else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) {
262 //uint32_t course = SPORT_DATA_U32(packet)/100;
263 }
264 else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
265 alt = SPORT_DATA_S32(packet) / 100;
266 telemetry_alt = alt;
267 gotAlt = true;
268 }
269
270 else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) {
271 uint32_t gps_long_lati_data = SPORT_DATA_U32(packet);
272 uint32_t gps_long_lati_b1w, gps_long_lati_a1w;
273 gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000;
274 gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000;
275
276 switch ((gps_long_lati_data & 0xc0000000) >> 30) {
277 case 0:
278 lat_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
279 lat_ap = gps_long_lati_a1w;
280 NS = 'N';
281 break;
282 case 1:
283 lat_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
284 lat_ap = gps_long_lati_a1w;
285 NS = 'S';
286 break;
287 case 2:
288 lon_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
289 lon_ap = gps_long_lati_a1w;
290 EW = 'E';
291 break;
292 case 3:
293 lon_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60);
294 lon_ap = gps_long_lati_a1w;
295 EW = 'W';
296 break;
297 }
298 if ((NS == 'N' || NS == 'S') && (EW == 'E' || EW == 'W')) {
299 telemetry_lat = frskyx_setLat();
300 telemetry_lon = frskyx_setLon();
301 telemetry_sats = sats;
302 gotFix = true;
303 }
304
305 }
306 else if(appId == GPS_STATUS && telemetry_provider == TELEMETRY_PROVIDER_APM10){
307 sats = (uint8_t) (SPORT_DATA_U32(packet) & bitmask(4,0));
308 //telemetry.gpsAlt = bit32.extract(VALUE,24,7) * (10^bit32.extract(VALUE,22,2)) * (bit32.extract(VALUE,31,1) == 1 and -1 or 1) -- dm
309 uint16_t msl_dc, msl_x, msl_sgn;
310 msl_dc = (SPORT_DATA_U32(packet) & bitmask(7,24)) >> 24;
311 msl_x = (SPORT_DATA_U32(packet) & bitmask(2,22)) >> 22;
312 msl_sgn = (SPORT_DATA_U32(packet) & bitmask(1,31)) >> 31;
313 telemetry_alt = (int16_t) (msl_dc * pow(10,msl_x) * 0.1 * (msl_sgn == 1?-1:1));
314 gotAlt = true;
315 }
316 break;
317 }
318 }
319
320 // Receive buffer state machine state enum
321 enum FrSkyDataState {
322 STATE_DATA_IDLE,
323 STATE_DATA_IN_FRAME,
324 STATE_DATA_XOR,
325 };
326
327 void frskyx_encodeTargetData(uint8_t data) {
328 static uint8_t numPktBytes = 0;
329 static uint8_t dataState = STATE_DATA_IDLE;
330
331 if (data == START_STOP) {
332 dataState = STATE_DATA_IN_FRAME;
333 numPktBytes = 0;
334 }
335 else {
336 switch (dataState) {
337 case STATE_DATA_XOR:
338 frskyx_RxBuffer[numPktBytes++] = data ^ STUFF_MASK;
339 dataState = STATE_DATA_IN_FRAME;
340 break;
341
342 case STATE_DATA_IN_FRAME:
343 if (data == BYTESTUFF)
344 dataState = STATE_DATA_XOR; // XOR next byte
345 else
346 frskyx_RxBuffer[numPktBytes++] = data;
347 break;
348 }
349 }
350
351 if (numPktBytes == FRSKY_RX_PACKET_SIZE) {
352 processSportPacket(frskyx_RxBuffer);
353 dataState = STATE_DATA_IDLE;
354 }
355 }