From 42ff9c7c096ad2b8e9a6152292ab12d315985aff Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 12 Oct 2020 16:48:42 -0700 Subject: [PATCH] altoslib: Add EasyMotor EEPROM support Signed-off-by: Keith Packard --- altoslib/AltosEepromRecordMotor.java | 101 +++++++++++++++++++++++++++ altoslib/AltosEepromRecordSet.java | 3 + altoslib/AltosIMU.java | 17 ++++- altoslib/AltosLib.java | 1 + altoslib/Makefile.am | 1 + 5 files changed, 120 insertions(+), 3 deletions(-) create mode 100644 altoslib/AltosEepromRecordMotor.java diff --git a/altoslib/AltosEepromRecordMotor.java b/altoslib/AltosEepromRecordMotor.java new file mode 100644 index 00000000..656feb9b --- /dev/null +++ b/altoslib/AltosEepromRecordMotor.java @@ -0,0 +1,101 @@ +/* + * Copyright © 2020 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, either version 2 of the License, or + * (at your option) any later version. + * + * 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. + */ + +package org.altusmetrum.altoslib_14; + +public class AltosEepromRecordMotor extends AltosEepromRecord { + public static final int record_length = 16; + + private int log_format; + + /* AO_LOG_FLIGHT elements */ + private int flight() { return data16(0); } + private int ground_accel() { return data16(2); } + private int ground_accel_along() { return data16(4); } + private int ground_accel_across() { return data16(6); } + private int ground_accel_through() { return data16(8); } + + /* AO_LOG_STATE elements */ + private int state() { return data16(0); } + private int reason() { return data16(2); } + + /* AO_LOG_SENSOR elements */ + private int motor_pres() { return data16(0); } + private int v_batt() { return data16(2); } + private int accel() { return data16(4); } + private int accel_across() { return data16(6); } + private int accel_along() { return data16(8); } + private int accel_through() { return data16(10); } + + private int imu_type() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_EASYMOTOR: + return AltosIMU.imu_type_easymotor_v2; + default: + return AltosLib.MISSING; + } + } + + public void provide_data(AltosDataListener listener, AltosCalData cal_data) { + super.provide_data(listener, cal_data); + + 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()); + 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())); + break; + case AltosLib.AO_LOG_STATE: + listener.set_state(state()); + break; + case AltosLib.AO_LOG_SENSOR: + AltosConfigData config_data = eeprom.config_data(); + + listener.set_battery_voltage(AltosConvert.easy_mini_2_voltage(v_batt())); + double pa = AltosConvert.easy_motor_2_motor_pressure(motor_pres()); + listener.set_motor_pressure(pa); + + int accel_along = accel_along(); + int accel_across = accel_across(); + int accel_through = accel_through(); + + listener.set_accel(cal_data.accel_along(accel_along), + cal_data.accel_across(accel_across), + cal_data.accel_through(accel_through)); + + listener.set_acceleration(cal_data.acceleration(accel())); + break; + } + } + + public AltosEepromRecord next() { + int s = next_start(); + if (s < 0) + return null; + return new AltosEepromRecordMotor(eeprom, s); + } + + public AltosEepromRecordMotor(AltosEeprom eeprom, int start) { + super(eeprom, start, record_length); + log_format = eeprom.config_data().log_format; + } + + public AltosEepromRecordMotor(AltosEeprom eeprom) { + this(eeprom, 0); + } +} diff --git a/altoslib/AltosEepromRecordSet.java b/altoslib/AltosEepromRecordSet.java index 689eaea6..2a7d97e3 100644 --- a/altoslib/AltosEepromRecordSet.java +++ b/altoslib/AltosEepromRecordSet.java @@ -106,6 +106,9 @@ public class AltosEepromRecordSet implements AltosRecordSet { case AltosLib.AO_LOG_FORMAT_MICROPEAK2: record = new AltosEepromRecordMicroPeak2(eeprom); break; + case AltosLib.AO_LOG_FORMAT_EASYMOTOR: + record = new AltosEepromRecordMotor(eeprom); + break; } ordered = new TreeSet(); diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 7f8be403..e66005f2 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -36,6 +36,7 @@ public class AltosIMU implements Cloneable { public static final double counts_per_g_mpu = 2048.0; public static final double counts_per_g_bmx = 2048.0; + public static final double counts_per_g_adxl = 20.5; private static double counts_per_g(int imu_type) { switch (imu_type) { @@ -47,6 +48,8 @@ public class AltosIMU implements Cloneable { case imu_type_telemega_v4: case imu_type_easytimer_v1: return counts_per_g_bmx; + case imu_type_easymotor_v2: + return counts_per_g_bmx; default: return AltosLib.MISSING; } @@ -161,6 +164,8 @@ public class AltosIMU implements Cloneable { public static final int imu_type_easytimer_v1 = 5; /* BMX160 */ + public static final int imu_type_easymotor_v2 = 6; /* ADXL375 (accel only) */ + private int accel_across(int imu_type) { switch (imu_type) { case imu_type_telemega_v1_v2: @@ -172,6 +177,8 @@ public class AltosIMU implements Cloneable { case imu_type_telemega_v4: case imu_type_easytimer_v1: return -accel_y; + case imu_type_easymotor_v2: + return accel_y; default: return AltosLib.MISSING; } @@ -187,6 +194,8 @@ public class AltosIMU implements Cloneable { case imu_type_telemega_v4: case imu_type_easytimer_v1: return accel_x; + case imu_type_easymotor_v2: + return accel_x; default: return AltosLib.MISSING; } @@ -308,9 +317,11 @@ public class AltosIMU implements Cloneable { cal_data.set_imu_type(imu_type); if (imu != null) { - listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)), - cal_data.gyro_pitch(imu.gyro_pitch(imu_type)), - cal_data.gyro_yaw(imu.gyro_yaw(imu_type))); + if (imu.gyro_x != AltosLib.MISSING) { + listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)), + cal_data.gyro_pitch(imu.gyro_pitch(imu_type)), + cal_data.gyro_yaw(imu.gyro_yaw(imu_type))); + } listener.set_accel_ground(cal_data.accel_along(imu.accel_along(imu_type)), cal_data.accel_across(imu.accel_across(imu_type)), cal_data.accel_through(imu.accel_through(imu_type))); diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index a7f27830..2d5083cf 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -382,6 +382,7 @@ public class AltosLib { public static final int AO_LOG_FORMAT_TELESTATIC = 17; public static final int AO_LOG_FORMAT_MICROPEAK2 = 18; public static final int AO_LOG_FORMAT_TELEMEGA_4 = 19; + public static final int AO_LOG_FORMAT_EASYMOTOR = 20; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 0c9392d1..75af5252 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -49,6 +49,7 @@ altoslib_JAVA = \ AltosEepromRecordGps.java \ AltosEepromRecordFireTwo.java \ AltosEepromRecordMicroPeak2.java \ + AltosEepromRecordMotor.java \ AltosEepromRecordSet.java \ AltosEepromChunk.java \ AltosEepromDownload.java \ -- 2.30.2