altoslib: Keep config data accel cal in antenna up form.

Instead of letting the accel cal data live in whatever form it was
fetched in, keep it in antenna up form and provide it in whatever
orientation is necessary.

This fixes bugs in changing pad configuration where the accel cal values
from the old orientation would get used in the new orientation and
wreck them.

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

diff --git a/altoslib/AltosAccelCal.java b/altoslib/AltosAccelCal.java
index 75592d9e1b87c393708f801c9c68c91f18677956..f98090efaec35edcfe452b00acf4ef65f16302a4 100644 (file)
--- a/altoslib/AltosAccelCal.java
+++ b/altoslib/AltosAccelCal.java
@@ -174,23 +174,23 @@ public class AltosAccelCal implements Runnable {
                                if (worked)
                                        new_config = new AltosConfigData(link);
                        } finally {
                                if (worked)
                                        new_config = new AltosConfigData(link);
                        } finally {

+                               int plus = config_data.accel_cal_plus(config_data.pad_orientation);
+                               int minus = config_data.accel_cal_minus(config_data.pad_orientation);

                                System.out.printf("Restore orientation %d +g %d -g %d\n",
                                                  config_data.pad_orientation,
                                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)
+                                                 plus, minus);
+                               if (config_data.pad_orientation != AltosLib.MISSING)

                                        link.printf("c o %d\n", config_data.pad_orientation);
                                        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);
+                               if (plus != AltosLib.MISSING && minus != AltosLib.MISSING)
+                                       link.printf("c a %d %d\n", plus, minus);

                                link.flush_output();
                                stop_link();
                        }
                        if (new_config != null) {
                                link.flush_output();
                                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);
+                               int plus = new_config.accel_cal_plus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP);
+                               int minus = new_config.accel_cal_minus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP);
+                               System.out.printf("*** +1g %d -1g %d\n", plus, minus);
+                               listener.cal_done(this, plus, minus);

                                if (!wait_signal())
                                        throw new InterruptedException("aborted");
                        } else
                                if (!wait_signal())
                                        throw new InterruptedException("aborted");
                        } else

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 03e2cbd72e17793aabf277fbc408188940b73fae..5bc6c4fd5b51299db71f92c868d22caa73d34988 100644 (file)
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -397,7 +397,7 @@ public class AltosCalData {
                set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
                set_pad_orientation(config_data.pad_orientation);
                set_product(config_data.product);
                set_flight_params(config_data.apogee_delay / ticks_per_sec, config_data.apogee_lockout / ticks_per_sec);
                set_pad_orientation(config_data.pad_orientation);
                set_product(config_data.product);

-               set_accel_plus_minus(config_data.accel_cal_plus, config_data.accel_cal_minus);
+               set_accel_plus_minus(config_data.accel_cal_plus(config_data.pad_orientation), config_data.accel_cal_minus(config_data.pad_orientation));

                set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
                set_ms5607(config_data.ms5607);
                try {
                set_accel_zero(config_data.accel_zero_along, config_data.accel_zero_across, config_data.accel_zero_through);
                set_ms5607(config_data.ms5607);
                try {

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index dc036867047cf278432aa99b7b35be0ccb244d6f..35a8602db4097651b149700ba6dfe863e36eead2 100644 (file)
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -52,7 +52,9 @@ public class AltosConfigData {
        public int      radio_setting;
 
        /* HAS_ACCEL */
        public int      radio_setting;
 
        /* HAS_ACCEL */

-       public int      accel_cal_plus, accel_cal_minus;
+       private int     accel_cal_plus_raw, accel_cal_minus_raw;
+       private int     accel_cal_plus_cooked, accel_cal_minus_cooked;
+       private boolean accel_cal_adjusted;

        public int      pad_orientation;
 
        /* HAS_LOG */
        public int      pad_orientation;
 
        /* HAS_LOG */


@@ -186,6 +188,23 @@ public class AltosConfigData {
                }
        }
 
                }
        }
 

+       public int invert_accel_value(int value) {
+               if (value == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+
+               switch (log_format) {
+               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:
+                       return 4095 - value;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+

        public boolean has_monitor_battery() {
                if (product.startsWith("TeleBT"))
                        return true;
        public boolean has_monitor_battery() {
                if (product.startsWith("TeleBT"))
                        return true;


@@ -252,9 +271,12 @@ public class AltosConfigData {
                radio_setting = AltosLib.MISSING;
                telemetry_rate = AltosLib.MISSING;
 
                radio_setting = AltosLib.MISSING;
                telemetry_rate = AltosLib.MISSING;
 

-               accel_cal_plus = AltosLib.MISSING;
-               accel_cal_minus = AltosLib.MISSING;
+               accel_cal_plus_cooked = AltosLib.MISSING;
+               accel_cal_minus_cooked = AltosLib.MISSING;
+               accel_cal_plus_raw = AltosLib.MISSING;
+               accel_cal_minus_raw = AltosLib.MISSING;

                pad_orientation = AltosLib.MISSING;
                pad_orientation = AltosLib.MISSING;

+               accel_cal_adjusted = false;

 
                flight_log_max = AltosLib.MISSING;
                log_fixed = AltosLib.MISSING;
 
                flight_log_max = AltosLib.MISSING;
                log_fixed = AltosLib.MISSING;


@@ -285,6 +307,57 @@ public class AltosConfigData {
                accel_zero_through = AltosLib.MISSING;
        }
 
                accel_zero_through = AltosLib.MISSING;
        }
 

+       /* Return + accel calibration relative to a specific pad orientation */
+       public int accel_cal_plus(int pad_orientation) {
+               switch (pad_orientation) {
+               case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+                       return accel_cal_plus_cooked;
+               case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+                       return invert_accel_value(accel_cal_minus_cooked);
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       /* Return - accel calibration relative to a specific pad orientation */
+       public int accel_cal_minus(int pad_orientation) {
+               switch (pad_orientation) {
+               case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+                       return accel_cal_minus_cooked;
+               case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+                       return invert_accel_value(accel_cal_plus_cooked);
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       /* Once we have all of the values from the config data, compute the
+        * accel cal values relative to Antenna Up orientation.
+        */
+       private void adjust_accel_cal() {
+               if (!accel_cal_adjusted &&
+                   pad_orientation != AltosLib.MISSING &&
+                   accel_cal_plus_raw != AltosLib.MISSING &&
+                   accel_cal_minus_raw != AltosLib.MISSING &&
+                   log_format != AltosLib.AO_LOG_FORMAT_UNKNOWN)
+               {
+                       switch (pad_orientation) {
+                       case AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP:
+                               accel_cal_plus_cooked = accel_cal_plus_raw;
+                               accel_cal_minus_cooked = accel_cal_minus_raw;
+                               accel_cal_adjusted = true;
+                               break;
+                       case AltosLib.AO_PAD_ORIENTATION_ANTENNA_DOWN:
+                               accel_cal_plus_cooked = invert_accel_value(accel_cal_minus_raw);
+                               accel_cal_minus_cooked = invert_accel_value(accel_cal_plus_raw);
+                               accel_cal_adjusted = true;
+                               break;
+                       default:
+                               break;
+                       }
+               }
+       }
+

        public void parse_line(String line) {
 
                /* Version replies */
        public void parse_line(String line) {
 
                /* Version replies */


@@ -349,8 +422,9 @@ public class AltosConfigData {
                        if (line.startsWith("Accel cal")) {
                                String[] bits = line.split("\\s+");
                                if (bits.length >= 6) {
                        if (line.startsWith("Accel cal")) {
                                String[] bits = line.split("\\s+");
                                if (bits.length >= 6) {

-                                       accel_cal_plus = Integer.parseInt(bits[3]);
-                                       accel_cal_minus = Integer.parseInt(bits[5]);
+                                       accel_cal_plus_raw = Integer.parseInt(bits[3]);
+                                       accel_cal_minus_raw = Integer.parseInt(bits[5]);
+                                       accel_cal_adjusted = false;

                                }
                        }
                } catch (Exception e) {}
                                }
                        }
                } catch (Exception e) {}


@@ -414,6 +488,9 @@ public class AltosConfigData {
                                }
                        }
                } catch (Exception e) {}
                                }
                        }
                } catch (Exception e) {}

+
+               /* Fix accel cal as soon as all of the necessary values appear */
+               adjust_accel_cal();

        }
 
        public AltosConfigData() {
        }
 
        public AltosConfigData() {


@@ -525,11 +602,11 @@ public class AltosConfigData {
                if (pad_orientation != AltosLib.MISSING)
                        pad_orientation = source.pad_orientation();
 
                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_plus_cooked != AltosLib.MISSING)
+                       accel_cal_plus_cooked = source.accel_cal_plus();

 
 

-               if (accel_cal_minus != AltosLib.MISSING)
-                       accel_cal_minus = source.accel_cal_minus();
+               if (accel_cal_minus_cooked != AltosLib.MISSING)
+                       accel_cal_minus_cooked = source.accel_cal_minus();

 
                /* HAS_LOG */
                if (flight_log_max != AltosLib.MISSING)
 
                /* HAS_LOG */
                if (flight_log_max != AltosLib.MISSING)


@@ -598,7 +675,8 @@ 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_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_accel_cal(accel_cal_plus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP),
+                                  accel_cal_minus(AltosLib.AO_PAD_ORIENTATION_ANTENNA_UP));

                dest.set_callsign(callsign);
                if (npyro != AltosLib.MISSING)
                        dest.set_pyros(pyros);
                dest.set_callsign(callsign);
                if (npyro != AltosLib.MISSING)
                        dest.set_pyros(pyros);


@@ -676,10 +754,13 @@ public class AltosConfigData {
                        link.printf("c e %d\n", radio_enable);
 
                /* HAS_ACCEL */
                        link.printf("c e %d\n", radio_enable);
 
                /* HAS_ACCEL */

+               /* set orientation first so that we know how to set the accel cal */

                if (pad_orientation != AltosLib.MISSING)
                        link.printf("c o %d\n", pad_orientation);
                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);
+               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);

 
                /* HAS_LOG */
                if (flight_log_max != 0)
 
                /* HAS_LOG */
                if (flight_log_max != 0)

diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java
index c25a6273136c095b5b945fe71946631a7d393b25..7d2b68e0bfe94d56788f06ca9a4e2d7f51168aef 100644 (file)
--- a/altoslib/AltosLib.java
+++ b/altoslib/AltosLib.java
@@ -348,6 +348,9 @@ public class AltosLib {
        public static final int AO_GPS_NUM_SAT_SHIFT = 0;
        public static final int AO_GPS_NUM_SAT_MASK = 0xf;
 
        public static final int AO_GPS_NUM_SAT_SHIFT = 0;
        public static final int AO_GPS_NUM_SAT_MASK = 0xf;
 

+       public static final int AO_PAD_ORIENTATION_ANTENNA_UP = 0;
+       public static final int AO_PAD_ORIENTATION_ANTENNA_DOWN = 1;
+

        public static final int AO_LOG_FORMAT_UNKNOWN = 0;
        public static final int AO_LOG_FORMAT_FULL = 1;
        public static final int AO_LOG_FORMAT_TINY = 2;
        public static final int AO_LOG_FORMAT_UNKNOWN = 0;
        public static final int AO_LOG_FORMAT_FULL = 1;
        public static final int AO_LOG_FORMAT_TINY = 2;