altos: Allow six-axis orientation

EasyTimer wants to be allowed to orient in any of six directions.

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

diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 9fbc64bdbd482e7da78d2dd60d3f71d92ae1141c..deea98cd09707827bd4e80b7c8f2790f05c9fbe7 100644 (file)
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -673,18 +673,56 @@ ao_config_pad_orientation_show(void)
        printf("Pad orientation: %d\n", ao_config.pad_orientation);
 }
 
+#if ALLOW_SIX_AXIS_PAD
+#define PAD_ORIENTATION_MAX    5
+#else
+#define PAD_ORIENTATION_MAX    1
+#endif
+
 static void
 ao_config_pad_orientation_set(void) 
 {
-       uint16_t r = ao_cmd_decimal() & 1;
+       uint16_t r = ao_cmd_decimal();
        if (ao_cmd_status != ao_cmd_success)
                return;
+       if (r > PAD_ORIENTATION_MAX) {
+               ao_cmd_status = ao_cmd_lex_error;
+               return;
+       }
        _ao_config_edit_start();
        if (ao_config.pad_orientation != r) {
+#if ALLOW_SIX_AXIS_PAD
+               accel_t zero;
+               switch (r >> 1) {
+               case 0:
+               default:
+                       zero = ao_config.accel_zero_along;
+                       break;
+               case 1:
+                       zero = ao_config.accel_zero_across;
+                       break;
+               case 2:
+                       zero = ao_config.accel_zero_through;
+                       break;
+               }
+               accel_t plus2 = ao_config.accel_minus_g - ao_config.accel_plus_g;
+               if (plus2 < 0)
+                       plus2 = -plus2;
+               accel_t plus1 = (plus2 >> 1);
+               accel_t minus1 = plus2 - plus1;
+               if (r & 1) {
+                       ao_config.accel_plus_g = ao_data_accel_invert(zero + minus1);
+                       ao_config.accel_minus_g = ao_data_accel_invert(zero - plus1);
+               } else {
+                       ao_config.accel_plus_g = zero - plus1;
+                       ao_config.accel_minus_g = zero + minus1;
+               }
+#else
                accel_t t;
                t = ao_config.accel_plus_g;
                ao_config.accel_plus_g = ao_data_accel_invert(ao_config.accel_minus_g);
                ao_config.accel_minus_g = ao_data_accel_invert(t);
+#endif
        }
        ao_config.pad_orientation = r;
        _ao_config_edit_finish();

diff --git a/src/kernel/ao_data.c b/src/kernel/ao_data.c
index 77cd98e40cdc577bbfd7d998cdfb9f21b1fb5887..f6a0e5ebec52131b587ff3d0127f68b79403cb26 100644 (file)
--- a/src/kernel/ao_data.c
+++ b/src/kernel/ao_data.c
@@ -36,8 +36,20 @@ ao_data_get(struct ao_data *packet)
 accel_t
 ao_data_accel(volatile struct ao_data *packet) {
        accel_t raw;
+#if ALLOW_SIX_AXIS_PAD
+       switch (ao_config.pad_orientation >> 1) {
+       default:
+       case 0:
+               raw = -ao_data_along(packet); break;
+       case 1:
+               raw = -ao_data_across(packet); break;
+       case 2:
+               raw = -ao_data_through(packet); break;
+       }
+#else
        raw = ao_data_accel_raw(packet);
-       if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
+#endif
+       if (ao_config.pad_orientation & 1)
                raw = ao_data_accel_invert(raw);
        return raw;
 }

diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index b43a1cd498dd636a65c0c59c55bcf7af83090bbb..87f0063fba01899df6ef93d01ecfc09ab793cd79 100644 (file)
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -442,7 +442,9 @@ static inline float ao_convert_accel(int16_t sensor)
 
 typedef int16_t accel_t;
 
+#if !ALLOW_SIX_AXIS_PAD
 #define ao_data_accel_raw(packet)              -ao_data_along(packet)
+#endif
 #define ao_data_accel_invert(a)                        (-(a))
 #define ao_data_accel_to_sample(accel)         ao_bmx_accel_to_sample(accel)