altoslib: Fix accel value flipping for TM v3.0

[fw/altos] / altoslib / AltosConfigData.java

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 2d22e7b677dfba0b4ebd1e9efbb2bea9c9ac27ca..7b78f760cbf43646fd5923f2fbce0aa7a695a0f6 100644 (file)
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -199,14 +199,25 @@ public class AltosConfigData {
                case AltosLib.AO_LOG_FORMAT_FULL:
                        return 0x7fff - value;
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
-               case AltosLib.AO_LOG_FORMAT_TELEMETRUM:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
-               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return 4095 - value;
+               case AltosLib.AO_LOG_FORMAT_TELEMETRUM:
+                       /*
+                        * TeleMetrum v2 and later use the same log format, but
+                        * have different accelerometers. This is the only place
+                        * it matters in altoslib.
+                        */
+                       if (product.startsWith("TeleMetrum-v2"))
+                               return 4095 - value;
+                       /* fall through */
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_EASYMOTOR:
                        return -value;
                default:
+                       if (product.startsWith("EasyTimer-"))
+                               return -value;
                        return AltosLib.MISSING;
                }
        }
@@ -318,10 +329,17 @@ public class AltosConfigData {
        /* Return + accel calibration relative to a specific pad orientation */
        public int accel_cal_plus(int pad_orientation) {
                adjust_accel_cal();
+               if (!accel_cal_adjusted)
+                       return AltosLib.MISSING;
+
                switch (pad_orientation) {
                case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+               case AltosLib.AO_PAD_ORIENTATION_WORDS_UPRIGHT:
+               case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_UP:
                        return accel_cal_plus_cooked;
                case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+               case AltosLib.AO_PAD_ORIENTATION_WORDS_UPSIDEDOWN:
+               case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_DOWN:
                        return invert_accel_value(accel_cal_minus_cooked);
                default:
                        return AltosLib.MISSING;
@@ -331,10 +349,17 @@ public class AltosConfigData {
        /* Return - accel calibration relative to a specific pad orientation */
        public int accel_cal_minus(int pad_orientation) {
                adjust_accel_cal();
+               if (!accel_cal_adjusted)
+                       return AltosLib.MISSING;
+
                switch (pad_orientation) {
                case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+               case AltosLib.AO_PAD_ORIENTATION_WORDS_UPRIGHT:
+               case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_UP:
                        return accel_cal_minus_cooked;
                case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+               case AltosLib.AO_PAD_ORIENTATION_WORDS_UPSIDEDOWN:
+               case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_DOWN:
                        return invert_accel_value(accel_cal_plus_cooked);
                default:
                        return AltosLib.MISSING;
@@ -346,18 +371,22 @@ public class AltosConfigData {
         */
        private void adjust_accel_cal() {
                if (!accel_cal_adjusted &&
+                   product != null &&
                    pad_orientation != AltosLib.MISSING &&
                    accel_cal_plus != AltosLib.MISSING &&
-                   accel_cal_minus != AltosLib.MISSING &&
-                   log_format != AltosLib.AO_LOG_FORMAT_UNKNOWN)
+                   accel_cal_minus != AltosLib.MISSING)
                {
                        switch (pad_orientation) {
                        case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+                       case AltosLib.AO_PAD_ORIENTATION_WORDS_UPRIGHT:
+                       case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_UP:
                                accel_cal_plus_cooked = accel_cal_plus;
                                accel_cal_minus_cooked = accel_cal_minus;
                                accel_cal_adjusted = true;
                                break;
                        case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+                       case AltosLib.AO_PAD_ORIENTATION_WORDS_UPSIDEDOWN:
+                       case AltosLib.AO_PAD_ORIENTATION_BIG_PARTS_DOWN:
                                accel_cal_plus_cooked = invert_accel_value(accel_cal_minus);
                                accel_cal_minus_cooked = invert_accel_value(accel_cal_plus);
                                accel_cal_adjusted = true;
@@ -524,9 +553,6 @@ public class AltosConfigData {
                                }
                        }
                } catch (Exception e) {}
-
-               /* Fix accel cal as soon as all of the necessary values appear */
-               adjust_accel_cal();
        }
 
        public AltosConfigData() {
@@ -624,6 +650,8 @@ public class AltosConfigData {
                                return true;
                        if (product.startsWith("TeleMega-v4"))
                                return true;
+                       if (product.startsWith("EasyMotor-v2"))
+                               return true;
                }
                throw new AltosUnknownProduct(product);
        }
@@ -636,6 +664,9 @@ public class AltosConfigData {
                                return AltosAdxl375.X_AXIS;
                        if (product.startsWith("TeleMega-v4"))
                                return AltosAdxl375.X_AXIS;
+                       if (product.startsWith("EasyMotor-v2"))
+                               return AltosAdxl375.X_AXIS;
+
                }
                throw new AltosUnknownProduct(product);
        }
@@ -784,33 +815,28 @@ public class AltosConfigData {
                                                        has_setting,
                                                        radio_calibration);
                        /* When remote, reset the dongle frequency at the same time */
-                       if (remote) {
-                               link.flush_output();
+                       if (remote && frequency != link.frequency) {
                                link.stop_remote();
                                link.set_radio_frequency(frequency);
-                               link.flush_output();
                                link.start_remote();
                        }
                }
 
                if (telemetry_rate != AltosLib.MISSING) {
                        link.printf("c T %d\n", telemetry_rate);
-                       if (remote) {
-                               link.flush_output();
+                       if (remote && telemetry_rate != link.telemetry_rate) {
                                link.stop_remote();
                                link.set_telemetry_rate(telemetry_rate);
-                               link.flush_output();
                                link.start_remote();
                        }
                }
 
                if (callsign != null) {
                        link.printf("c c %s\n", callsign);
-                       if (remote) {
-                               link.flush_output();
+                       if (remote && !callsign.equals(link.callsign)) {
+                               System.out.printf("changing link callsign from %s to %s\n", link.callsign, callsign);
                                link.stop_remote();
                                link.set_callsign(callsign);
-                               link.flush_output();
                                link.start_remote();
                        }
                }
@@ -824,8 +850,22 @@ public class AltosConfigData {
                        link.printf("c o %d\n", pad_orientation);
                int plus = accel_cal_plus(pad_orientation);
                int minus = accel_cal_minus(pad_orientation);
-               if (plus != AltosLib.MISSING && minus != AltosLib.MISSING)
-                       link.printf("c a %d %d\n", plus, minus);
+               if (plus != AltosLib.MISSING && minus != AltosLib.MISSING) {
+                       if (plus < 0)
+                               plus = 65536 + plus;
+                       if (minus < 0)
+                               minus = 65536 + minus;
+                       if (accel_zero_along != AltosLib.MISSING &&
+                           accel_zero_across != AltosLib.MISSING &&
+                           accel_zero_through != AltosLib.MISSING)
+                               link.printf("c a %d %d %d %d %d\n",
+                                           plus, minus,
+                                           accel_zero_along,
+                                           accel_zero_across,
+                                           accel_zero_through);
+                       else
+                               link.printf("c a %d %d\n", plus, minus);
+               }
 
                /* HAS_LOG */
                if (flight_log_max != 0 && flight_log_max != AltosLib.MISSING)
@@ -870,7 +910,7 @@ public class AltosConfigData {
                /* UI doesn't support accel cal */
 
                link.printf("c w\n");
-               link.flush_output();
+               read_link(link, "Saved");
        }
 
        public AltosConfigData(AltosLink link) throws InterruptedException, TimeoutException {
@@ -886,5 +926,6 @@ public class AltosConfigData {
                        read_link(link, "done");
                        break;
                }
+               adjust_accel_cal();
        }
 }