altoslib: Deal with TeleMetrum v2.0 MMA6555 being inverted

[fw/altos] / altoslib / AltosMma655x.java

diff --git a/altoslib/AltosMma655x.java b/altoslib/AltosMma655x.java
index da57c27feccd612cec6c2d463baf91f527e2c78d..b09ec74b250e4a46c2cf377768130f2f15282af2 100644 (file)
--- a/altoslib/AltosMma655x.java
+++ b/altoslib/AltosMma655x.java
@@ -21,7 +21,7 @@ import java.util.concurrent.*;
 
 public class AltosMma655x implements Cloneable {
 
-       int     accel;
+       private int     accel;
 
        public boolean parse_line(String line) throws NumberFormatException {
                if (line.startsWith("MMA655X value")) {
@@ -45,12 +45,18 @@ public class AltosMma655x implements Cloneable {
                return n;
        }
 
-       static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
+       static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException, AltosUnknownProduct {
                try {
                        AltosMma655x    mma655x = new AltosMma655x(link);
 
-                       if (mma655x != null)
-                               state.set_accel(mma655x.accel);
+                       if (mma655x != null) {
+                               int accel = mma655x.accel;
+                               if (config_data.mma655x_inverted())
+                                       accel = 4095 - accel;
+                               if (config_data.pad_orientation == 1)
+                                       accel = 4095 - accel;
+                               state.set_accel(accel);
+                       }
                } catch (TimeoutException te) {
                } catch (NumberFormatException ne) {
                }