X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosEepromRecordMega.java;h=168b352c652a8a67707a45bb5bb6c516568ea7c4;hp=6b28c50a42e67a935ad7ab723153bb8c996c3a80;hb=a4039a60bbe972fd022699d62ab06a8f1bc4af69;hpb=297eb795b24ec31f6599f48bc8c3769557a7ec6f diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index 6b28c50a..168b352c 100644 --- a/altoslib/AltosEepromRecordMega.java +++ b/altoslib/AltosEepromRecordMega.java @@ -32,6 +32,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { 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); @@ -43,6 +45,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { 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); @@ -54,6 +58,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { 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); @@ -78,6 +84,122 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int mag_x() { return data16(20); } 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 */ @@ -117,14 +239,16 @@ public class AltosEepromRecordMega extends AltosEepromRecord { AltosGPS gps; + cal_data.set_imu_type(imu_type()); + switch (cmd()) { case AltosLib.AO_LOG_FLIGHT: 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()); + 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); @@ -138,16 +262,16 @@ public class AltosEepromRecordMega extends AltosEepromRecord { listener.set_pressure(pt.pres); listener.set_temperature(pt.temp); - int accel_along = accel_y(); - int accel_across = accel_x(); - int accel_through = accel_z(); - int gyro_roll = gyro_y(); - int gyro_pitch = gyro_x(); - int gyro_yaw = gyro_z(); + 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_y(); - int mag_across = mag_x(); - int mag_through = mag_z(); + int mag_along = mag_along(); + int mag_across = mag_across(); + int mag_through = mag_through(); if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD) cal_data.check_imu_wrap(gyro_roll, gyro_pitch, gyro_yaw);