From 8b139566544bf22cadd6f35d6ff78f8934d775ee Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Thu, 4 Jun 2020 23:43:21 -0700
Subject: [PATCH] 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>
---
 src/kernel/ao_config.c | 40 +++++++++++++++++++++++++++++++++++++++-
 src/kernel/ao_data.c   | 14 +++++++++++++-
 src/kernel/ao_data.h   |  2 ++
 3 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 9fbc64bd..deea98cd 100644
--- 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 77cd98e4..f6a0e5eb 100644
--- 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 b43a1cd4..87f0063f 100644
--- 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)
 
-- 
2.30.2