ALPAKA/TeleStern
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity
TeleStern/altoslib/AltosTelemetryLegacy.java

574 lines
18 KiB
Java
Raw Permalink Blame History

/*
* Copyright © 2010 Keith Packard <keithp@keithp.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
package org.altusmetrum.altoslib_14;
import java.text.*;
/*
* Telemetry data contents
*/
/*
* The packet format is a simple hex dump of the raw telemetry frame.
* It starts with 'TELEM', then contains hex digits with a checksum as the last
* byte on the line.
*
* Version 4 is a replacement with consistent syntax. Each telemetry line
* contains a sequence of space-separated names and values, the values are
* either integers or strings. The names are all unique. All values are
* optional
*
* VERSION 4 c KD7SQG n 236 f 18 r -25 s pad t 513 r_a 15756 r_b 26444 r_t 20944
* r_v 26640 r_d 512 r_m 208 c_a 15775 c_b 26439 c_p 15749 c_m 16281 a_a 15764
* a_s 0 a_b 26439 g_s u g_n 0 s_n 0
*
* VERSION 4 c KD7SQG n 19 f 0 r -23 s pad t 513 r_b 26372 r_t 21292 r_v 26788
* r_d 136 r_m 140 c_b 26370 k_h 0 k_s 0 k_a 0
*
* General header fields
*
* Name Value
*
* VERSION Telemetry version number (4 or more). Must be first.
* c Callsign (string, no spaces allowed)
* n Flight unit serial number (integer)
* f Flight number (integer)
* r Packet RSSI value (integer)
* s Flight computer state (string, no spaces allowed)
* t Flight computer clock (integer in centiseconds)
*
* Version 3 is Version 2 with fixed RSSI numbers -- the radio reports
* in 1/2dB increments while this protocol provides only integers. So,
* the syntax didn't change just the interpretation of the RSSI
* values.
*
* Version 2 of the telemetry data stream is a bit of a mess, with no
* consistent formatting. In particular, the GPS data is formatted for
* viewing instead of parsing. However, the key feature is that every
* telemetry line contains all of the information necessary to
* describe the current rocket state, including the calibration values
* for accelerometer and barometer.
*
* GPS unlocked:
*
* VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -68 STATUS ff STATE pad 1001 \
* a: 16032 p: 21232 t: 20284 v: 25160 d: 204 m: 204 fa: 16038 ga: 16032 fv: 0 \
* fp: 21232 gp: 21230 a+: 16049 a-: 16304 GPS 0 sat unlocked SAT 1 15 30
*
* GPS locked:
*
* VERSION 2 CALL KB0G SERIAL 51 FLIGHT 2 RSSI -71 STATUS ff STATE pad 2504 \
* a: 16028 p: 21220 t: 20360 v: 25004 d: 208 m: 200 fa: 16031 ga: 16032 fv: 330 \
* fp: 21231 gp: 21230 a+: 16049 a-: 16304 \
* GPS 9 sat 2010-02-13 17:16:51 35°20.0803'N 106°45.2235'W 1790m \
* 0.00m/s(H) 0° 0.00m/s(V) 1.0(hdop) 0(herr) 0(verr) \
* SAT 10 29 30 24 28 5 25 21 20 15 33 1 23 30 24 18 26 10 29 2 26
*
*/
public class AltosTelemetryLegacy extends AltosTelemetry {
/*
* General header fields
*
* Name Value
*
* VERSION Telemetry version number (4 or more). Must be first.
* c Callsign (string, no spaces allowed)
* n Flight unit serial number (integer)
* f Flight number (integer)
* r Packet RSSI value (integer)
* s Flight computer state (string, no spaces allowed)
* t Flight computer clock (integer in centiseconds)
*/
final static String AO_TELEM_VERSION = "VERSION";
final static String AO_TELEM_CALL = "c";
final static String AO_TELEM_SERIAL = "n";
final static String AO_TELEM_FLIGHT = "f";
final static String AO_TELEM_RSSI = "r";
final static String AO_TELEM_STATE = "s";
final static String AO_TELEM_TICK = "t";
/*
* Raw sensor values
*
* Name Value
* r_a Accelerometer reading (integer)
* r_b Barometer reading (integer)
* r_t Thermometer reading (integer)
* r_v Battery reading (integer)
* r_d Drogue continuity (integer)
* r_m Main continuity (integer)
*/
final static String AO_TELEM_RAW_ACCEL = "r_a";
final static String AO_TELEM_RAW_BARO = "r_b";
final static String AO_TELEM_RAW_THERMO = "r_t";
final static String AO_TELEM_RAW_BATT = "r_v";
final static String AO_TELEM_RAW_DROGUE = "r_d";
final static String AO_TELEM_RAW_MAIN = "r_m";
/*
* Sensor calibration values
*
* Name Value
* c_a Ground accelerometer reading (integer)
* c_b Ground barometer reading (integer)
* c_p Accelerometer reading for +1g
* c_m Accelerometer reading for -1g
*/
final static String AO_TELEM_CAL_ACCEL_GROUND = "c_a";
final static String AO_TELEM_CAL_BARO_GROUND = "c_b";
final static String AO_TELEM_CAL_ACCEL_PLUS = "c_p";
final static String AO_TELEM_CAL_ACCEL_MINUS = "c_m";
/*
* Kalman state values
*
* Name Value
* k_h Height above pad (integer, meters)
* k_s Vertical speeed (integer, m/s * 16)
* k_a Vertical acceleration (integer, m/s² * 16)
*/
final static String AO_TELEM_KALMAN_HEIGHT = "k_h";
final static String AO_TELEM_KALMAN_SPEED = "k_s";
final static String AO_TELEM_KALMAN_ACCEL = "k_a";
/*
* Ad-hoc flight values
*
* Name Value
* a_a Acceleration (integer, sensor units)
* a_s Speed (integer, integrated acceleration value)
* a_b Barometer reading (integer, sensor units)
*/
final static String AO_TELEM_ADHOC_ACCEL = "a_a";
final static String AO_TELEM_ADHOC_SPEED = "a_s";
final static String AO_TELEM_ADHOC_BARO = "a_b";
/*
* GPS values
*
* Name Value
* g_s GPS state (string):
* l locked
* u unlocked
* e error (missing or broken)
* g_n Number of sats used in solution
* g_ns Latitude (degrees * 10e7)
* g_ew Longitude (degrees * 10e7)
* g_a Altitude (integer meters)
* g_Y GPS year (integer)
* g_M GPS month (integer - 1-12)
* g_D GPS day (integer - 1-31)
* g_h GPS hour (integer - 0-23)
* g_m GPS minute (integer - 0-59)
* g_s GPS second (integer - 0-59)
* g_v GPS vertical speed (integer, cm/sec)
* g_s GPS horizontal speed (integer, cm/sec)
* g_c GPS course (integer, 0-359)
* g_pd GPS pdop (integer * 10)
* g_hd GPS hdop (integer * 10)
* g_vd GPS vdop (integer * 10)
* g_he GPS h error (integer)
* g_ve GPS v error (integer)
*/
final static String AO_TELEM_GPS_STATE = "g";
final static String AO_TELEM_GPS_STATE_LOCKED = "l";
final static String AO_TELEM_GPS_STATE_UNLOCKED = "u";
final static String AO_TELEM_GPS_STATE_ERROR = "e";
final static String AO_TELEM_GPS_NUM_SAT = "g_n";
final static String AO_TELEM_GPS_LATITUDE = "g_ns";
final static String AO_TELEM_GPS_LONGITUDE = "g_ew";
final static String AO_TELEM_GPS_ALTITUDE = "g_a";
final static String AO_TELEM_GPS_YEAR = "g_Y";
final static String AO_TELEM_GPS_MONTH = "g_M";
final static String AO_TELEM_GPS_DAY = "g_D";
final static String AO_TELEM_GPS_HOUR = "g_h";
final static String AO_TELEM_GPS_MINUTE = "g_m";
final static String AO_TELEM_GPS_SECOND = "g_s";
final static String AO_TELEM_GPS_VERTICAL_SPEED = "g_v";
final static String AO_TELEM_GPS_HORIZONTAL_SPEED = "g_g";
final static String AO_TELEM_GPS_COURSE = "g_c";
final static String AO_TELEM_GPS_PDOP = "g_pd";
final static String AO_TELEM_GPS_HDOP = "g_hd";
final static String AO_TELEM_GPS_VDOP = "g_vd";
final static String AO_TELEM_GPS_HERROR = "g_he";
final static String AO_TELEM_GPS_VERROR = "g_ve";
/*
* GPS satellite values
*
* Name Value
* s_n Number of satellites reported (integer)
* s_v0 Space vehicle ID (integer) for report 0
* s_c0 C/N0 number (integer) for report 0
* s_v1 Space vehicle ID (integer) for report 1
* s_c1 C/N0 number (integer) for report 1
* ...
*/
final static String AO_TELEM_SAT_NUM = "s_n";
final static String AO_TELEM_SAT_SVID = "s_v";
final static String AO_TELEM_SAT_C_N_0 = "s_c";
public int tick;
public int serial;
public int rssi;
public int status;
public int tick() { return tick; }
public int serial() { return serial; }
public int rssi() { return rssi; }
public int status() { return status; }
public int version;
public String callsign;
public int flight;
public int state;
public AltosGPS gps;
public int gps_sequence;
/* Telemetry sources have these values recorded from the flight computer */
public double kalman_height;
public double kalman_speed;
public double kalman_acceleration;
/* Sensor values */
public int accel;
public int pres;
public int temp;
public int batt;
public int apogee;
public int main;
public int ground_accel;
public int ground_pres;
public int accel_plus_g;
public int accel_minus_g;
public int flight_accel;
public int flight_vel;
public int flight_pres;
private void parse_v4(String[] words, int i) throws ParseException {
AltosTelemetryMap map = new AltosTelemetryMap(words, i);
callsign = map.get_string(AO_TELEM_CALL, "N0CALL");
serial = map.get_int(AO_TELEM_SERIAL, AltosLib.MISSING);
flight = map.get_int(AO_TELEM_FLIGHT, AltosLib.MISSING);
rssi = map.get_int(AO_TELEM_RSSI, AltosLib.MISSING);
state = AltosLib.state(map.get_string(AO_TELEM_STATE, "invalid"));
/* raw sensor values */
accel = map.get_int(AO_TELEM_RAW_ACCEL, AltosLib.MISSING);
pres = map.get_int(AO_TELEM_RAW_BARO, AltosLib.MISSING);
temp = map.get_int(AO_TELEM_RAW_THERMO, AltosLib.MISSING);
batt = map.get_int(AO_TELEM_RAW_BATT, AltosLib.MISSING);
apogee = map.get_int(AO_TELEM_RAW_DROGUE, AltosLib.MISSING);
main = map.get_int(AO_TELEM_RAW_MAIN, AltosLib.MISSING);
/* sensor calibration information */
ground_accel = map.get_int(AO_TELEM_CAL_ACCEL_GROUND, AltosLib.MISSING);
ground_pres = map.get_int(AO_TELEM_CAL_BARO_GROUND, AltosLib.MISSING);
accel_plus_g = map.get_int(AO_TELEM_CAL_ACCEL_PLUS, AltosLib.MISSING);
accel_minus_g = map.get_int(AO_TELEM_CAL_ACCEL_MINUS, AltosLib.MISSING);
/* flight computer values */
kalman_acceleration = map.get_double(AO_TELEM_KALMAN_ACCEL, AltosLib.MISSING, 1/16.0);
kalman_speed = map.get_double(AO_TELEM_KALMAN_SPEED, AltosLib.MISSING, 1/16.0);
kalman_height = map.get_int(AO_TELEM_KALMAN_HEIGHT, AltosLib.MISSING);
flight_accel = map.get_int(AO_TELEM_ADHOC_ACCEL, AltosLib.MISSING);
flight_vel = map.get_int(AO_TELEM_ADHOC_SPEED, AltosLib.MISSING);
flight_pres = map.get_int(AO_TELEM_ADHOC_BARO, AltosLib.MISSING);
if (map.has(AO_TELEM_GPS_STATE))
gps = new AltosGPS(map);
else
gps = null;
}
private void parse_legacy(String[] words, int i) throws ParseException {
AltosParse.word (words[i++], "CALL");
callsign = words[i++];
AltosParse.word (words[i++], "SERIAL");
serial = AltosParse.parse_int(words[i++]);
if (version >= 2) {
AltosParse.word (words[i++], "FLIGHT");
flight = AltosParse.parse_int(words[i++]);
} else
flight = 0;
AltosParse.word(words[i++], "RSSI");
rssi = AltosParse.parse_int(words[i++]);
/* Older telemetry data had mis-computed RSSI value */
if (version <= 2)
rssi = (rssi + 74) / 2 - 74;
AltosParse.word(words[i++], "STATUS");
status = AltosParse.parse_hex(words[i++]);
AltosParse.word(words[i++], "STATE");
state = AltosLib.state(words[i++]);
tick = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "a:");
accel = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "p:");
pres = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "t:");
temp = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "v:");
batt = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "d:");
apogee = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "m:");
main = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "fa:");
flight_accel = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "ga:");
ground_accel = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "fv:");
flight_vel = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "fp:");
flight_pres = AltosParse.parse_int(words[i++]);
/* Old TeleDongle code with kalman-reporting TeleMetrum code */
if ((flight_vel & 0xffff0000) == 0x80000000) {
kalman_speed = ((short) flight_vel) / 16.0;
kalman_acceleration = flight_accel / 16.0;
kalman_height = flight_pres;
flight_vel = AltosLib.MISSING;
flight_pres = AltosLib.MISSING;
flight_accel = AltosLib.MISSING;
} else {
kalman_speed = AltosLib.MISSING;
kalman_acceleration = AltosLib.MISSING;
kalman_height = AltosLib.MISSING;
}
AltosParse.word(words[i++], "gp:");
ground_pres = AltosParse.parse_int(words[i++]);
if (version >= 1) {
AltosParse.word(words[i++], "a+:");
accel_plus_g = AltosParse.parse_int(words[i++]);
AltosParse.word(words[i++], "a-:");
accel_minus_g = AltosParse.parse_int(words[i++]);
} else {
accel_plus_g = ground_accel;
accel_minus_g = ground_accel + 530;
}
gps = new AltosGPS(words, i, version);
gps_sequence++;
}
public AltosTelemetryLegacy(String line) throws ParseException, AltosCRCException {
String[] words = line.split("\\s+");
int i = 0;
if (words[i].equals("CRC") && words[i+1].equals("INVALID")) {
i += 2;
AltosParse.word(words[i++], "RSSI");
rssi = AltosParse.parse_int(words[i++]);
throw new AltosCRCException(rssi);
}
if (words[i].equals("CALL")) {
version = 0;
} else {
AltosParse.word (words[i++], "VERSION");
version = AltosParse.parse_int(words[i++]);
}
if (version < 4)
parse_legacy(words, i);
else
parse_v4(words, i);
}
/*
* Given a hex dump of a legacy telemetry line, construct an AltosRecordTM from that
*/
int adjust;
/*
private int int8(int i) {
return AltosLib.int8(bytes, i + 1 + adjust);
}
*/
private int uint8(int i) {
return AltosLib.uint8(bytes, i + 1 + adjust);
}
private int int16(int i) {
return AltosLib.int16(bytes, i + 1 + adjust);
}
private int uint16(int i) {
return AltosLib.uint16(bytes, i + 1 + adjust);
}
private int uint32(int i) {
return AltosLib.uint32(bytes, i + 1 + adjust);
}
private String string(int i, int l) {
return AltosLib.string(bytes, i + 1 + adjust, l);
}
static final int AO_GPS_NUM_SAT_MASK = (0xf << 0);
static final int AO_GPS_NUM_SAT_SHIFT = (0);
static final int AO_GPS_VALID = (1 << 4);
static final int AO_GPS_RUNNING = (1 << 5);
static final int AO_GPS_DATE_VALID = (1 << 6);
static final int AO_GPS_COURSE_VALID = (1 << 7);
public AltosTelemetryLegacy(int[] in_bytes) throws AltosCRCException {
super(in_bytes);
version = 4;
adjust = 0;
if (bytes.length == AltosLib.ao_telemetry_0_8_len + 4) {
serial = uint8(0);
adjust = -1;
} else
serial = uint16(0);
rssi = super.rssi();
callsign = string(62, 8);
flight = uint16(2);
state = uint8(4);
tick = uint16(21);
accel = int16(23);
pres = int16(25);
temp = int16(27);
batt = int16(29);
apogee = int16(31);
main = int16(33);
ground_accel = int16(7);
ground_pres = int16(15);
accel_plus_g = int16(17);
accel_minus_g = int16(19);
if (uint16(11) == 0x8000) {
kalman_acceleration = int16(5);
kalman_speed = int16(9);
kalman_height = int16(13);
flight_accel = AltosLib.MISSING;
flight_vel = AltosLib.MISSING;
flight_pres = AltosLib.MISSING;
} else {
flight_accel = int16(5);
flight_vel = uint32(9);
flight_pres = int16(13);
kalman_acceleration = AltosLib.MISSING;
kalman_speed = AltosLib.MISSING;
kalman_height = AltosLib.MISSING;
}
gps = null;
int gps_flags = uint8(41);
if ((gps_flags & (AO_GPS_VALID|AO_GPS_RUNNING)) != 0) {
gps = new AltosGPS();
gps_sequence++;
gps.nsat = (gps_flags & AO_GPS_NUM_SAT_MASK);
gps.locked = (gps_flags & AO_GPS_VALID) != 0;
gps.connected = true;
gps.lat = uint32(42) / 1.0e7;
gps.lon = uint32(46) / 1.0e7;
gps.alt = int16(50);
gps.ground_speed = uint16(52) / 100.0;
gps.course = uint8(54) * 2;
gps.hdop = uint8(55) / 5.0;
gps.h_error = uint16(58);
gps.v_error = uint16(60);
int n_tracking_reported = uint8(70);
if (n_tracking_reported > 12)
n_tracking_reported = 12;
int n_tracking_actual = 0;
for (int i = 0; i < n_tracking_reported; i++) {
if (uint8(71 + i*2) != 0)
n_tracking_actual++;
}
if (n_tracking_actual > 0) {
gps.cc_gps_sat = new AltosGPSSat[n_tracking_actual];
n_tracking_actual = 0;
for (int i = 0; i < n_tracking_reported; i++) {
int svid = uint8(71 + i*2);
int c_n0 = uint8(72 + i*2);
if (svid != 0)
gps.cc_gps_sat[n_tracking_actual++] = new AltosGPSSat(svid, c_n0);
}
}
}
}
public void provide_data(AltosDataListener listener) {
listener.set_serial(serial);
listener.set_tick(tick);
listener.set_state(this.state);
listener.set_flight(flight);
listener.set_rssi(rssi, status);
listener.set_pressure(AltosConvert.barometer_to_pressure(pres));
AltosCalData cal_data = listener.cal_data();
cal_data.set_accel_plus_minus(accel_plus_g, accel_minus_g);
listener.set_acceleration(cal_data.acceleration(accel));
if (kalman_height != AltosLib.MISSING)
listener.set_kalman(kalman_height, kalman_speed, kalman_acceleration);
listener.set_temperature(AltosConvert.thermometer_to_temperature(temp));
listener.set_battery_voltage(AltosConvert.cc_battery_to_voltage(batt));
listener.set_apogee_voltage(AltosConvert.cc_igniter_to_voltage(apogee));
listener.set_main_voltage(AltosConvert.cc_igniter_to_voltage(main));
if (gps != null)
listener.set_gps(gps, true, true);
}
}
Reference in New Issue View Git Blame Copy Permalink