From: Keith Packard Date: Fri, 2 Apr 2010 23:07:40 +0000 (-0700) Subject: Steal C code from ao-view X-Git-Tag: debian/0.6+163+g01e524f~26 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=65079f84ea64220fa928c3ad96652fed159bf74b Steal C code from ao-view --- diff --git a/ao-tools/altosui/AltosConvert.java b/ao-tools/altosui/AltosConvert.java new file mode 100644 index 00000000..3be0716c --- /dev/null +++ b/ao-tools/altosui/AltosConvert.java @@ -0,0 +1,245 @@ +/* + * 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. + */ + +/* + * Sensor data conversion functions + */ +package altosui; + +public class AltosConvert { + /* + * Pressure Sensor Model, version 1.1 + * + * written by Holly Grimes + * + * Uses the International Standard Atmosphere as described in + * "A Quick Derivation relating altitude to air pressure" (version 1.03) + * from the Portland State Aerospace Society, except that the atmosphere + * is divided into layers with each layer having a different lapse rate. + * + * Lapse rate data for each layer was obtained from Wikipedia on Sept. 1, 2007 + * at site MAXIMUM_ALTITUDE) /* FIX ME: use sensor data to improve model */ + return 0; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted altitude */ + for(layer_number = 0; layer_number < NUMBER_OF_LAYERS - 1 && altitude > base_altitude[layer_number + 1]; layer_number++) { + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + base_pressure *= Math.exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + base_pressure *= Math.pow(base, exponent); + } + base_temperature += delta_z * lapse_rate[layer_number]; + } + + /* calculate the pressure at the inputted altitude */ + delta_z = altitude - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + pressure = base_pressure * Math.exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + pressure = base_pressure * Math.pow(base, exponent); + } + + return pressure; + } + + +/* outputs the altitude associated with the given pressure. the altitude + returned is measured with respect to the mean sea level */ + static double + cc_pressure_to_altitude(double pressure) + { + + double next_base_temperature = LAYER0_BASE_TEMPERATURE; + double next_base_pressure = LAYER0_BASE_PRESSURE; + + double altitude; + double base_pressure; + double base_temperature; + double base; /* base for function to determine base pressure of next layer */ + double exponent; /* exponent for function to determine base pressure + of next layer */ + double coefficient; + int layer_number; /* identifies layer in the atmosphere */ + int delta_z; /* difference between two altitudes */ + + if (pressure < 0) /* illegal pressure */ + return -1; + if (pressure < MINIMUM_PRESSURE) /* FIX ME: use sensor data to improve model */ + return MAXIMUM_ALTITUDE; + + /* calculate the base temperature and pressure for the atmospheric layer + associated with the inputted pressure. */ + layer_number = -1; + do { + layer_number++; + base_pressure = next_base_pressure; + base_temperature = next_base_temperature; + delta_z = base_altitude[layer_number + 1] - base_altitude[layer_number]; + if (lapse_rate[layer_number] == 0.0) { + exponent = GRAVITATIONAL_ACCELERATION * delta_z + / AIR_GAS_CONSTANT / base_temperature; + next_base_pressure *= Math.exp(exponent); + } + else { + base = (lapse_rate[layer_number] * delta_z / base_temperature) + 1.0; + exponent = GRAVITATIONAL_ACCELERATION / + (AIR_GAS_CONSTANT * lapse_rate[layer_number]); + next_base_pressure *= Math.pow(base, exponent); + } + next_base_temperature += delta_z * lapse_rate[layer_number]; + } + while(layer_number < NUMBER_OF_LAYERS - 1 && pressure < next_base_pressure); + + /* calculate the altitude associated with the inputted pressure */ + if (lapse_rate[layer_number] == 0.0) { + coefficient = (AIR_GAS_CONSTANT / GRAVITATIONAL_ACCELERATION) + * base_temperature; + altitude = base_altitude[layer_number] + + coefficient * Math.log(pressure / base_pressure); + } + else { + base = pressure / base_pressure; + exponent = AIR_GAS_CONSTANT * lapse_rate[layer_number] + / GRAVITATIONAL_ACCELERATION; + coefficient = base_temperature / lapse_rate[layer_number]; + altitude = base_altitude[layer_number] + + coefficient * (Math.pow(base, exponent) - 1); + } + + return altitude; + } + + /* + * Values for our MP3H6115A pressure sensor + * + * From the data sheet: + * + * Pressure range: 15-115 kPa + * Voltage at 115kPa: 2.82 + * Output scale: 27mV/kPa + * + * + * 27 mV/kPa * 2047 / 3300 counts/mV = 16.75 counts/kPa + * 2.82V * 2047 / 3.3 counts/V = 1749 counts/115 kPa + */ + + static final double counts_per_kPa = 27 * 2047 / 3300; + static final double counts_at_101_3kPa = 1674.0; + + static double + cc_barometer_to_pressure(double count) + { + return ((count / 16.0) / 2047.0 + 0.095) / 0.009 * 1000.0; + } + + static double + cc_barometer_to_altitude(double baro) + { + double Pa = cc_barometer_to_pressure(baro); + return cc_pressure_to_altitude(Pa); + } + + static final double count_per_mss = 27.0; + + static double + cc_accelerometer_to_acceleration(double accel, double ground_accel) + { + return (ground_accel - accel) / count_per_mss; + } + + /* Value for the CC1111 built-in temperature sensor + * Output voltage at 0°C = 0.755V + * Coefficient = 0.00247V/°C + * Reference voltage = 1.25V + * + * temp = ((value / 32767) * 1.25 - 0.755) / 0.00247 + * = (value - 19791.268) / 32768 * 1.25 / 0.00247 + */ + + static double + cc_thermometer_to_temperature(double thermo) + { + return (thermo - 19791.268) / 32728.0 * 1.25 / 0.00247; + } + + static double + cc_battery_to_voltage(double battery) + { + return battery / 32767.0 * 5.0; + } + + static double + cc_ignitor_to_voltage(double ignite) + { + return ignite / 32767 * 15.0; + } +} diff --git a/ao-tools/altosui/AltosGPS.java b/ao-tools/altosui/AltosGPS.java new file mode 100644 index 00000000..d242ad57 --- /dev/null +++ b/ao-tools/altosui/AltosGPS.java @@ -0,0 +1,118 @@ +/* + * 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.*; +import altosui.AltosParse; + + +public class AltosGPS { + public class AltosGPSTime { + int year; + int month; + int day; + int hour; + int minute; + int second; + + public AltosGPSTime(String date, String time) throws ParseException { + String[] ymd = date.split("-"); + if (ymd.length != 3) + throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0); + year = AltosParse.parse_int(ymd[0]); + month = AltosParse.parse_int(ymd[1]); + day = AltosParse.parse_int(ymd[2]); + + String[] hms = time.split(":"); + if (hms.length != 3) + throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 0); + hour = AltosParse.parse_int(hms[0]); + minute = AltosParse.parse_int(hms[1]); + second = AltosParse.parse_int(hms[2]); + } + + public AltosGPSTime() { + year = month = day = 0; + hour = minute = second = 0; + } + + } + + public class AltosGPSSat { + int svid; + int c_n0; + } + + int nsat; + boolean gps_locked; + boolean 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 */ + + AltosGPSSat[] cc_gps_sat; /* tracking data */ + + public AltosGPS(String[] words, int i) throws ParseException { + AltosParse.word(words[i++], "GPS"); + nsat = AltosParse.parse_int(words[i++]); + AltosParse.word(words[i++], "sat"); + + gps_connected = false; + gps_locked = false; + lat = lon = 0; + alt = 0; + if ((words[i]).equals("unlocked")) { + gps_connected = true; + gps_time = new AltosGPSTime(); + i++; + } else if (words.length >= 40) { + gps_locked = true; + gps_connected = true; + + gps_time = new AltosGPSTime(words[i], words[i+1]); i += 2; + lat = AltosParse.parse_coord(words[i++]); + lon = AltosParse.parse_coord(words[i++]); + alt = AltosParse.parse_int(AltosParse.strip_suffix(words[i++], "m")); + ground_speed = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(H)")); + course = AltosParse.parse_int(AltosParse.strip_suffix(words[i++], "°")); + climb_rate = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "m/s(V)")); + hdop = AltosParse.parse_double(AltosParse.strip_suffix(words[i++], "(hdop)")); + h_error = AltosParse.parse_int(AltosParse.strip_suffix(words[i++], "(herr)")); + v_error = AltosParse.parse_int(AltosParse.strip_suffix(words[i++], "(verr)")); + } else { + gps_time = new AltosGPSTime(); + } + AltosParse.word(words[i++], "SAT"); + int tracking_channels = AltosParse.parse_int(words[i++]); + cc_gps_sat = new AltosGPS.AltosGPSSat[tracking_channels]; + for (int chan = 0; chan < tracking_channels; chan++) { + cc_gps_sat[chan].svid = AltosParse.parse_int(words[i++]); + cc_gps_sat[chan].c_n0 = AltosParse.parse_int(words[i++]); + } + } +} diff --git a/ao-tools/altosui/AltosGreatCircle.java b/ao-tools/altosui/AltosGreatCircle.java new file mode 100644 index 00000000..878da03e --- /dev/null +++ b/ao-tools/altosui/AltosGreatCircle.java @@ -0,0 +1,66 @@ +/* + * 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.Math; + +public class AltosGreatCircle { + double distance; + double bearing; + + double sqr(double a) { return a * a; } + + static final double rad = Math.PI / 180; + static final double earth_radius = 6371.2 * 1000; /* in meters */ + + AltosGreatCircle (double start_lat, double start_lon, + double end_lat, double end_lon) + { + double lat1 = rad * start_lat; + double lon1 = rad * -start_lon; + double lat2 = rad * end_lat; + double lon2 = rad * -end_lon; + + double d_lon = lon2 - lon1; + + /* From http://en.wikipedia.org/wiki/Great-circle_distance */ + double vdn = Math.sqrt(sqr(Math.cos(lat2) * Math.sin(d_lon)) + + sqr(Math.cos(lat1) * Math.sin(lat2) - + Math.sin(lat1) * Math.cos(lat2) * Math.cos(d_lon))); + double vdd = Math.sin(lat1) * Math.sin(lat2) + Math.cos(lat1) * Math.cos(lat2) * Math.cos(d_lon); + double d = Math.atan2(vdn,vdd); + double course; + + if (Math.cos(lat1) < 1e-20) { + if (lat1 > 0) + course = Math.PI; + else + course = -Math.PI; + } else { + if (d < 1e-10) + course = 0; + else + course = Math.acos((Math.sin(lat2)-Math.sin(lat1)*Math.cos(d)) / + (Math.sin(d)*Math.cos(lat1))); + if (Math.sin(lon2-lon1) > 0) + course = 2 * Math.PI-course; + } + distance = d * earth_radius; + bearing = course * 180/Math.PI; + } +} diff --git a/ao-tools/altosui/AltosParse.java b/ao-tools/altosui/AltosParse.java new file mode 100644 index 00000000..a60dc694 --- /dev/null +++ b/ao-tools/altosui/AltosParse.java @@ -0,0 +1,75 @@ +/* + * 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.text.*; +import java.lang.*; + +public class AltosParse { + static int parse_int(String v) throws ParseException { + try { + return Integer.parseInt(v); + } catch (NumberFormatException e) { + throw new ParseException("error parsing int " + v, 0); + } + } + + static int parse_hex(String v) throws ParseException { + try { + return Integer.parseInt(v, 16); + } catch (NumberFormatException e) { + throw new ParseException("error parsing hex " + v, 0); + } + } + + static double parse_double(String v) throws ParseException { + try { + return Double.parseDouble(v); + } catch (NumberFormatException e) { + throw new ParseException("error parsing double " + v, 0); + } + } + + static double parse_coord(String coord) throws ParseException { + String[] dsf = coord.split("\\D+"); + + if (dsf.length != 3) { + throw new ParseException("error parsing coord " + 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; + } + + static String strip_suffix(String v, String suffix) { + if (v.endsWith(suffix)) + return v.substring(0, v.length() - suffix.length()); + return v; + } + + static void word(String v, String m) throws ParseException { + if (!v.equals(m)) { + throw new ParseException("error matching '" + v + "' '" + m + "'", 0); + } + } +} diff --git a/ao-tools/altosui/AltosState.java b/ao-tools/altosui/AltosState.java new file mode 100644 index 00000000..da465c75 --- /dev/null +++ b/ao-tools/altosui/AltosState.java @@ -0,0 +1,166 @@ +/* + * 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. + */ + +/* + * Track flight state from telemetry data stream + */ + +package altosui; + +import altosui.AltosTelemetry; +import altosui.AltosGPS; + +public class AltosState { + AltosTelemetry data; + AltosTelemetry prev_data; + + /* derived data */ + + double report_time; + + int state; + boolean ascent; /* going up? */ + + double ground_altitude; + double height; + double speed; + double acceleration; + double battery; + double temperature; + double main_sense; + double drogue_sense; + double baro_speed; + + double max_height; + double max_acceleration; + double max_speed; + + AltosGPS gps; + AltosGPSTracking gps_tracking; + + boolean gps_valid; + double pad_lat; + double pad_lon; + double pad_alt; + double pad_lat_total; + double pad_lon_total; + double pad_alt_total; + int npad; + int prev_npad; + + AltosGreatCircle from_pad; + + double gps_height; + + int speak_tick; + double speak_altitude; + + static double + aoview_time() + { + return System.currentTimeMillis() / 1000.0; + } + + public AltosState (AltosTelemetry cur, AltosTelemetry prev, int prev_npad) { + int i; + double new_height; + double height_change; + double time_change; + double accel_counts_per_mss; + int tick_count; + + data = cur; + prev_data = prev; + npad = prev_npad; + tick_count = data.tick; + if (tick_count < prev_data.tick) + tick_count += 65536; + time_change = (tick_count - prev_data.tick) / 100.0; + + report_time = aoview_time(); + + ground_altitude = AltosConvert.cc_pressure_to_altitude(data.ground_pres); + new_height = AltosConvert.cc_pressure_to_altitude(data.flight_pres) - ground_altitude; + height_change = new_height - height; + height = new_height; + if (time_change > 0) + baro_speed = (baro_speed * 3 + (height_change / time_change)) / 4.0; + accel_counts_per_mss = ((data.accel_minus_g - data.accel_plus_g) / 2.0) / 9.80665; + acceleration = (data.ground_accel - data.flight_accel) / accel_counts_per_mss; + speed = data.flight_vel / (accel_counts_per_mss * 100.0); + temperature = AltosConvert.cc_thermometer_to_temperature(data.temp); + drogue_sense = AltosConvert.cc_ignitor_to_voltage(data.drogue); + main_sense = AltosConvert.cc_ignitor_to_voltage(data.main); + battery = AltosConvert.cc_battery_to_voltage(data.batt); + state = data.state(); + if (state == AltosTelemetry.ao_flight_pad) { + if (data.gps.gps_locked && data.gps.nsat >= 4) { + npad++; + pad_lat_total += data.gps.lat; + pad_lon_total += data.gps.lon; + pad_alt_total += data.gps.alt; + if (npad > 1) { + pad_lat = (pad_lat * 31 + data.gps.lat) / 32.0; + pad_lon = (pad_lon * 31 + data.gps.lon) / 32.0; + pad_alt = (pad_alt * 31 + data.gps.alt) / 32.0; + } else { + pad_lat = data.gps.lat; + pad_lon = data.gps.lon; + pad_alt = data.gps.alt; + } + } + } + ascent = (AltosTelemetry.ao_flight_boost <= state && + state <= AltosTelemetry.ao_flight_coast); + + /* Only look at accelerometer data on the way up */ + if (ascent && acceleration > max_acceleration) + max_acceleration = acceleration; + if (ascent && speed > max_speed) + max_speed = speed; + + if (height > max_height) + max_height = height; + gps.gps_locked = data.gps.gps_locked; + gps.gps_connected = data.gps.gps_connected; + if (data.gps.gps_locked) { + gps = data.gps; + gps_valid = true; + if (npad > 0) + from_pad = new AltosGreatCircle(pad_lat, pad_lon, gps.lat, gps.lon); + } + if (npad > 0) { + gps_height = gps.alt - pad_alt; + } else { + gps_height = 0; + } + } + + public AltosState(AltosTelemetry cur) { + this(cur, cur, 0); + } + + public AltosState (AltosTelemetry cur, AltosState prev) { + this(cur, prev.data, prev.npad); + if (gps == null) { + gps = prev.gps; + gps_valid = prev.gps_valid; + } + if (gps_tracking == null) + gps_tracking = prev.gps_tracking; + } +} diff --git a/ao-tools/altosui/AltosTelemetry.java b/ao-tools/altosui/AltosTelemetry.java index 99e82bbf..34b4099f 100644 --- a/ao-tools/altosui/AltosTelemetry.java +++ b/ao-tools/altosui/AltosTelemetry.java @@ -19,77 +19,14 @@ package altosui; import java.lang.*; import java.text.*; +import java.util.HashMap; +import altosui.AltosConvert; +import altosui.AltosGPS; /* * 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("error parsing GPS value " + v, 0); - } - } - - public AltosGPSTime(String date, String time) throws ParseException { - String[] ymd = date.split("-"); - if (ymd.length != 3) - throw new ParseException("error parsing GPS date " + date + " got " + ymd.length, 0); - year = parse_int(ymd[0]); - month = parse_int(ymd[1]); - day = parse_int(ymd[2]); - - String[] hms = time.split(":"); - if (hms.length != 3) - throw new ParseException("Error parsing GPS time " + time + " got " + hms.length, 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; -} /* * The telemetry data stream is a bit of a mess at present, with no consistent @@ -137,58 +74,44 @@ public class AltosTelemetry { 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) { - throw new ParseException("error parsing int " + v, 0); - } - } - int parse_hex(String v) throws ParseException { - try { - return Integer.parseInt(v, 16); - } catch (NumberFormatException e) { - throw new ParseException("error parsing hex " + v, 0); - } + public static final int ao_flight_startup = 0; + public static final int ao_flight_idle = 1; + public static final int ao_flight_pad = 2; + public static final int ao_flight_boost = 3; + public static final int ao_flight_fast = 4; + public static final int ao_flight_coast = 5; + public static final int ao_flight_drogue = 6; + public static final int ao_flight_main = 7; + public static final int ao_flight_landed = 8; + public static final int ao_flight_invalid = 9; + + static HashMap states = new HashMap(); + { + states.put("startup", ao_flight_startup); + states.put("idle", ao_flight_idle); + states.put("pad", ao_flight_pad); + states.put("boost", ao_flight_boost); + states.put("fast", ao_flight_fast); + states.put("coast", ao_flight_coast); + states.put("drogue", ao_flight_drogue); + states.put("main", ao_flight_main); + states.put("landed", ao_flight_landed); + states.put("invalid", ao_flight_invalid); } - double parse_double(String v) throws ParseException { - try { - return Double.parseDouble(v); - } catch (NumberFormatException e) { - throw new ParseException("error parsing double " + v, 0); - } - } - - double parse_coord(String coord) throws ParseException { - String[] dsf = coord.split("\\D+"); - - if (dsf.length != 3) { - throw new ParseException("error parsing coord " + 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; + public int state() { + if (states.containsKey(state)) + return states.get(state); + return ao_flight_invalid; } - String strip_suffix(String v, String suffix) { - if (v.endsWith(suffix)) - return v.substring(0, v.length() - suffix.length()); - return v; + public double altitude() { + return AltosConvert.cc_pressure_to_altitude(AltosConvert.cc_barometer_to_pressure(pres)); } - void word(String v, String m) throws ParseException { - if (!v.equals(m)) { - throw new ParseException("error matching '" + v + "' '" + m + "'", 0); - } + public double pad_altitude() { + return AltosConvert.cc_pressure_to_altitude(AltosConvert.cc_barometer_to_pressure(ground_pres)); } public AltosTelemetry(String line) throws ParseException { @@ -196,106 +119,67 @@ public class AltosTelemetry { int i = 0; - word (words[i++], "VERSION"); - version = parse_int(words[i++]); + AltosParse.word (words[i++], "VERSION"); + version = AltosParse.parse_int(words[i++]); - word (words[i++], "CALL"); + AltosParse.word (words[i++], "CALL"); callsign = words[i++]; - word (words[i++], "SERIAL"); - serial = parse_int(words[i++]); + AltosParse.word (words[i++], "SERIAL"); + serial = AltosParse.parse_int(words[i++]); - word (words[i++], "FLIGHT"); - flight = parse_int(words[i++]); + AltosParse.word (words[i++], "FLIGHT"); + flight = AltosParse.parse_int(words[i++]); - word(words[i++], "RSSI"); - rssi = parse_int(words[i++]); + AltosParse.word(words[i++], "RSSI"); + rssi = AltosParse.parse_int(words[i++]); - word(words[i++], "STATUS"); - status = parse_hex(words[i++]); + AltosParse.word(words[i++], "STATUS"); + status = AltosParse.parse_hex(words[i++]); - word(words[i++], "STATE"); + AltosParse.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++]); - } + 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:"); + drogue = 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++]); + + AltosParse.word(words[i++], "gp:"); + ground_pres = AltosParse.parse_int(words[i++]); + + 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++]); + } } diff --git a/ao-tools/altosui/AltosUI.java b/ao-tools/altosui/AltosUI.java index 89eaac15..21c3e7a2 100644 --- a/ao-tools/altosui/AltosUI.java +++ b/ao-tools/altosui/AltosUI.java @@ -20,6 +20,8 @@ package altosui; import java.awt.*; import java.awt.event.*; import javax.swing.*; +import javax.swing.filechooser.FileNameExtensionFilter; +import javax.swing.table.AbstractTableModel; import java.io.*; import java.util.*; import java.text.*; @@ -27,6 +29,8 @@ import gnu.io.CommPortIdentifier; import altosui.AltosSerial; import altosui.AltosSerialMonitor; +import altosui.AltosTelemetry; +import altosui.AltosState; class AltosUIMonitor implements AltosSerialMonitor { public void data(String data) { @@ -34,6 +38,32 @@ class AltosUIMonitor implements AltosSerialMonitor { } } +class AltosFlightStatusTableModel extends AbstractTableModel { + private String[] columnNames = {"Height (m)", "State", "RSSI (dBm)", "Speed (m/s)" }; + private Object[] data = { 0, "idle", 0, 0 }; + + public int getColumnCount() { return columnNames.length; } + public int getRowCount() { return 1; } + public String getColumnName(int col) { return columnNames[col]; } + public Object getValueAt(int row, int col) { return data[col]; } + + public void setValueAt(Object value, int col) { + data[col] = value; + fireTableCellUpdated(0, col); + } + + public void setValueAt(Object value, int row, int col) { + setValueAt(value, col); + } + + public void set(AltosState state) { + setValueAt(state.height, 0); + setValueAt(state.data.state, 1); + setValueAt(state.data.rssi, 2); + setValueAt(state.speed, 3); + } +} + public class AltosUI extends JFrame { private int channel = -1; @@ -71,6 +101,31 @@ public class AltosUI extends JFrame { } final JFileChooser deviceChooser = new JFileChooser(); + final JFileChooser logdirChooser = new JFileChooser(); + final String logdirName = "TeleMetrum"; + File logdir = null; + + private void setLogdir() { + if (logdir == null) + logdir = new File(logdirChooser.getCurrentDirectory(), logdirName); + logdirChooser.setCurrentDirectory(logdir); + } + + private void makeLogdir() { + setLogdir(); + if (!logdir.exists()) { + if (!logdir.mkdirs()) + JOptionPane.showMessageDialog(AltosUI.this, + logdir.getName(), + "Cannot create directory", + JOptionPane.ERROR_MESSAGE); + } else if (!logdir.isDirectory()) { + JOptionPane.showMessageDialog(AltosUI.this, + logdir.getName(), + "Is not a directory", + JOptionPane.ERROR_MESSAGE); + } + } private void PickSerialDevice() { java.util.Enumeration port_list = CommPortIdentifier.getPortIdentifiers(); @@ -111,16 +166,23 @@ public class AltosUI extends JFrame { return null; } + /* + * Open an existing telemetry file and replay it in realtime + */ + private void Replay() { -// int returnVal = deviceChooser.showOpenDialog(AltosUI.this); + setLogdir(); + logdirChooser.setDialogTitle("Select Telemetry File"); + logdirChooser.setFileFilter(new FileNameExtensionFilter("Telemetry file", "telem")); + int returnVal = logdirChooser.showOpenDialog(AltosUI.this); - /* if (returnVal == JFileChooser.APPROVE_OPTION) */ { -// File file = deviceChooser.getSelectedFile(); -// String filename = file.getName(); - String filename = "/home/keithp/src/cc1111/flights/2010-02-13-serial-051-flight-002.telem"; + if (returnVal == JFileChooser.APPROVE_OPTION) { + File file = logdirChooser.getSelectedFile(); + if (file == null) + System.out.println("No file selected?"); + String filename = file.getName(); try { -// FileInputStream replay = new FileInputStream(file); - FileInputStream replay = new FileInputStream(filename); + FileInputStream replay = new FileInputStream(file); String line; try { diff --git a/ao-tools/altosui/Makefile b/ao-tools/altosui/Makefile index 090911ef..1fb964d6 100644 --- a/ao-tools/altosui/Makefile +++ b/ao-tools/altosui/Makefile @@ -1,7 +1,17 @@ .SUFFIXES: .java .class CLASSPATH=..:/usr/share/java/* -CLASSFILES=AltosSerialMonitor.class AltosSerial.class AltosTelemetry.class AltosUI.class +CLASSFILES=\ + AltosConvert.class \ + AltosGPS.class \ + AltosGreatCircle.class \ + AltosParse.class \ + AltosSerialMonitor.class \ + AltosSerial.class \ + AltosState.class \ + AltosTelemetry.class \ + AltosUI.class + JAVAFLAGS=-Xlint:unchecked all: $(CLASSFILES) altosui