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