altos/test: Adjust CRC error rate after FEC fix
[fw/altos] / altoslib / AltosTelemetryLegacy.java
1 /*
2  * Copyright © 2010 Keith Packard <keithp@keithp.com>
3  *
4  * This program is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published by
6  * the Free Software Foundation; either version 2 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12  * General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License along
15  * with this program; if not, write to the Free Software Foundation, Inc.,
16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
17  */
18
19 package org.altusmetrum.altoslib_14;
20
21 import java.text.*;
22
23 /*
24  * Telemetry data contents
25  */
26
27
28 /*
29  * The packet format is a simple hex dump of the raw telemetry frame.
30  * It starts with 'TELEM', then contains hex digits with a checksum as the last
31  * byte on the line.
32  *
33  * Version 4 is a replacement with consistent syntax. Each telemetry line
34  * contains a sequence of space-separated names and values, the values are
35  * either integers or strings. The names are all unique. All values are
36  * optional
37  *
38  * VERSION 4 c KD7SQG n 236 f 18 r -25 s pad t 513 r_a 15756 r_b 26444 r_t 20944
39  *   r_v 26640 r_d 512 r_m 208 c_a 15775 c_b 26439 c_p 15749 c_m 16281 a_a 15764
40  *   a_s 0 a_b 26439 g_s u g_n 0 s_n 0
41  *
42  * VERSION 4 c KD7SQG n 19 f 0 r -23 s pad t 513 r_b 26372 r_t 21292 r_v 26788
43  *   r_d 136 r_m 140 c_b 26370 k_h 0 k_s 0 k_a 0
44  *
45  * General header fields
46  *
47  *      Name            Value
48  *
49  *      VERSION         Telemetry version number (4 or more). Must be first.
50  *      c               Callsign (string, no spaces allowed)
51  *      n               Flight unit serial number (integer)
52  *      f               Flight number (integer)
53  *      r               Packet RSSI value (integer)
54  *      s               Flight computer state (string, no spaces allowed)
55  *      t               Flight computer clock (integer in centiseconds)
56  *
57  * Version 3 is Version 2 with fixed RSSI numbers -- the radio reports
58  * in 1/2dB increments while this protocol provides only integers. So,
59  * the syntax didn't change just the interpretation of the RSSI
60  * values.
61  *
62  * Version 2 of the telemetry data stream is a bit of a mess, with no
63  * consistent formatting. In particular, the GPS data is formatted for
64  * viewing instead of parsing.  However, the key feature is that every
65  * telemetry line contains all of the information necessary to
66  * describe the current rocket state, including the calibration values
67  * for accelerometer and barometer.
68  *
69  * GPS unlocked:
70  *
71  * VERSION 2 CALL KB0G SERIAL  51 FLIGHT     2 RSSI  -68 STATUS ff STATE     pad  1001 \
72  *    a: 16032 p: 21232 t: 20284 v: 25160 d:   204 m:   204 fa: 16038 ga: 16032 fv:       0 \
73  *    fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS  0 sat unlocked SAT 1   15  30
74  *
75  * GPS locked:
76  *
77  * VERSION 2 CALL KB0G SERIAL  51 FLIGHT     2 RSSI  -71 STATUS ff STATE     pad  2504 \
78  *     a: 16028 p: 21220 t: 20360 v: 25004 d:   208 m:   200 fa: 16031 ga: 16032 fv:     330 \
79  *     fp: 21231 gp: 21230 a+: 16049 a-: 16304 \
80  *     GPS  9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W  1790m  \
81  *     0.00m/s(H) 0°     0.00m/s(V) 1.0(hdop)     0(herr)     0(verr) \
82  *     SAT 10   29  30  24  28   5  25  21  20  15  33   1  23  30  24  18  26  10  29   2  26
83  *
84  */
85
86 public class AltosTelemetryLegacy extends AltosTelemetry {
87         /*
88          * General header fields
89          *
90          *      Name            Value
91          *
92          *      VERSION         Telemetry version number (4 or more). Must be first.
93          *      c               Callsign (string, no spaces allowed)
94          *      n               Flight unit serial number (integer)
95          *      f               Flight number (integer)
96          *      r               Packet RSSI value (integer)
97          *      s               Flight computer state (string, no spaces allowed)
98          *      t               Flight computer clock (integer in centiseconds)
99          */
100
101         final static String AO_TELEM_VERSION    = "VERSION";
102         final static String AO_TELEM_CALL       = "c";
103         final static String AO_TELEM_SERIAL     = "n";
104         final static String AO_TELEM_FLIGHT     = "f";
105         final static String AO_TELEM_RSSI       = "r";
106         final static String AO_TELEM_STATE      = "s";
107         final static String AO_TELEM_TICK       = "t";
108
109         /*
110          * Raw sensor values
111          *
112          *      Name            Value
113          *      r_a             Accelerometer reading (integer)
114          *      r_b             Barometer reading (integer)
115          *      r_t             Thermometer reading (integer)
116          *      r_v             Battery reading (integer)
117          *      r_d             Drogue continuity (integer)
118          *      r_m             Main continuity (integer)
119          */
120
121         final static String AO_TELEM_RAW_ACCEL  = "r_a";
122         final static String AO_TELEM_RAW_BARO   = "r_b";
123         final static String AO_TELEM_RAW_THERMO = "r_t";
124         final static String AO_TELEM_RAW_BATT   = "r_v";
125         final static String AO_TELEM_RAW_DROGUE = "r_d";
126         final static String AO_TELEM_RAW_MAIN   = "r_m";
127
128         /*
129          * Sensor calibration values
130          *
131          *      Name            Value
132          *      c_a             Ground accelerometer reading (integer)
133          *      c_b             Ground barometer reading (integer)
134          *      c_p             Accelerometer reading for +1g
135          *      c_m             Accelerometer reading for -1g
136          */
137
138         final static String AO_TELEM_CAL_ACCEL_GROUND   = "c_a";
139         final static String AO_TELEM_CAL_BARO_GROUND    = "c_b";
140         final static String AO_TELEM_CAL_ACCEL_PLUS     = "c_p";
141         final static String AO_TELEM_CAL_ACCEL_MINUS    = "c_m";
142
143         /*
144          * Kalman state values
145          *
146          *      Name            Value
147          *      k_h             Height above pad (integer, meters)
148          *      k_s             Vertical speeed (integer, m/s * 16)
149          *      k_a             Vertical acceleration (integer, m/s² * 16)
150          */
151
152         final static String AO_TELEM_KALMAN_HEIGHT      = "k_h";
153         final static String AO_TELEM_KALMAN_SPEED       = "k_s";
154         final static String AO_TELEM_KALMAN_ACCEL       = "k_a";
155
156         /*
157          * Ad-hoc flight values
158          *
159          *      Name            Value
160          *      a_a             Acceleration (integer, sensor units)
161          *      a_s             Speed (integer, integrated acceleration value)
162          *      a_b             Barometer reading (integer, sensor units)
163          */
164
165         final static String AO_TELEM_ADHOC_ACCEL        = "a_a";
166         final static String AO_TELEM_ADHOC_SPEED        = "a_s";
167         final static String AO_TELEM_ADHOC_BARO         = "a_b";
168
169         /*
170          * GPS values
171          *
172          *      Name            Value
173          *      g_s             GPS state (string):
174          *                              l       locked
175          *                              u       unlocked
176          *                              e       error (missing or broken)
177          *      g_n             Number of sats used in solution
178          *      g_ns            Latitude (degrees * 10e7)
179          *      g_ew            Longitude (degrees * 10e7)
180          *      g_a             Altitude (integer meters)
181          *      g_Y             GPS year (integer)
182          *      g_M             GPS month (integer - 1-12)
183          *      g_D             GPS day (integer - 1-31)
184          *      g_h             GPS hour (integer - 0-23)
185          *      g_m             GPS minute (integer - 0-59)
186          *      g_s             GPS second (integer - 0-59)
187          *      g_v             GPS vertical speed (integer, cm/sec)
188          *      g_s             GPS horizontal speed (integer, cm/sec)
189          *      g_c             GPS course (integer, 0-359)
190          *      g_pd            GPS pdop (integer * 10)
191          *      g_hd            GPS hdop (integer * 10)
192          *      g_vd            GPS vdop (integer * 10)
193          *      g_he            GPS h error (integer)
194          *      g_ve            GPS v error (integer)
195          */
196
197         final static String AO_TELEM_GPS_STATE                  = "g";
198         final static String AO_TELEM_GPS_STATE_LOCKED           = "l";
199         final static String AO_TELEM_GPS_STATE_UNLOCKED         = "u";
200         final static String AO_TELEM_GPS_STATE_ERROR            = "e";
201         final static String AO_TELEM_GPS_NUM_SAT                = "g_n";
202         final static String AO_TELEM_GPS_LATITUDE               = "g_ns";
203         final static String AO_TELEM_GPS_LONGITUDE              = "g_ew";
204         final static String AO_TELEM_GPS_ALTITUDE               = "g_a";
205         final static String AO_TELEM_GPS_YEAR                   = "g_Y";
206         final static String AO_TELEM_GPS_MONTH                  = "g_M";
207         final static String AO_TELEM_GPS_DAY                    = "g_D";
208         final static String AO_TELEM_GPS_HOUR                   = "g_h";
209         final static String AO_TELEM_GPS_MINUTE                 = "g_m";
210         final static String AO_TELEM_GPS_SECOND                 = "g_s";
211         final static String AO_TELEM_GPS_VERTICAL_SPEED         = "g_v";
212         final static String AO_TELEM_GPS_HORIZONTAL_SPEED       = "g_g";
213         final static String AO_TELEM_GPS_COURSE                 = "g_c";
214         final static String AO_TELEM_GPS_PDOP                   = "g_pd";
215         final static String AO_TELEM_GPS_HDOP                   = "g_hd";
216         final static String AO_TELEM_GPS_VDOP                   = "g_vd";
217         final static String AO_TELEM_GPS_HERROR                 = "g_he";
218         final static String AO_TELEM_GPS_VERROR                 = "g_ve";
219
220         /*
221          * GPS satellite values
222          *
223          *      Name            Value
224          *      s_n             Number of satellites reported (integer)
225          *      s_v0            Space vehicle ID (integer) for report 0
226          *      s_c0            C/N0 number (integer) for report 0
227          *      s_v1            Space vehicle ID (integer) for report 1
228          *      s_c1            C/N0 number (integer) for report 1
229          *      ...
230          */
231
232         final static String AO_TELEM_SAT_NUM    = "s_n";
233         final static String AO_TELEM_SAT_SVID   = "s_v";
234         final static String AO_TELEM_SAT_C_N_0  = "s_c";
235
236         public int      tick;
237         public int      serial;
238         public int      rssi;
239         public int      status;
240
241         public int tick() { return tick; }
242         public int serial() { return serial; }
243
244         public int rssi() { return rssi; }
245         public int status() { return status; }
246
247         public int      version;
248         public String   callsign;
249         public int      flight;
250         public int      state;
251
252         public AltosGPS gps;
253         public int      gps_sequence;
254
255         /* Telemetry sources have these values recorded from the flight computer */
256         public double   kalman_height;
257         public double   kalman_speed;
258         public double   kalman_acceleration;
259
260         /* Sensor values */
261         public int      accel;
262         public int      pres;
263         public int      temp;
264         public int      batt;
265         public int      apogee;
266         public int      main;
267
268         public int      ground_accel;
269         public int      ground_pres;
270         public int      accel_plus_g;
271         public int      accel_minus_g;
272
273         public int      flight_accel;
274         public int      flight_vel;
275         public int      flight_pres;
276
277         private void parse_v4(String[] words, int i) throws ParseException {
278                 AltosTelemetryMap       map = new AltosTelemetryMap(words, i);
279
280                 callsign = map.get_string(AO_TELEM_CALL, "N0CALL");
281                 serial = map.get_int(AO_TELEM_SERIAL, AltosLib.MISSING);
282                 flight = map.get_int(AO_TELEM_FLIGHT, AltosLib.MISSING);
283                 rssi = map.get_int(AO_TELEM_RSSI, AltosLib.MISSING);
284                 state = AltosLib.state(map.get_string(AO_TELEM_STATE, "invalid"));
285
286                 /* raw sensor values */
287                 accel = map.get_int(AO_TELEM_RAW_ACCEL, AltosLib.MISSING);
288                 pres = map.get_int(AO_TELEM_RAW_BARO, AltosLib.MISSING);
289                 temp = map.get_int(AO_TELEM_RAW_THERMO, AltosLib.MISSING);
290                 batt = map.get_int(AO_TELEM_RAW_BATT, AltosLib.MISSING);
291                 apogee = map.get_int(AO_TELEM_RAW_DROGUE, AltosLib.MISSING);
292                 main = map.get_int(AO_TELEM_RAW_MAIN, AltosLib.MISSING);
293
294                 /* sensor calibration information */
295                 ground_accel = map.get_int(AO_TELEM_CAL_ACCEL_GROUND, AltosLib.MISSING);
296                 ground_pres = map.get_int(AO_TELEM_CAL_BARO_GROUND, AltosLib.MISSING);
297                 accel_plus_g = map.get_int(AO_TELEM_CAL_ACCEL_PLUS, AltosLib.MISSING);
298                 accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, AltosLib.MISSING);
299
300                 /* flight computer values */
301                 kalman_acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosLib.MISSING, 1/16.0);
302                 kalman_speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosLib.MISSING, 1/16.0);
303                 kalman_height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosLib.MISSING);
304
305                 flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, AltosLib.MISSING);
306                 flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, AltosLib.MISSING);
307                 flight_pres = map.get_int(AO_TELEM_ADHOC_BARO, AltosLib.MISSING);
308
309                 if (map.has(AO_TELEM_GPS_STATE))
310                         gps = new AltosGPS(map);
311                 else
312                         gps = null;
313         }
314
315         private void parse_legacy(String[] words, int i) throws ParseException {
316
317                 AltosParse.word (words[i++], "CALL");
318                 callsign = words[i++];
319
320                 AltosParse.word (words[i++], "SERIAL");
321                 serial = AltosParse.parse_int(words[i++]);
322
323                 if (version >= 2) {
324                         AltosParse.word (words[i++], "FLIGHT");
325                         flight = AltosParse.parse_int(words[i++]);
326                 } else
327                         flight = 0;
328
329                 AltosParse.word(words[i++], "RSSI");
330                 rssi = AltosParse.parse_int(words[i++]);
331
332                 /* Older telemetry data had mis-computed RSSI value */
333                 if (version <= 2)
334                         rssi = (rssi + 74) / 2 - 74;
335
336                 AltosParse.word(words[i++], "STATUS");
337                 status = AltosParse.parse_hex(words[i++]);
338
339                 AltosParse.word(words[i++], "STATE");
340                 state = AltosLib.state(words[i++]);
341
342                 tick = AltosParse.parse_int(words[i++]);
343
344                 AltosParse.word(words[i++], "a:");
345                 accel = AltosParse.parse_int(words[i++]);
346
347                 AltosParse.word(words[i++], "p:");
348                 pres = AltosParse.parse_int(words[i++]);
349
350                 AltosParse.word(words[i++], "t:");
351                 temp = AltosParse.parse_int(words[i++]);
352
353                 AltosParse.word(words[i++], "v:");
354                 batt = AltosParse.parse_int(words[i++]);
355
356                 AltosParse.word(words[i++], "d:");
357                 apogee = AltosParse.parse_int(words[i++]);
358
359                 AltosParse.word(words[i++], "m:");
360                 main = AltosParse.parse_int(words[i++]);
361
362                 AltosParse.word(words[i++], "fa:");
363                 flight_accel = AltosParse.parse_int(words[i++]);
364
365                 AltosParse.word(words[i++], "ga:");
366                 ground_accel = AltosParse.parse_int(words[i++]);
367
368                 AltosParse.word(words[i++], "fv:");
369                 flight_vel = AltosParse.parse_int(words[i++]);
370
371                 AltosParse.word(words[i++], "fp:");
372                 flight_pres = AltosParse.parse_int(words[i++]);
373
374                 /* Old TeleDongle code with kalman-reporting TeleMetrum code */
375                 if ((flight_vel & 0xffff0000) == 0x80000000) {
376                         kalman_speed = ((short) flight_vel) / 16.0;
377                         kalman_acceleration = flight_accel / 16.0;
378                         kalman_height = flight_pres;
379                         flight_vel = AltosLib.MISSING;
380                         flight_pres = AltosLib.MISSING;
381                         flight_accel = AltosLib.MISSING;
382                 } else {
383                         kalman_speed = AltosLib.MISSING;
384                         kalman_acceleration = AltosLib.MISSING;
385                         kalman_height = AltosLib.MISSING;
386                 }
387
388                 AltosParse.word(words[i++], "gp:");
389                 ground_pres = AltosParse.parse_int(words[i++]);
390
391                 if (version >= 1) {
392                         AltosParse.word(words[i++], "a+:");
393                         accel_plus_g = AltosParse.parse_int(words[i++]);
394
395                         AltosParse.word(words[i++], "a-:");
396                         accel_minus_g = AltosParse.parse_int(words[i++]);
397                 } else {
398                         accel_plus_g = ground_accel;
399                         accel_minus_g = ground_accel + 530;
400                 }
401
402                 gps = new AltosGPS(words, i, version);
403                 gps_sequence++;
404         }
405
406         public AltosTelemetryLegacy(String line) throws ParseException, AltosCRCException {
407                 String[] words = line.split("\\s+");
408                 int     i = 0;
409
410                 if (words[i].equals("CRC") && words[i+1].equals("INVALID")) {
411                         i += 2;
412                         AltosParse.word(words[i++], "RSSI");
413                         rssi = AltosParse.parse_int(words[i++]);
414                         throw new AltosCRCException(rssi);
415                 }
416                 if (words[i].equals("CALL")) {
417                         version = 0;
418                 } else {
419                         AltosParse.word (words[i++], "VERSION");
420                         version = AltosParse.parse_int(words[i++]);
421                 }
422
423                 if (version < 4)
424                         parse_legacy(words, i);
425                 else
426                         parse_v4(words, i);
427         }
428
429         /*
430          * Given a hex dump of a legacy telemetry line, construct an AltosRecordTM from that
431          */
432
433         int     adjust;
434
435         /*
436         private int int8(int i) {
437                 return AltosLib.int8(bytes, i + 1 + adjust);
438         }
439         */
440         private int uint8(int i) {
441                 return AltosLib.uint8(bytes, i + 1 + adjust);
442         }
443         private int int16(int i) {
444                 return AltosLib.int16(bytes, i + 1 + adjust);
445         }
446         private int uint16(int i) {
447                 return AltosLib.uint16(bytes, i + 1 + adjust);
448         }
449         private int uint32(int i) {
450                 return AltosLib.uint32(bytes, i + 1 + adjust);
451         }
452         private String string(int i, int l) {
453                 return AltosLib.string(bytes, i + 1 + adjust, l);
454         }
455
456         static final int AO_GPS_NUM_SAT_MASK    = (0xf << 0);
457         static final int AO_GPS_NUM_SAT_SHIFT   = (0);
458
459         static final int AO_GPS_VALID           = (1 << 4);
460         static final int AO_GPS_RUNNING         = (1 << 5);
461         static final int AO_GPS_DATE_VALID      = (1 << 6);
462         static final int AO_GPS_COURSE_VALID    = (1 << 7);
463
464         public AltosTelemetryLegacy(int[] in_bytes) throws AltosCRCException {
465                 super(in_bytes);
466
467                 version = 4;
468                 adjust = 0;
469
470                 if (bytes.length == AltosLib.ao_telemetry_0_8_len + 4) {
471                         serial = uint8(0);
472                         adjust = -1;
473                 } else
474                         serial = uint16(0);
475
476                 rssi = super.rssi();
477                 callsign = string(62, 8);
478                 flight = uint16(2);
479                 state = uint8(4);
480                 tick = uint16(21);
481                 accel = int16(23);
482                 pres = int16(25);
483                 temp = int16(27);
484                 batt = int16(29);
485                 apogee = int16(31);
486                 main = int16(33);
487
488                 ground_accel = int16(7);
489                 ground_pres = int16(15);
490                 accel_plus_g = int16(17);
491                 accel_minus_g = int16(19);
492
493                 if (uint16(11) == 0x8000) {
494                         kalman_acceleration = int16(5);
495                         kalman_speed = int16(9);
496                         kalman_height = int16(13);
497                         flight_accel = AltosLib.MISSING;
498                         flight_vel = AltosLib.MISSING;
499                         flight_pres = AltosLib.MISSING;
500                 } else {
501                         flight_accel = int16(5);
502                         flight_vel = uint32(9);
503                         flight_pres = int16(13);
504                         kalman_acceleration = AltosLib.MISSING;
505                         kalman_speed = AltosLib.MISSING;
506                         kalman_height = AltosLib.MISSING;
507                 }
508
509                 gps = null;
510
511                 int gps_flags = uint8(41);
512
513                 if ((gps_flags & (AO_GPS_VALID|AO_GPS_RUNNING)) != 0) {
514                         gps = new AltosGPS();
515                         gps_sequence++;
516
517                         gps.nsat = (gps_flags & AO_GPS_NUM_SAT_MASK);
518                         gps.locked = (gps_flags & AO_GPS_VALID) != 0;
519                         gps.connected = true;
520                         gps.lat = uint32(42) / 1.0e7;
521                         gps.lon = uint32(46) / 1.0e7;
522                         gps.alt = int16(50);
523                         gps.ground_speed = uint16(52) / 100.0;
524                         gps.course = uint8(54) * 2;
525                         gps.hdop = uint8(55) / 5.0;
526                         gps.h_error = uint16(58);
527                         gps.v_error = uint16(60);
528
529                         int     n_tracking_reported = uint8(70);
530                         if (n_tracking_reported > 12)
531                                 n_tracking_reported = 12;
532                         int     n_tracking_actual = 0;
533                         for (int i = 0; i < n_tracking_reported; i++) {
534                                 if (uint8(71 + i*2) != 0)
535                                         n_tracking_actual++;
536                         }
537                         if (n_tracking_actual > 0) {
538                                 gps.cc_gps_sat = new AltosGPSSat[n_tracking_actual];
539
540                                 n_tracking_actual = 0;
541                                 for (int i = 0; i < n_tracking_reported; i++) {
542                                         int     svid = uint8(71 + i*2);
543                                         int     c_n0 = uint8(72 + i*2);
544                                         if (svid != 0)
545                                                 gps.cc_gps_sat[n_tracking_actual++] = new AltosGPSSat(svid, c_n0);
546                                 }
547                         }
548                 }
549         }
550
551         public void provide_data(AltosDataListener listener) {
552                 listener.set_serial(serial);
553                 listener.set_tick(tick);
554                 listener.set_state(this.state);
555                 listener.set_flight(flight);
556                 listener.set_rssi(rssi, status);
557
558                 listener.set_pressure(AltosConvert.barometer_to_pressure(pres));
559
560                 AltosCalData cal_data = listener.cal_data();
561
562                 cal_data.set_accel_plus_minus(accel_plus_g, accel_minus_g);
563                 listener.set_acceleration(cal_data.acceleration(accel));
564                 if (kalman_height != AltosLib.MISSING)
565                         listener.set_kalman(kalman_height, kalman_speed, kalman_acceleration);
566                 listener.set_temperature(AltosConvert.thermometer_to_temperature(temp));
567                 listener.set_battery_voltage(AltosConvert.cc_battery_to_voltage(batt));
568                 listener.set_apogee_voltage(AltosConvert.cc_igniter_to_voltage(apogee));
569                 listener.set_main_voltage(AltosConvert.cc_igniter_to_voltage(main));
570                 if (gps != null)
571                         listener.set_gps(gps, true, true);
572         }
573 }