altoslib: Avoid sending negative accel cal values to flight computer

[fw/altos] / altoslib / AltosConfigData.java

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 5d58566ecb3dc0b9836e1bd676a5857f04a1cde1..c0855728541737b58aa8435b3ffcc663dde4f4d9 100644 (file)
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -205,8 +205,11 @@ public class AltosConfigData {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return 4095 - value;
                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;
                }
        }
@@ -356,8 +359,7 @@ public class AltosConfigData {
                if (!accel_cal_adjusted &&
                    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:
@@ -636,6 +638,8 @@ public class AltosConfigData {
                                return true;
                        if (product.startsWith("TeleMega-v4"))
                                return true;
+                       if (product.startsWith("EasyMotor-v2"))
+                               return true;
                }
                throw new AltosUnknownProduct(product);
        }
@@ -648,6 +652,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);
        }
@@ -796,33 +803,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();
                        }
                }
@@ -836,8 +838,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)
@@ -882,7 +898,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 {