X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_config.c;h=deea98cd09707827bd4e80b7c8f2790f05c9fbe7;hb=8b139566544bf22cadd6f35d6ff78f8934d775ee;hp=bd2e95ef6a56f1feeb260beb410f527482f092a8;hpb=f4ff561bfaa7c59493eb9d6b99f0347db381a167;p=fw%2Faltos

diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index bd2e95ef..deea98cd 100644
--- 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();