From 40c6aa050654d43f20c6a9c6bd67541e1747ae38 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 23 Oct 2020 16:10:43 -0700 Subject: [PATCH] altoslib, altosuilib: Get Idle Monitor working with EasyTimer and EasyMotor This involve splitting out the gyro and mag sensor handling from the 3-d accel stuff, displaying only information that is present. The IMU support now allows for using the along axis as the primary acceleration indicator. The Adxl375 now allows using all three axes as the 3d accelerometer. Signed-off-by: Keith Packard --- altoslib/AltosAdxl375.java | 44 ++++++++++++++++---- altoslib/AltosIMU.java | 33 +++++++++++++-- altoslib/AltosIdleFetch.java | 13 +++++- altoslib/AltosSensorEasyMotor2.java | 63 +++++++++++++++++++++++++++++ altoslib/Makefile.am | 1 + altosuilib/AltosInfoTable.java | 16 ++++---- 6 files changed, 152 insertions(+), 18 deletions(-) create mode 100644 altoslib/AltosSensorEasyMotor2.java diff --git a/altoslib/AltosAdxl375.java b/altoslib/AltosAdxl375.java index 621cceeb..5e490541 100644 --- a/altoslib/AltosAdxl375.java +++ b/altoslib/AltosAdxl375.java @@ -22,7 +22,7 @@ import java.util.concurrent.*; public class AltosAdxl375 implements Cloneable { - private int accel; + private int[] accels = new int[3]; private int axis; public static final int X_AXIS = 0; @@ -35,7 +35,8 @@ public class AltosAdxl375 implements Cloneable { if (axis == AltosLib.MISSING) throw new NumberFormatException("No ADXL375 axis specified"); if (items.length >= 3) { - accel = Integer.parseInt(items[2 + axis]); + for (int i = 0; i < 3; i++) + accels[i] = Integer.parseInt(items[2+i]); return true; } } @@ -45,22 +46,50 @@ public class AltosAdxl375 implements Cloneable { public AltosAdxl375 clone() { AltosAdxl375 n = new AltosAdxl375(axis); - n.accel = accel; + for (int i = 0; i < 3; i++) + n.accels[i] = accels[i]; return n; } - static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException, AltosUnknownProduct { + private int accel_along() { + return accels[axis]; + } + + private int accel_across() { + if (axis == X_AXIS) + return accels[Y_AXIS]; + else + return accels[X_AXIS]; + } + + private int accel_through() { + return accels[Z_AXIS]; + } + + static public void provide_data(AltosDataListener listener, AltosLink link, boolean three_axis, int imu_type) throws InterruptedException, AltosUnknownProduct { try { AltosCalData cal_data = listener.cal_data(); AltosAdxl375 adxl375 = new AltosAdxl375(link, cal_data.adxl375_axis); if (adxl375 != null) { - int accel = adxl375.accel; - if (cal_data.adxl375_inverted) + int accel = adxl375.accel_along(); + if (!cal_data.adxl375_inverted) accel = -accel; if (cal_data.pad_orientation == 1) accel = -accel; listener.set_acceleration(cal_data.acceleration(accel)); + if (three_axis) { + cal_data.set_imu_type(imu_type); + double accel_along = cal_data.accel_along(-accel); + double accel_across = cal_data.accel_across(adxl375.accel_across()); + double accel_through = cal_data.accel_through(adxl375.accel_through()); + listener.set_accel_ground(accel_along, + accel_across, + accel_through); + listener.set_accel(accel_along, + accel_across, + accel_through); + } } } catch (TimeoutException te) { } catch (NumberFormatException ne) { @@ -68,7 +97,8 @@ public class AltosAdxl375 implements Cloneable { } public AltosAdxl375() { - accel = AltosLib.MISSING; + for (int i = 0; i < 3; i++) + accels[i] = AltosLib.MISSING; axis = AltosLib.MISSING; } diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index 2944a2a5..f3c85e9a 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -56,7 +56,10 @@ public class AltosIMU implements Cloneable { } public static double convert_accel(double counts, int imu_type) { - return counts / counts_per_g(imu_type) * AltosConvert.gravity; + double cpg = counts_per_g(imu_type); + if (cpg == AltosLib.MISSING) + return AltosLib.MISSING; + return counts / cpg * AltosConvert.gravity; } public static final double GYRO_FULLSCALE_DEGREES_MPU = 2000.0; @@ -82,7 +85,10 @@ public class AltosIMU implements Cloneable { } public static double gyro_degrees_per_second(double counts, int imu_type) { - return counts / counts_per_degree(imu_type); + double cpd = counts_per_degree(imu_type); + if (cpd == AltosLib.MISSING) + return AltosLib.MISSING; + return counts / cpd; } public static final int imu_axis_x = 0; @@ -112,7 +118,10 @@ public class AltosIMU implements Cloneable { } public static double convert_gauss(double counts, int imu_type, int imu_axis) { - return counts / counts_per_gauss(imu_type, imu_axis); + double cpg = counts_per_gauss(imu_type, imu_axis); + if (cpg == AltosLib.MISSING) + return AltosLib.MISSING; + return counts / cpg; } public boolean parse_string(String line) { @@ -301,6 +310,15 @@ public class AltosIMU implements Cloneable { } } + private static boolean is_primary_accel(int imu_type) { + switch (imu_type) { + case imu_type_easytimer_v1: + return true; + default: + return false; + } + } + public static int mag_through_axis(int imu_type) { return imu_axis_z; } @@ -318,6 +336,7 @@ public class AltosIMU implements Cloneable { if (imu != null) { if (imu.gyro_x != AltosLib.MISSING) { + cal_data.set_gyro_zero(0, 0, 0); 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))); @@ -328,6 +347,14 @@ public class AltosIMU implements Cloneable { listener.set_accel(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))); + if (is_primary_accel(imu_type)) { + int accel = imu.accel_along(imu_type); + if (!cal_data.adxl375_inverted) + accel = -accel; + if (cal_data.pad_orientation == 1) + accel = -accel; + listener.set_acceleration(cal_data.acceleration(accel)); + } if (imu.mag_x != AltosLib.MISSING) { listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)), cal_data.mag_across(imu.mag_across(imu_type)), diff --git a/altoslib/AltosIdleFetch.java b/altoslib/AltosIdleFetch.java index 1ac075e3..c4a32788 100644 --- a/altoslib/AltosIdleFetch.java +++ b/altoslib/AltosIdleFetch.java @@ -38,6 +38,7 @@ class AltosIdler { static final int idle_mma655x = 8; static final int idle_ms5607 = 9; static final int idle_adxl375 = 10; + static final int idle_adxl375_easymotor_v2 = 11; static final int idle_sensor_tm = 100; static final int idle_sensor_metrum = 101; @@ -49,6 +50,7 @@ class AltosIdler { static final int idle_sensor_tgps2 = 107; static final int idle_sensor_tmini3 = 108; static final int idle_sensor_easytimer1 = 109; + static final int idle_sensor_easymotor2 = 110; public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException, TimeoutException, AltosUnknownProduct { for (int idler : idlers) { @@ -81,7 +83,10 @@ class AltosIdler { AltosMma655x.provide_data(listener, link); break; case idle_adxl375: - AltosAdxl375.provide_data(listener, link); + AltosAdxl375.provide_data(listener, link, false, AltosLib.MISSING); + break; + case idle_adxl375_easymotor_v2: + AltosAdxl375.provide_data(listener, link, true, AltosIMU.imu_type_easymotor_v2); break; case idle_ms5607: AltosMs5607.provide_data(listener, link); @@ -116,6 +121,9 @@ class AltosIdler { case idle_sensor_easytimer1: AltosSensorEasyTimer1.provide_data(listener, link); break; + case idle_sensor_easymotor2: + AltosSensorEasyMotor2.provide_data(listener, link); + break; } } } @@ -219,6 +227,9 @@ public class AltosIdleFetch implements AltosDataProvider { new AltosIdler("EasyTimer-v1", AltosIdler.idle_imu_et_v1, AltosIdler.idle_sensor_easytimer1), + new AltosIdler("EasyMotor-v2", + AltosIdler.idle_adxl375_easymotor_v2, + AltosIdler.idle_sensor_easymotor2), }; AltosLink link; diff --git a/altoslib/AltosSensorEasyMotor2.java b/altoslib/AltosSensorEasyMotor2.java new file mode 100644 index 00000000..b3720a3b --- /dev/null +++ b/altoslib/AltosSensorEasyMotor2.java @@ -0,0 +1,63 @@ +/* + * 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. + * + * 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 org.altusmetrum.altoslib_14; + +import java.util.concurrent.TimeoutException; + +class AltosSensorEasyMotor2 { + int tick; + int v_batt; + int motor_pressure; + + public AltosSensorEasyMotor2(AltosLink link) throws InterruptedException, TimeoutException { + String[] items = link.adc(); + for (int i = 0; i < items.length;) { + if (items[i].equals("tick:")) { + tick = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + if (items[i].equals("motor_pressure:")) { + motor_pressure = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + if (items[i].equals("batt:")) { + v_batt = Integer.parseInt(items[i+1]); + i += 2; + continue; + } + i++; + } + } + + static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException { + try { + AltosSensorEasyMotor2 sensor_easymotor2 = new AltosSensorEasyMotor2(link); + + listener.set_battery_voltage(AltosConvert.easy_mini_2_voltage(sensor_easymotor2.v_batt)); + double pa = AltosConvert.easy_motor_2_motor_pressure(sensor_easymotor2.motor_pressure, + listener.cal_data().ground_motor_pressure); + listener.set_motor_pressure(pa); + + } catch (TimeoutException te) { + } + } +} + diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 75af5252..9c8c6691 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -114,6 +114,7 @@ altoslib_JAVA = \ AltosSensorMetrum.java \ AltosSensorTGPS1.java \ AltosSensorTGPS2.java \ + AltosSensorEasyMotor2.java \ AltosState.java \ AltosStateName.java \ AltosStringInputStream.java \ diff --git a/altosuilib/AltosInfoTable.java b/altosuilib/AltosInfoTable.java index 226f112f..89d45303 100644 --- a/altosuilib/AltosInfoTable.java +++ b/altosuilib/AltosInfoTable.java @@ -275,16 +275,18 @@ public class AltosInfoTable extends JTable implements AltosFlightDisplay, Hierar info_add_row(3, "Accel along", "%8.1f m/s²", state.accel_along()); info_add_row(3, "Accel across", "%8.1f m/s²", state.accel_across()); info_add_row(3, "Accel through", "%8.1f m/s²", state.accel_through()); + } + if (state != null && state.gyro_roll() != AltosLib.MISSING) { info_add_row(3, "Gyro roll", "%8.1f °/s", state.gyro_roll()); info_add_row(3, "Gyro pitch", "%8.1f °/s", state.gyro_pitch()); info_add_row(3, "Gyro yaw", "%8.1f °/s", state.gyro_yaw()); - if (state.mag_along() != AltosLib.MISSING) { - /* Report mag in nanoteslas (1 G = 100000 nT (or γ)) */ - info_add_row(3, "Mag along", "%8.1f µT", state.mag_along() * 100.0); - info_add_row(3, "Mag across", "%8.1f µT", state.mag_across() * 100.0); - info_add_row(3, "Mag Through", "%8.1f µT", state.mag_through() * 100.0); - info_add_row(3, "Mag Bearing", "%8.1f°", Math.atan2(state.mag_across(), state.mag_through()) * 180/Math.PI); - } + } + if (state != null && state.mag_along() != AltosLib.MISSING) { + /* Report mag in nanoteslas (1 G = 100000 nT (or γ)) */ + info_add_row(3, "Mag along", "%8.1f µT", state.mag_along() * 100.0); + info_add_row(3, "Mag across", "%8.1f µT", state.mag_across() * 100.0); + info_add_row(3, "Mag Through", "%8.1f µT", state.mag_through() * 100.0); + info_add_row(3, "Mag Bearing", "%8.1f°", Math.atan2(state.mag_across(), state.mag_through()) * 180/Math.PI); } if (state != null && state.igniter_voltage != null) { -- 2.30.2