X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosEepromRecordMega.java;h=ce9dbc109e4955c40766c484b738e1d51aa947f4;hp=1c6d1aee90a39747f976d3c73ca60da7e47b93d2;hb=6dea82844075348049ce5ff9bb8670ba8efc6668;hpb=17e20a6d2dab1f4bd1375bfd9e1c5230ee2c1119 diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index 1c6d1aee..ce9dbc10 100644 --- a/altoslib/AltosEepromRecordMega.java +++ b/altoslib/AltosEepromRecordMega.java @@ -12,7 +12,7 @@ * General Public License for more details. */ -package org.altusmetrum.altoslib_11; +package org.altusmetrum.altoslib_13; public class AltosEepromRecordMega extends AltosEepromRecord { public static final int record_length = 32; @@ -31,6 +31,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int ground_roll() { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(16); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(14); @@ -41,6 +43,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int ground_pitch() { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(20); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(16); @@ -51,6 +55,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int ground_yaw() { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(24); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(18); @@ -73,8 +79,100 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int gyro_y() { return data16(16); } private int gyro_z() { return data16(18); } private int mag_x() { return data16(20); } - private int mag_y() { return data16(22); } - private int mag_z() { return data16(24); } + private int mag_z() { return data16(22); } + private int mag_y() { return data16(24); } + + private int accel_across() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return accel_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -accel_y(); + default: + return AltosLib.MISSING; + } + } + + private int accel_along(){ + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return accel_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return accel_x(); + default: + return AltosLib.MISSING; + } + } + + private int accel_through() { + return accel_z(); + } + + private int gyro_pitch() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return gyro_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -gyro_y(); + default: + return AltosLib.MISSING; + } + } + + private int gyro_roll() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return gyro_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return gyro_x(); + default: + return AltosLib.MISSING; + } + } + + private int gyro_yaw() { + return gyro_z(); + } + + private int mag_across() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return mag_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -mag_y(); + default: + return AltosLib.MISSING; + } + } + + private int mag_along() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return mag_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return mag_x(); + default: + return AltosLib.MISSING; + } + } + + private int mag_through() { + return mag_z(); + } + + private int accel() { return data16(26); } /* AO_LOG_TEMP_VOLT elements */ @@ -109,84 +207,90 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int svid(int n) { return data8(2 + n * 2); } private int c_n(int n) { return data8(2 + n * 2 + 1); } - public void update_state(AltosState state) { - super.update_state(state); - AltosGPS gps; + public void provide_data(AltosDataListener listener, AltosCalData cal_data) { + super.provide_data(listener, cal_data); - /* Flush any pending GPS changes */ - if (state.gps_pending) { - switch (cmd()) { - case AltosLib.AO_LOG_GPS_LAT: - case AltosLib.AO_LOG_GPS_LON: - case AltosLib.AO_LOG_GPS_ALT: - case AltosLib.AO_LOG_GPS_SAT: - case AltosLib.AO_LOG_GPS_DATE: - break; - default: - state.set_temp_gps(); - break; - } - } + AltosGPS gps; switch (cmd()) { case AltosLib.AO_LOG_FLIGHT: - state.set_flight(flight()); - state.set_ground_accel(ground_accel()); - state.set_ground_pressure(ground_pres()); - state.set_accel_ground(ground_accel_along(), - ground_accel_across(), - ground_accel_through()); - state.set_gyro_zero(ground_roll() / 512.0, - ground_pitch() / 512.0, - ground_yaw() / 512.0); + cal_data.set_flight(flight()); + cal_data.set_ground_accel(ground_accel()); + cal_data.set_ground_pressure(ground_pres()); + listener.set_accel_ground(ground_accel_along(), + ground_accel_across(), + ground_accel_through()); + cal_data.set_gyro_zero(ground_roll() / 512.0, + ground_pitch() / 512.0, + ground_yaw() / 512.0); break; case AltosLib.AO_LOG_STATE: - state.set_state(state()); + listener.set_state(state()); break; case AltosLib.AO_LOG_SENSOR: - state.set_ms5607(pres(), temp()); - - AltosIMU imu = new AltosIMU(accel_y(), /* along */ - accel_x(), /* across */ - accel_z(), /* through */ - gyro_y(), /* roll */ - gyro_x(), /* pitch */ - gyro_z()); /* yaw */ + AltosConfigData config_data = eeprom.config_data(); + AltosPresTemp pt = config_data.ms5607().pres_temp(pres(), temp());; + listener.set_pressure(pt.pres); + listener.set_temperature(pt.temp); + + int accel_along = accel_along(); + int accel_across = accel_across(); + int accel_through = accel_through(); + int gyro_roll = gyro_roll(); + int gyro_pitch = gyro_pitch(); + int gyro_yaw = gyro_yaw(); + + int mag_along = mag_along(); + int mag_across = mag_across(); + int mag_through = mag_through(); if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD) - state.check_imu_wrap(imu); + cal_data.check_imu_wrap(gyro_roll, gyro_pitch, gyro_yaw); + + listener.set_accel(cal_data.accel_along(accel_along), + cal_data.accel_across(accel_across), + cal_data.accel_through(accel_through)); + listener.set_gyro(cal_data.gyro_roll(gyro_roll), + cal_data.gyro_pitch(gyro_pitch), + cal_data.gyro_yaw(gyro_yaw)); + + listener.set_mag(cal_data.mag_along(mag_along), + cal_data.mag_across(mag_across), + cal_data.mag_through(mag_through)); - state.set_imu(imu); - state.set_mag(new AltosMag(mag_x(), - mag_y(), - mag_z())); + final double lsb_per_g = 1920.0/105.5; - state.set_accel(accel()); + double acceleration = AltosConvert.acceleration_from_sensor( + accel(), + cal_data.ground_accel, + cal_data.ground_accel + 2 * lsb_per_g, + cal_data.ground_accel); + listener.set_acceleration(acceleration); break; case AltosLib.AO_LOG_TEMP_VOLT: - state.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); - state.set_pyro_voltage(AltosConvert.mega_pyro_voltage(v_pbatt())); + listener.set_battery_voltage(AltosConvert.mega_battery_voltage(v_batt())); + listener.set_pyro_voltage(AltosConvert.mega_pyro_voltage(v_pbatt())); int nsense = nsense(); - state.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-2))); - state.set_main_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-1))); + listener.set_apogee_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-2))); + listener.set_main_voltage(AltosConvert.mega_pyro_voltage(sense(nsense-1))); double voltages[] = new double[nsense-2]; for (int i = 0; i < nsense-2; i++) voltages[i] = AltosConvert.mega_pyro_voltage(sense(i)); - state.set_ignitor_voltage(voltages); - state.set_pyro_fired(pyro()); + listener.set_igniter_voltage(voltages); + listener.set_pyro_fired(pyro()); break; case AltosLib.AO_LOG_GPS_TIME: - gps = state.make_temp_gps(false); + gps = listener.make_temp_gps(false); gps.lat = latitude() / 1e7; gps.lon = longitude() / 1e7; - if (state.altitude_32()) + if (config_data().altitude_32()) gps.alt = (altitude_low() & 0xffff) | (altitude_high() << 16); else gps.alt = altitude_low(); @@ -208,7 +312,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { gps.ground_speed = ground_speed() * 1.0e-2; gps.course = course() * 2; gps.climb_rate = climb_rate() * 1.0e-2; - if (state.compare_version("1.4.9") >= 0) { + if (config_data().compare_version("1.4.9") >= 0) { gps.pdop = pdop() / 10.0; gps.hdop = hdop() / 10.0; gps.vdop = vdop() / 10.0; @@ -225,7 +329,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } break; case AltosLib.AO_LOG_GPS_SAT: - gps = state.make_temp_gps(true); + gps = listener.make_temp_gps(true); int n = nsat(); if (n > max_sat) @@ -243,12 +347,12 @@ public class AltosEepromRecordMega extends AltosEepromRecord { return new AltosEepromRecordMega(eeprom, s); } - public AltosEepromRecordMega(AltosEepromNew eeprom, int start) { + public AltosEepromRecordMega(AltosEeprom eeprom, int start) { super(eeprom, start, record_length); log_format = eeprom.config_data().log_format; } - public AltosEepromRecordMega(AltosEepromNew eeprom) { + public AltosEepromRecordMega(AltosEeprom eeprom) { this(eeprom, 0); } }