X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosEepromRecordMega.java;h=168b352c652a8a67707a45bb5bb6c516568ea7c4;hp=3c5f60b31170c036060f902247c352c4177aa5b0;hb=a4039a60bbe972fd022699d62ab06a8f1bc4af69;hpb=258808d296aeadaf150fa9b07e9c99a6bf1ca7c8 diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index 3c5f60b3..168b352c 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,9 @@ 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: return data32(16); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(14); @@ -41,6 +44,9 @@ 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: return data32(20); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(16); @@ -51,6 +57,9 @@ 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: return data32(24); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(18); @@ -73,8 +82,124 @@ 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 imu_type() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return AltosIMU.imu_type_telemega_v1_v2; + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + return AltosIMU.imu_type_telemega_v3; + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return AltosIMU.imu_type_easymega_v2; + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + return AltosIMU.imu_type_telemega_v4; + default: + return AltosLib.MISSING; + } + } + + 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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(); + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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(); + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + 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,80 +234,88 @@ 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(AltosFlightListener state) { - super.update_state(state); + public void provide_data(AltosDataListener listener, AltosCalData cal_data) { + super.provide_data(listener, cal_data); + AltosGPS gps; - /* 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; - } - } + cal_data.set_imu_type(imu_type()); 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(cal_data.accel_along(ground_accel_along()), + cal_data.accel_across(ground_accel_across()), + cal_data.accel_through(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; @@ -225,7 +358,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 +376,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); } }