altos: Allow six-axis orientation

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

diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index bd2e95ef6a56f1feeb260beb410f527482f092a8..deea98cd09707827bd4e80b7c8f2790f05c9fbe7 100644 (file)
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -86,7 +86,7 @@ _ao_config_put(void)
        ao_config_setup();
        ao_config_erase();
        ao_config_write(0, &ao_config, sizeof (ao_config));
-#if HAS_FLIGHT
+#if HAS_FLIGHT && HAS_LOG
        ao_log_write_erase(0);
 #endif
        ao_config_flush();
@@ -625,24 +625,24 @@ ao_config_log_set(void)
 #if FLIGHT_LOG_APPEND
        printf("Flight log fixed size %d kB\n", ao_storage_log_max >> 10);
 #else
-       uint16_t        block = (uint16_t) (ao_storage_block >> 10);
-       uint16_t        log_max = (uint16_t) (ao_storage_log_max >> 10);
        uint32_t        r;
 
        r = ao_cmd_decimal();
        if (ao_cmd_status != ao_cmd_success)
                return;
-       if (ao_log_present())
-               printf("Storage must be empty before changing log size\n");
-       else if (block > 1024 && (r & (block - 1)))
-               printf("Flight log size must be multiple of %d kB\n", block);
-       else if (r > log_max)
-               printf("Flight log max %d kB\n", log_max);
-       else {
-               _ao_config_edit_start();
-               ao_config.flight_log_max = r << 10;
-               _ao_config_edit_finish();
+       r = r << 10;
+       if (ao_log_present()) {
+               if (r != ao_config.flight_log_max)
+                       printf("Storage must be empty before changing log size\n");
+               return;
        }
+       if (r > ao_storage_log_max) {
+               printf("Flight log max %d kB\n", ao_storage_log_max >> 10);
+               return;
+       }
+       _ao_config_edit_start();
+       ao_config.flight_log_max = r & ~(ao_storage_block - 1);
+       _ao_config_edit_finish();
 #endif
 }
 #endif /* HAS_LOG */
@@ -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();