From f7c8f0b14cf19804106860a5689cf1f37df20669 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 23 Oct 2020 16:00:01 -0700 Subject: [PATCH] altos: Make accel cal take three axes when present This allows AltosUI to save/restore all of the accel calibration values instead of just the primary axis. Signed-off-by: Keith Packard --- src/kernel/ao_config.c | 50 +++++++++++++++++++++++++----------------- src/kernel/ao_config.h | 2 +- 2 files changed, 31 insertions(+), 21 deletions(-) diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index 24eb79d9..bfc0325e 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -190,7 +190,7 @@ _ao_config_get(void) if (minor < 14) ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; #endif -#if HAS_GYRO +#if HAS_IMU if (minor < 15) { ao_config.accel_zero_along = 0; ao_config.accel_zero_across = 0; @@ -395,7 +395,7 @@ ao_config_accel_calibrate_show(void) { printf("Accel cal +1g: %d -1g: %d\n", ao_config.accel_plus_g, ao_config.accel_minus_g); -#if HAS_GYRO +#if HAS_IMU printf ("IMU cal along %d across %d through %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, @@ -406,7 +406,7 @@ ao_config_accel_calibrate_show(void) #define ACCEL_CALIBRATE_SAMPLES 1024 #define ACCEL_CALIBRATE_SHIFT 10 -#if HAS_GYRO +#if HAS_IMU static int16_t accel_cal_along; static int16_t accel_cal_across; static int16_t accel_cal_through; @@ -418,7 +418,7 @@ ao_config_accel_calibrate_auto(char *orientation) uint16_t i; int32_t accel_total; uint8_t cal_data_ring; -#if HAS_GYRO +#if HAS_IMU int32_t accel_along_total = 0; int32_t accel_across_total = 0; int32_t accel_through_total = 0; @@ -436,7 +436,7 @@ ao_config_accel_calibrate_auto(char *orientation) ao_sleep(&ao_sample_data); while (i && cal_data_ring != ao_sample_data) { accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); -#if HAS_GYRO +#if HAS_IMU accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); @@ -445,7 +445,7 @@ ao_config_accel_calibrate_auto(char *orientation) i--; } } -#if HAS_GYRO +#if HAS_IMU accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; @@ -457,35 +457,31 @@ static void ao_config_accel_calibrate_set(void) { int16_t up, down; - uint16_t r; -#if HAS_GYRO + bool auto_cal; +#if HAS_IMU int16_t accel_along_up = 0, accel_along_down = 0; int16_t accel_across_up = 0, accel_across_down = 0; int16_t accel_through_up = 0, accel_through_down = 0; #endif - r = ao_cmd_decimal(); + up = ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; - if (r == 0) { + down = ao_cmd_decimal(); + auto_cal = (up == 0 && ao_cmd_status != ao_cmd_success); + if (auto_cal) { up = ao_config_accel_calibrate_auto("up"); -#if HAS_GYRO +#if HAS_IMU accel_along_up = accel_cal_along; accel_across_up = accel_cal_across; accel_through_up = accel_cal_through; #endif down = ao_config_accel_calibrate_auto("down"); -#if HAS_GYRO +#if HAS_IMU accel_along_down = accel_cal_along; accel_across_down = accel_cal_across; accel_through_down = accel_cal_through; #endif - } else { - up = r; - r = ao_cmd_decimal(); - if (ao_cmd_status != ao_cmd_success) - return; - down = r; } if (up >= down) { printf("Invalid accel: up (%d) down (%d)\n", @@ -495,11 +491,25 @@ ao_config_accel_calibrate_set(void) _ao_config_edit_start(); ao_config.accel_plus_g = up; ao_config.accel_minus_g = down; -#if HAS_GYRO - if (r == 0) { +#if HAS_IMU + if (auto_cal) { ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; + } else { + int16_t v; + + v = ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) { + ao_config.accel_zero_along = v; + v = ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) { + ao_config.accel_zero_across = v; + v = ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) + ao_config.accel_zero_through = v; + } + } } #endif _ao_config_edit_finish(); diff --git a/src/kernel/ao_config.h b/src/kernel/ao_config.h index 3cfd0d07..8a367733 100644 --- a/src/kernel/ao_config.h +++ b/src/kernel/ao_config.h @@ -92,7 +92,7 @@ struct ao_config { #if HAS_RADIO_AMP uint8_t radio_amp; /* minor version 14 */ #endif -#if HAS_GYRO +#if HAS_IMU int16_t accel_zero_along; /* minor version 15 */ int16_t accel_zero_across; /* minor version 15 */ int16_t accel_zero_through; /* minor version 15 */ -- 2.30.2