From: Keith Packard Date: Thu, 1 Apr 2010 06:05:03 +0000 (-0700) Subject: Add telemetry data parsing code X-Git-Tag: debian/0.6+163+g01e524f~30 X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=a06bee96e648d9ded8776f3d6cba9505e7be1a60;hp=7f233369e32c3254165ee251df0a3dbc21ea5a29;p=fw%2Faltos Add telemetry data parsing code --- diff --git a/ao-tools/altosui/.gitignore b/ao-tools/altosui/.gitignore new file mode 100644 index 00000000..59913193 --- /dev/null +++ b/ao-tools/altosui/.gitignore @@ -0,0 +1,2 @@ +*.class +altosui diff --git a/ao-tools/altosui/AltosTelemetry.java b/ao-tools/altosui/AltosTelemetry.java new file mode 100644 index 00000000..e072bb34 --- /dev/null +++ b/ao-tools/altosui/AltosTelemetry.java @@ -0,0 +1,287 @@ +/* + * Copyright © 2010 Keith Packard + * + * 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; version 2 of the License. + * + * 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 altosui; + +import java.lang.*; +import java.text.*; + +/* + * Telemetry data contents + */ + +class AltosGPSTime { + int year; + int month; + int day; + int hour; + int minute; + int second; + + int parse_int(String v) throws ParseException { + try { + return Integer.parseInt(v); + } catch (NumberFormatException e) { + throw new ParseException(v, 0); + } + } + + public AltosGPSTime(String date, String time) throws ParseException { + String[] ymd = date.split("-"); + if (ymd.length != 3) { + System.out.println("Error parsing GPS date " + date + " got " + ymd.length); + throw new ParseException(date, 0); + } + year = parse_int(ymd[0]); + month = parse_int(ymd[1]); + day = parse_int(ymd[2]); + + String[] hms = time.split(":"); + if (hms.length != 3) { + System.out.println("Error parsing GPS time " + time + " got " + hms.length); + throw new ParseException(time, 0); + } + hour = parse_int(hms[0]); + minute = parse_int(hms[1]); + second = parse_int(hms[2]); + } + + public AltosGPSTime() { + year = month = day = 0; + hour = minute = second = 0; + } +}; + +class AltosGPS { + int nsat; + int gps_locked; + int gps_connected; + AltosGPSTime gps_time; + double lat; /* degrees (+N -S) */ + double lon; /* degrees (+E -W) */ + int alt; /* m */ + + int gps_extended; /* has extra data */ + double ground_speed; /* m/s */ + int course; /* degrees */ + double climb_rate; /* m/s */ + double hdop; /* unitless? */ + int h_error; /* m */ + int v_error; /* m */ + +} + +class AltosGPSSat { + int svid; + int c_n0; +} + +class AltosGPSTracking { + int channels; + AltosGPSSat[] cc_gps_sat; +} + +public class AltosTelemetry { + int version; + String callsign; + int serial; + int flight; + int rssi; + int status; + String state; + int tick; + int accel; + int pres; + int temp; + int batt; + int drogue; + int main; + int flight_accel; + int ground_accel; + int flight_vel; + int flight_pres; + int ground_pres; + int accel_plus_g; + int accel_minus_g; + AltosGPS gps; + AltosGPSTracking gps_tracking; + + int parse_int(String v) throws ParseException { + try { + return Integer.parseInt(v); + } catch (NumberFormatException e) { + System.out.println("error parsing int " + v); + throw new ParseException(v, 0); + } + } + + int parse_hex(String v) throws ParseException { + try { + return Integer.parseInt(v, 16); + } catch (NumberFormatException e) { + System.out.println("error parsing hex " + v); + throw new ParseException(v, 0); + } + } + + double parse_double(String v) throws ParseException { + try { + return Double.parseDouble(v); + } catch (NumberFormatException e) { + System.out.println("error parsing double " + v); + throw new ParseException(v, 0); + } + } + + double parse_coord(String coord) throws ParseException { + String[] dsf = coord.split("\\D+"); + + if (dsf.length != 3) { + System.out.println("error parsing coord " + coord); + throw new ParseException(coord, 0); + } + int deg = parse_int(dsf[0]); + int min = parse_int(dsf[1]); + int frac = parse_int(dsf[2]); + + double r = deg + (min + frac / 10000.0) / 60.0; + if (coord.endsWith("S") || coord.endsWith("W")) + r = -r; + return r; + } + + String strip_suffix(String v, String suffix) { + if (v.endsWith(suffix)) + return v.substring(0, v.length() - suffix.length()); + return v; + } + + void word(String v, String m) throws ParseException { + if (!v.equals(m)) { + System.out.println("error matching '" + v + "' '" + m + "'"); + throw new ParseException(v, 0); + } + } + + public AltosTelemetry(String line) throws ParseException { + String[] words = line.split("\\s+"); + + int i = 0; + + word (words[i++], "VERSION"); + version = parse_int(words[i++]); + + word (words[i++], "CALL"); + callsign = words[i++]; + + word (words[i++], "SERIAL"); + serial = parse_int(words[i++]); + + word (words[i++], "FLIGHT"); + flight = parse_int(words[i++]); + + word(words[i++], "RSSI"); + rssi = parse_int(words[i++]); + + word(words[i++], "STATUS"); + status = parse_hex(words[i++]); + + word(words[i++], "STATE"); + state = words[i++]; + + tick = parse_int(words[i++]); + + word(words[i++], "a:"); + accel = parse_int(words[i++]); + + word(words[i++], "p:"); + pres = parse_int(words[i++]); + + word(words[i++], "t:"); + temp = parse_int(words[i++]); + + word(words[i++], "v:"); + batt = parse_int(words[i++]); + + word(words[i++], "d:"); + drogue = parse_int(words[i++]); + + word(words[i++], "m:"); + main = parse_int(words[i++]); + + word(words[i++], "fa:"); + flight_accel = parse_int(words[i++]); + + word(words[i++], "ga:"); + ground_accel = parse_int(words[i++]); + + word(words[i++], "fv:"); + flight_vel = parse_int(words[i++]); + + word(words[i++], "fp:"); + flight_pres = parse_int(words[i++]); + + word(words[i++], "gp:"); + ground_pres = parse_int(words[i++]); + + word(words[i++], "a+:"); + accel_plus_g = parse_int(words[i++]); + + word(words[i++], "a-:"); + accel_minus_g = parse_int(words[i++]); + + word(words[i++], "GPS"); + gps = new AltosGPS(); + gps.nsat = parse_int(words[i++]); + word(words[i++], "sat"); + + gps.gps_connected = 0; + gps.gps_locked = 0; + gps.lat = gps.lon = 0; + gps.alt = 0; + if ((words[i]).equals("unlocked")) { + gps.gps_connected = 1; + gps.gps_time = new AltosGPSTime(); + i++; + } else if (words.length >= 40) { + gps.gps_locked = 1; + gps.gps_connected = 1; + + gps.gps_time = new AltosGPSTime(words[i], words[i+1]); i += 2; + gps.lat = parse_coord(words[i++]); + gps.lon = parse_coord(words[i++]); + gps.alt = parse_int(strip_suffix(words[i++], "m")); + gps.ground_speed = parse_double(strip_suffix(words[i++], "m/s(H)")); + gps.course = parse_int(strip_suffix(words[i++], "°")); + gps.climb_rate = parse_double(strip_suffix(words[i++], "m/s(V)")); + gps.hdop = parse_double(strip_suffix(words[i++], "(hdop)")); + gps.h_error = parse_int(strip_suffix(words[i++], "(herr)")); + gps.v_error = parse_int(strip_suffix(words[i++], "(verr)")); + } else { + gps.gps_time = new AltosGPSTime(); + } + word(words[i++], "SAT"); + gps_tracking = new AltosGPSTracking(); + gps_tracking.channels = parse_int(words[i++]); + gps_tracking.cc_gps_sat = new AltosGPSSat[gps_tracking.channels]; + for (int chan = 0; chan < gps_tracking.channels; chan++) { + gps_tracking.cc_gps_sat[chan] = new AltosGPSSat(); + gps_tracking.cc_gps_sat[chan].svid = parse_int(words[i++]); + gps_tracking.cc_gps_sat[chan].c_n0 = parse_int(words[i++]); + } + } +}