altosui: Accel calibration UI

Provides a GUI for re-calibrating accelerometers

Signed-off-by: Keith Packard <keithp@keithp.com>

6 files changed, 291 insertions, 1 deletions

diff --git a/altoslib/AltosAccelCal.java b/altoslib/AltosAccelCal.java
new file mode 100644
index 00000000..03d9fbf2
--- /dev/null
+++ b/altoslib/AltosAccelCal.java
@@ -0,0 +1,217 @@
+/*
+ * Copyright © 2017 Keith Packard <keithp@keithp.com>
+ *
+ * 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 3 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.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+package org.altusmetrum.altoslib_12;
+
+import java.io.*;
+import java.util.concurrent.*;
+
+public class AltosAccelCal implements Runnable {
+
+	AltosLink		link;
+	AltosAccelCalListener	listener;
+
+	boolean			remote;
+	boolean			close_on_exit;
+	double			frequency;
+	String			callsign;
+
+	Thread			accel_thread;
+
+	AltosConfigData		config_data;
+
+	public static final int	phase_antenna_up = 0;
+	public static final int	phase_antenna_down = 1;
+
+	void start_link() throws InterruptedException, TimeoutException {
+		if (remote) {
+			link.set_radio_frequency(frequency);
+			link.set_callsign(callsign);
+			link.start_remote();
+		} else
+			link.flush_input();
+	}
+
+	boolean stop_link() throws InterruptedException, TimeoutException {
+		if (remote)
+			link.stop_remote();
+		return link.reply_abort;
+	}
+
+	public void set_frequency(double in_frequency) {
+		frequency = in_frequency;
+		link.abort_reply();
+	}
+
+	public void set_callsign(String in_callsign) {
+		callsign = in_callsign;
+		link.abort_reply();
+	}
+
+	public void abort() throws InterruptedException {
+		while (accel_thread.isAlive()) {
+			accel_thread.interrupt();
+			link.abort_reply();
+			Thread.sleep(100);
+		}
+		accel_thread.join();
+	}
+
+	static private final String press_msg = "press a key...";
+
+	private Semaphore ui_signal_semaphore;
+	private boolean ui_signal_reply;
+
+	public void signal(boolean reply) {
+		System.out.printf("Signal cal semaphore %b\n", reply);
+		ui_signal_reply = reply;
+		ui_signal_semaphore.release();
+	}
+
+	private boolean wait_signal() throws InterruptedException {
+		System.out.printf("\twait for cal signal...\n");
+		ui_signal_semaphore.acquire();
+		System.out.printf("\tgot cal signal %b\n", ui_signal_reply);
+		return ui_signal_reply;
+	}
+
+	private boolean wait_press(int timeout) throws InterruptedException {
+		for (;;) {
+			String line = link.get_reply(timeout);
+			if (line == null) {
+				System.out.printf("get_reply timeout\n");
+				return false;
+			}
+			System.out.printf("got line %s\n", line);
+			if (line.contains(press_msg))
+				return true;
+			if (line.contains("Invalid"))
+				return false;
+			if (line.contains("Syntax"))
+				return false;
+			if (line.contains("Calibrating"))
+				listener.message(this, line);
+		}
+	}
+
+	static final int cal_timeout = 20 * 1000;
+
+	public void run() {
+		System.out.printf("start accel cal procedure\n");
+		try {
+			AltosConfigData new_config = null;
+
+			try {
+				start_link();
+				config_data = link.config_data();
+
+				/* set back to antenna up for calibration */
+				if (config_data.pad_orientation != 0)
+					link.printf("c o 0\n");
+
+				/* Start calibration */
+				try {
+					System.out.printf("*** start cal\n");
+					link.set_match(press_msg);
+					link.printf("c a 0\n");
+					System.out.printf("*** wait press\n");
+					if (!wait_press(cal_timeout))
+						throw new TimeoutException("timeout");
+					System.out.printf("*** set_phase antenna_up\n");
+					listener.set_phase(this, phase_antenna_up);
+					System.out.printf("*** wait_signal\n");
+					if (!wait_signal())
+						throw new InterruptedException("aborted");
+					link.set_match(press_msg);
+					System.out.printf("*** send newline\n");
+					link.printf("\n");
+					System.out.printf("*** wait press\n");
+					if (!wait_press(cal_timeout))
+						throw new TimeoutException("timeout");
+					System.out.printf("***set_phase antenna_down\n");
+					listener.set_phase(this, phase_antenna_down);
+					System.out.printf("*** wait_signal\n");
+					if (!wait_signal())
+						throw new InterruptedException("aborted");
+					System.out.printf("*** send newline and version command\n");
+					link.printf("\nv\n");
+				} catch (TimeoutException e) {
+					throw e;
+				} catch (InterruptedException e) {
+					throw e;
+				}
+				link.set_match(null);
+
+				boolean worked = true;
+				for (;;) {
+					String line = link.get_reply(cal_timeout);
+					if (line == null)
+						throw new TimeoutException();
+					System.out.printf("*** waiting for finish: %s\n", line);
+					if (line.contains("Invalid"))
+						worked = false;
+					if (line.contains("software-version"))
+						break;
+					if (line.contains("Calibrating"))
+						listener.message(this, line);
+				}
+				System.out.printf("*** worked: %b\n", worked);
+				if (worked)
+					new_config = new AltosConfigData(link);
+			} finally {
+				System.out.printf("Restore orientation %d +g %d -g %d\n",
+						  config_data.pad_orientation,
+						  config_data.accel_cal_plus,
+						  config_data.accel_cal_minus);
+				if (config_data.pad_orientation != AltosLib.MISSING && config_data.pad_orientation != 0)
+					link.printf("c o %d\n", config_data.pad_orientation);
+				if (config_data.accel_cal_plus != AltosLib.MISSING && config_data.accel_cal_minus != AltosLib.MISSING)
+					link.printf("c a %d %d\n",
+						    config_data.accel_cal_plus, config_data.accel_cal_minus);
+				stop_link();
+			}
+			if (new_config != null) {
+				System.out.printf("*** +1g %d -1g %d\n",
+						  new_config.accel_cal_plus,
+						  new_config.accel_cal_minus);
+				listener.cal_done(this, new_config.accel_cal_plus, new_config.accel_cal_minus);
+				if (!wait_signal())
+					throw new InterruptedException("aborted");
+			} else
+				listener.error(this, "Calibration failed");
+		} catch (TimeoutException te) {
+			System.out.printf("timeout");
+			listener.error(this, "timeout");
+		} catch (InterruptedException ie) {
+			System.out.printf("interrupted\n");
+			listener.error(this, "interrupted");
+		}
+	}
+
+	public void start() {
+		accel_thread = new Thread(this);
+		listener.set_thread(this, accel_thread);
+		accel_thread.start();
+	}
+
+	public AltosAccelCal(AltosLink link, AltosAccelCalListener listener) {
+		this.link = link;
+		this.listener = listener;
+		ui_signal_semaphore = new Semaphore(0);
+	}
+}
diff --git a/altoslib/AltosAccelCalListener.java b/altoslib/AltosAccelCalListener.java
new file mode 100644
index 00000000..17359245
--- /dev/null
+++ b/altoslib/AltosAccelCalListener.java
@@ -0,0 +1,34 @@
+/*
+ * Copyright © 2017 Keith Packard <keithp@keithp.com>
+ *
+ * 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 3 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.,
+ * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
+ */
+
+package org.altusmetrum.altoslib_12;
+
+import java.io.*;
+import java.util.concurrent.*;
+
+public interface AltosAccelCalListener {
+	public void set_thread(AltosAccelCal cal, Thread thread);
+
+	public void set_phase(AltosAccelCal cal, int phase);
+
+	public void cal_done(AltosAccelCal cal, int plus, int minus);
+
+	public void error(AltosAccelCal cal, String msg);
+
+	public void message(AltosAccelCal cal, String msg);
+}
diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 63c34310..d85f735b 100644
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -525,6 +525,12 @@ public class AltosConfigData {
 		if (pad_orientation != AltosLib.MISSING)
 			pad_orientation = source.pad_orientation();
 
+		if (accel_cal_plus != AltosLib.MISSING)
+			accel_cal_plus = source.accel_cal_plus();
+
+		if (accel_cal_minus != AltosLib.MISSING)
+			accel_cal_minus = source.accel_cal_minus();
+
 		/* HAS_LOG */
 		if (flight_log_max != AltosLib.MISSING)
 			flight_log_max = source.flight_log_max();
@@ -592,6 +598,7 @@ public class AltosConfigData {
 		dest.set_flight_log_max(flight_log_max);
 		dest.set_ignite_mode(ignite_mode);
 		dest.set_pad_orientation(pad_orientation);
+		dest.set_accel_cal(accel_cal_plus, accel_cal_minus);
 		dest.set_callsign(callsign);
 		if (npyro != AltosLib.MISSING)
 			dest.set_pyros(pyros);
@@ -669,9 +676,10 @@ public class AltosConfigData {
 			link.printf("c e %d\n", radio_enable);
 
 		/* HAS_ACCEL */
-		/* UI doesn't support accel cal */
 		if (pad_orientation != AltosLib.MISSING)
 			link.printf("c o %d\n", pad_orientation);
+		if (accel_cal_plus != AltosLib.MISSING && accel_cal_minus != AltosLib.MISSING)
+			link.printf("c a %d %d\n", accel_cal_plus, accel_cal_minus);
 
 		/* HAS_LOG */
 		if (flight_log_max != 0)
diff --git a/altoslib/AltosConfigValues.java b/altoslib/AltosConfigValues.java
index 170b1112..10478fd4 100644
--- a/altoslib/AltosConfigValues.java
+++ b/altoslib/AltosConfigValues.java
@@ -74,6 +74,16 @@ public interface AltosConfigValues {
 
 	public abstract int pad_orientation();
 
+	public abstract void set_accel_cal(int accel_cal_plus, int accel_cal_minus);
+
+	public abstract int accel_cal_plus();
+
+	public abstract int accel_cal_minus();
+
+	public abstract void set_dirty();
+
+	public abstract void set_clean();
+
 	public abstract void set_pyros(AltosPyro[] new_pyros);
 
 	public abstract AltosPyro[] pyros() throws AltosConfigDataException;
diff --git a/altoslib/AltosLink.java b/altoslib/AltosLink.java
index 5a802ef1..5413de9d 100644
--- a/altoslib/AltosLink.java
+++ b/altoslib/AltosLink.java
@@ -43,6 +43,8 @@ public abstract class AltosLink implements Runnable {
 	public LinkedBlockingQueue<AltosLine> reply_queue = new LinkedBlockingQueue<AltosLine>();
 	public LinkedBlockingQueue<byte[]> binary_queue = new LinkedBlockingQueue<byte[]>();
 
+	private String match_string = null;
+
 	public synchronized void add_monitor(LinkedBlockingQueue<AltosLine> q) {
 		set_monitor(true);
 		monitors.add(q);
@@ -112,6 +114,15 @@ public abstract class AltosLink implements Runnable {
 
 	private int	len_read = 0;
 
+	private boolean match_bytes(byte[] bytes, int byte_count, String match) {
+		if (byte_count < match.length())
+			return false;
+		String	line = new String(bytes, 0, byte_count, AltosLib.unicode_set);
+		if (line == null)
+			return false;
+		return line.indexOf(match) >= 0;
+	}
+
 	public void run () {
 		int c;
 		byte[] line_bytes = null;
@@ -159,6 +170,11 @@ public abstract class AltosLink implements Runnable {
 							line_count = 0;
 							len_read = 0;
 						}
+						if (match_string != null && match_bytes(line_bytes, line_count, match_string)) {
+							match_string = null;
+							add_bytes(line_bytes, line_count);
+							line_count = 0;
+						}
 					}
 				}
 			}
@@ -166,6 +182,9 @@ public abstract class AltosLink implements Runnable {
 		}
 	}
 
+	public void set_match(String match) {
+		match_string = match;
+	}
 
 	public String get_reply(int timeout) throws InterruptedException {
 		boolean	can_cancel = can_cancel_reply();
diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am
index 11b5d562..08af9496 100644
--- a/altoslib/Makefile.am
+++ b/altoslib/Makefile.am
@@ -26,6 +26,8 @@ record_files = \
 
 altoslib_JAVA = \
 	AltosLib.java \
+	AltosAccelCal.java \
+	AltosAccelCalListener.java \
 	AltosCalData.java \
 	AltosCompanion.java \
 	AltosConfigData.java \