altos: Allow six-axis orientation

[fw/altos] / src / kernel / ao_data.c

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;
 }