2 * Copyright © 2010 Keith Packard <keithp@keithp.com>
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.
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.
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.
22 import java.util.HashMap;
25 * Telemetry data contents
30 * The packet format is a simple hex dump of the raw telemetry frame.
31 * It starts with 'TELEM', then contains hex digits with a checksum as the last
34 * Version 4 is a replacement with consistent syntax. Each telemetry line
35 * contains a sequence of space-separated names and values, the values are
36 * either integers or strings. The names are all unique. All values are
39 * VERSION 4 c KD7SQG n 236 f 18 r -25 s pad t 513 r_a 15756 r_b 26444 r_t 20944
40 * r_v 26640 r_d 512 r_m 208 c_a 15775 c_b 26439 c_p 15749 c_m 16281 a_a 15764
41 * a_s 0 a_b 26439 g_s u g_n 0 s_n 0
43 * VERSION 4 c KD7SQG n 19 f 0 r -23 s pad t 513 r_b 26372 r_t 21292 r_v 26788
44 * r_d 136 r_m 140 c_b 26370 k_h 0 k_s 0 k_a 0
46 * General header fields
50 * VERSION Telemetry version number (4 or more). Must be first.
51 * c Callsign (string, no spaces allowed)
52 * n Flight unit serial number (integer)
53 * f Flight number (integer)
54 * r Packet RSSI value (integer)
55 * s Flight computer state (string, no spaces allowed)
56 * t Flight computer clock (integer in centiseconds)
58 * Version 3 is Version 2 with fixed RSSI numbers -- the radio reports
59 * in 1/2dB increments while this protocol provides only integers. So,
60 * the syntax didn't change just the interpretation of the RSSI
63 * Version 2 of the telemetry data stream is a bit of a mess, with no
64 * consistent formatting. In particular, the GPS data is formatted for
65 * viewing instead of parsing. However, the key feature is that every
66 * telemetry line contains all of the information necessary to
67 * describe the current rocket state, including the calibration values
68 * for accelerometer and barometer.
72 * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -68 STATUS ff STATE pad 1001 \
73 * a: 16032 p: 21232 t: 20284 v: 25160 d: 204 m: 204 fa: 16038 ga: 16032 fv: 0 \
74 * fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS 0 sat unlocked SAT 1 15 30
78 * VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -71 STATUS ff STATE pad 2504 \
79 * a: 16028 p: 21220 t: 20360 v: 25004 d: 208 m: 200 fa: 16031 ga: 16032 fv: 330 \
80 * fp: 21231 gp: 21230 a+: 16049 a-: 16304 \
81 * GPS 9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W 1790m \
82 * 0.00m/s(H) 0° 0.00m/s(V) 1.0(hdop) 0(herr) 0(verr) \
83 * SAT 10 29 30 24 28 5 25 21 20 15 33 1 23 30 24 18 26 10 29 2 26
87 public class AltosTelemetryRecordLegacy extends AltosRecord implements AltosTelemetryRecord {
89 * General header fields
93 * VERSION Telemetry version number (4 or more). Must be first.
94 * c Callsign (string, no spaces allowed)
95 * n Flight unit serial number (integer)
96 * f Flight number (integer)
97 * r Packet RSSI value (integer)
98 * s Flight computer state (string, no spaces allowed)
99 * t Flight computer clock (integer in centiseconds)
102 final static String AO_TELEM_VERSION = "VERSION";
103 final static String AO_TELEM_CALL = "c";
104 final static String AO_TELEM_SERIAL = "n";
105 final static String AO_TELEM_FLIGHT = "f";
106 final static String AO_TELEM_RSSI = "r";
107 final static String AO_TELEM_STATE = "s";
108 final static String AO_TELEM_TICK = "t";
114 * r_a Accelerometer reading (integer)
115 * r_b Barometer reading (integer)
116 * r_t Thermometer reading (integer)
117 * r_v Battery reading (integer)
118 * r_d Drogue continuity (integer)
119 * r_m Main continuity (integer)
122 final static String AO_TELEM_RAW_ACCEL = "r_a";
123 final static String AO_TELEM_RAW_BARO = "r_b";
124 final static String AO_TELEM_RAW_THERMO = "r_t";
125 final static String AO_TELEM_RAW_BATT = "r_v";
126 final static String AO_TELEM_RAW_DROGUE = "r_d";
127 final static String AO_TELEM_RAW_MAIN = "r_m";
130 * Sensor calibration values
133 * c_a Ground accelerometer reading (integer)
134 * c_b Ground barometer reading (integer)
135 * c_p Accelerometer reading for +1g
136 * c_m Accelerometer reading for -1g
139 final static String AO_TELEM_CAL_ACCEL_GROUND = "c_a";
140 final static String AO_TELEM_CAL_BARO_GROUND = "c_b";
141 final static String AO_TELEM_CAL_ACCEL_PLUS = "c_p";
142 final static String AO_TELEM_CAL_ACCEL_MINUS = "c_m";
145 * Kalman state values
148 * k_h Height above pad (integer, meters)
149 * k_s Vertical speeed (integer, m/s * 16)
150 * k_a Vertical acceleration (integer, m/s² * 16)
153 final static String AO_TELEM_KALMAN_HEIGHT = "k_h";
154 final static String AO_TELEM_KALMAN_SPEED = "k_s";
155 final static String AO_TELEM_KALMAN_ACCEL = "k_a";
158 * Ad-hoc flight values
161 * a_a Acceleration (integer, sensor units)
162 * a_s Speed (integer, integrated acceleration value)
163 * a_b Barometer reading (integer, sensor units)
166 final static String AO_TELEM_ADHOC_ACCEL = "a_a";
167 final static String AO_TELEM_ADHOC_SPEED = "a_s";
168 final static String AO_TELEM_ADHOC_BARO = "a_b";
174 * g_s GPS state (string):
177 * e error (missing or broken)
178 * g_n Number of sats used in solution
179 * g_ns Latitude (degrees * 10e7)
180 * g_ew Longitude (degrees * 10e7)
181 * g_a Altitude (integer meters)
182 * g_Y GPS year (integer)
183 * g_M GPS month (integer - 1-12)
184 * g_D GPS day (integer - 1-31)
185 * g_h GPS hour (integer - 0-23)
186 * g_m GPS minute (integer - 0-59)
187 * g_s GPS second (integer - 0-59)
188 * g_v GPS vertical speed (integer, cm/sec)
189 * g_s GPS horizontal speed (integer, cm/sec)
190 * g_c GPS course (integer, 0-359)
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)
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_HDOP = "g_hd";
215 final static String AO_TELEM_GPS_VDOP = "g_vd";
216 final static String AO_TELEM_GPS_HERROR = "g_he";
217 final static String AO_TELEM_GPS_VERROR = "g_ve";
220 * GPS satellite values
223 * s_n Number of satellites reported (integer)
224 * s_v0 Space vehicle ID (integer) for report 0
225 * s_c0 C/N0 number (integer) for report 0
226 * s_v1 Space vehicle ID (integer) for report 1
227 * s_c1 C/N0 number (integer) for report 1
231 final static String AO_TELEM_SAT_NUM = "s_n";
232 final static String AO_TELEM_SAT_SVID = "s_v";
233 final static String AO_TELEM_SAT_C_N_0 = "s_c";
235 private void parse_v4(String[] words, int i) throws ParseException {
236 AltosTelemetryMap map = new AltosTelemetryMap(words, i);
238 callsign = map.get_string(AO_TELEM_CALL, "N0CALL");
239 serial = map.get_int(AO_TELEM_SERIAL, MISSING);
240 flight = map.get_int(AO_TELEM_FLIGHT, MISSING);
241 rssi = map.get_int(AO_TELEM_RSSI, MISSING);
242 state = Altos.state(map.get_string(AO_TELEM_STATE, "invalid"));
243 tick = map.get_int(AO_TELEM_TICK, 0);
245 /* raw sensor values */
246 accel = map.get_int(AO_TELEM_RAW_ACCEL, MISSING);
247 pres = map.get_int(AO_TELEM_RAW_BARO, MISSING);
248 temp = map.get_int(AO_TELEM_RAW_THERMO, MISSING);
249 batt = map.get_int(AO_TELEM_RAW_BATT, MISSING);
250 drogue = map.get_int(AO_TELEM_RAW_DROGUE, MISSING);
251 main = map.get_int(AO_TELEM_RAW_MAIN, MISSING);
253 /* sensor calibration information */
254 ground_accel = map.get_int(AO_TELEM_CAL_ACCEL_GROUND, MISSING);
255 ground_pres = map.get_int(AO_TELEM_CAL_BARO_GROUND, MISSING);
256 accel_plus_g = map.get_int(AO_TELEM_CAL_ACCEL_PLUS, MISSING);
257 accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, MISSING);
259 /* flight computer values */
260 acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, MISSING, 1/16.0);
261 speed = map.get_double(AO_TELEM_KALMAN_SPEED, MISSING, 1/16.0);
262 height = map.get_int(AO_TELEM_KALMAN_HEIGHT, MISSING);
264 flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, MISSING);
265 flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, MISSING);
266 flight_pres = map.get_int(AO_TELEM_ADHOC_BARO, MISSING);
268 if (map.has(AO_TELEM_GPS_STATE))
269 gps = new AltosGPS(map);
274 private void parse_legacy(String[] words, int i) throws ParseException {
276 AltosParse.word (words[i++], "CALL");
277 callsign = words[i++];
279 AltosParse.word (words[i++], "SERIAL");
280 serial = AltosParse.parse_int(words[i++]);
283 AltosParse.word (words[i++], "FLIGHT");
284 flight = AltosParse.parse_int(words[i++]);
288 AltosParse.word(words[i++], "RSSI");
289 rssi = AltosParse.parse_int(words[i++]);
291 /* Older telemetry data had mis-computed RSSI value */
293 rssi = (rssi + 74) / 2 - 74;
295 AltosParse.word(words[i++], "STATUS");
296 status = AltosParse.parse_hex(words[i++]);
298 AltosParse.word(words[i++], "STATE");
299 state = Altos.state(words[i++]);
301 tick = AltosParse.parse_int(words[i++]);
303 AltosParse.word(words[i++], "a:");
304 accel = AltosParse.parse_int(words[i++]);
306 AltosParse.word(words[i++], "p:");
307 pres = AltosParse.parse_int(words[i++]);
309 AltosParse.word(words[i++], "t:");
310 temp = AltosParse.parse_int(words[i++]);
312 AltosParse.word(words[i++], "v:");
313 batt = AltosParse.parse_int(words[i++]);
315 AltosParse.word(words[i++], "d:");
316 drogue = AltosParse.parse_int(words[i++]);
318 AltosParse.word(words[i++], "m:");
319 main = AltosParse.parse_int(words[i++]);
321 AltosParse.word(words[i++], "fa:");
322 flight_accel = AltosParse.parse_int(words[i++]);
324 AltosParse.word(words[i++], "ga:");
325 ground_accel = AltosParse.parse_int(words[i++]);
327 AltosParse.word(words[i++], "fv:");
328 flight_vel = AltosParse.parse_int(words[i++]);
330 AltosParse.word(words[i++], "fp:");
331 flight_pres = AltosParse.parse_int(words[i++]);
333 /* Old TeleDongle code with kalman-reporting TeleMetrum code */
334 if ((flight_vel & 0xffff0000) == 0x80000000) {
335 speed = ((short) flight_vel) / 16.0;
336 acceleration = flight_accel / 16.0;
337 height = flight_pres;
338 flight_vel = MISSING;
339 flight_pres = MISSING;
340 flight_accel = MISSING;
343 AltosParse.word(words[i++], "gp:");
344 ground_pres = AltosParse.parse_int(words[i++]);
347 AltosParse.word(words[i++], "a+:");
348 accel_plus_g = AltosParse.parse_int(words[i++]);
350 AltosParse.word(words[i++], "a-:");
351 accel_minus_g = AltosParse.parse_int(words[i++]);
353 accel_plus_g = ground_accel;
354 accel_minus_g = ground_accel + 530;
357 gps = new AltosGPS(words, i, version);
360 public AltosTelemetryRecordLegacy(String line) throws ParseException, AltosCRCException {
361 String[] words = line.split("\\s+");
364 if (words[i].equals("CRC") && words[i+1].equals("INVALID")) {
366 AltosParse.word(words[i++], "RSSI");
367 rssi = AltosParse.parse_int(words[i++]);
368 throw new AltosCRCException(rssi);
370 if (words[i].equals("CALL")) {
373 AltosParse.word (words[i++], "VERSION");
374 version = AltosParse.parse_int(words[i++]);
378 parse_legacy(words, i);
384 * Given a hex dump of a legacy telemetry line, construct an AltosRecord from that
389 private int int8(int i) {
390 return Altos.int8(bytes, i + 1);
392 private int uint8(int i) {
393 return Altos.uint8(bytes, i + 1);
395 private int int16(int i) {
396 return Altos.int16(bytes, i + 1);
398 private int uint16(int i) {
399 return Altos.uint16(bytes, i + 1);
401 private int uint32(int i) {
402 return Altos.uint32(bytes, i + 1);
404 private String string(int i, int l) {
405 return Altos.string(bytes, i + 1, l);
408 static final int AO_GPS_NUM_SAT_MASK = (0xf << 0);
409 static final int AO_GPS_NUM_SAT_SHIFT = (0);
411 static final int AO_GPS_VALID = (1 << 4);
412 static final int AO_GPS_RUNNING = (1 << 5);
413 static final int AO_GPS_DATE_VALID = (1 << 6);
414 static final int AO_GPS_COURSE_VALID = (1 << 7);
416 static class theLock extends Object {
418 static public theLock lockObject = new theLock();
419 public AltosTelemetryRecordLegacy(int[] in_bytes, int in_rssi, int in_status) {
421 synchronized(lockObject) {
422 for (int i = 0; i < in_bytes.length - 2; i++) {
424 System.out.printf("%3d:", i);
425 System.out.printf(" %02x", uint8(i));
426 if ((i % 10) == 9 || i == in_bytes.length - 3)
427 System.out.printf("\n");
431 callsign = string(62, 8);
445 ground_accel = int16(7);
446 ground_pres = int16(15);
447 accel_plus_g = int16(17);
448 accel_minus_g = int16(19);
450 if (uint16(11) == 0x8000) {
451 acceleration = int16(5);
454 flight_accel = MISSING;
455 flight_vel = MISSING;
456 flight_pres = MISSING;
458 flight_accel = int16(5);
459 flight_vel = uint32(9);
460 flight_pres = int16(13);
461 acceleration = MISSING;
468 int gps_flags = uint8(41);
470 if ((gps_flags & (AO_GPS_VALID|AO_GPS_RUNNING)) != 0) {
471 gps = new AltosGPS();
473 gps.nsat = (gps_flags & AO_GPS_NUM_SAT_MASK);
474 gps.locked = (gps_flags & AO_GPS_VALID) != 0;
475 gps.connected = true;
476 gps.lat = uint32(42) / 1.0e7;
477 gps.lon = uint32(46) / 1.0e7;
479 gps.ground_speed = uint16(52) / 100.0;
480 gps.course = uint8(54) * 2;
481 gps.hdop = uint8(55) / 5.0;
482 gps.h_error = uint16(58);
483 gps.v_error = uint16(60);
485 int n_tracking_reported = uint8(70);
486 if (n_tracking_reported > 12)
487 n_tracking_reported = 12;
488 int n_tracking_actual = 0;
489 for (int i = 0; i < n_tracking_reported; i++) {
490 if (uint8(71 + i*2) != 0)
493 if (n_tracking_actual > 0) {
494 gps.cc_gps_sat = new AltosGPSSat[n_tracking_actual];
496 n_tracking_actual = 0;
497 for (int i = 0; i < n_tracking_reported; i++) {
498 int svid = uint8(71 + i*2);
499 int c_n0 = uint8(72 + i*2);
501 gps.cc_gps_sat[n_tracking_actual++] = new AltosGPSSat(svid, c_n0);
509 public AltosRecord update_state(AltosRecord previous) {