altoslib: Fetch 'orient' value from Mega Sensor packet

The mega sensor packet had a spare byte to hold the current
orientation value.

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

2 files changed, 18 insertions, 0 deletions

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 6d55b833..4ddcc4ed 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -464,6 +464,16 @@ public class AltosState implements Cloneable {
 		return acceleration.max();
 	}
 
+	public AltosValue	orient;
+
+	public void set_orient(double new_orient) {
+		orient.set(new_orient, time);
+	}
+
+	public double orient() {
+		return orient.value();
+	}
+
 	public AltosValue	kalman_height, kalman_speed, kalman_acceleration;
 
 	public void set_kalman(double height, double speed, double acceleration) {
@@ -560,6 +570,7 @@ public class AltosState implements Cloneable {
 		pressure = new AltosPressure();
 		speed = new AltosSpeed();
 		acceleration = new AltosAccel();
+		orient = new AltosValue();
 
 		temperature = AltosLib.MISSING;
 		battery_voltage = AltosLib.MISSING;
@@ -621,6 +632,7 @@ public class AltosState implements Cloneable {
 		pressure.finish_update();
 		speed.finish_update();
 		acceleration.finish_update();
+		orient.finish_update();
 
 		kalman_height.finish_update();
 		kalman_speed.finish_update();
@@ -665,6 +677,7 @@ public class AltosState implements Cloneable {
 		pressure.copy(old.pressure);
 		speed.copy(old.speed);
 		acceleration.copy(old.acceleration);
+		orient.copy(old.orient);
 
 		battery_voltage = old.battery_voltage;
 		pyro_voltage = old.pyro_voltage;
diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index 8afabd30..3c4f1f8f 100644
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -34,9 +34,12 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 	int	mag_y;
 	int	mag_z;
 
+	int	orient;
+
 	public AltosTelemetryMegaSensor(int[] bytes) {
 		super(bytes);
 
+		orient	      = int8(5);
 		accel         = int16(6);
 		pres          = int32(8);
 		temp          = int16(12);
@@ -61,6 +64,8 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 		state.set_pressure(pres);
 		state.set_temperature(temp / 100.0);
 
+		state.set_orient(orient);
+
 		AltosIMU imu = new AltosIMU();
 		
 		imu.accel_x = AltosIMU.convert_accel(accel_x);