From 758acb92cccbe4b64a35a1883b42713738c90630 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 20 Dec 2013 22:08:11 -0800 Subject: altos: Complain about sensor self-test errors only in idle mode When the accelerometer says to go into pad mode, don't look for other sensor self test errors. Only look for sensor self test errors to choose between idle and invalid mode. This will prevent minor sensor self test errors from letting the rocket fly safely. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/core/ao_flight.c') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 463ff4a2..5918b997 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -104,9 +104,6 @@ ao_flight(void) ao_config.accel_minus_g == 0 || ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP || -#if HAS_IMU - ao_sensor_errors || -#endif ao_ground_height < -1000 || ao_ground_height > 7000) { @@ -152,7 +149,11 @@ ao_flight(void) #endif } else { /* Set idle mode */ - ao_flight_state = ao_flight_idle; + ao_flight_state = ao_flight_idle; +#if HAS_IMU + if (ao_sensor_errors) + ao_flight_state = ao_flight_invalid; +#endif #if HAS_ACCEL && HAS_RADIO && PACKET_HAS_SLAVE /* Turn on packet system in idle mode on TeleMetrum */ -- cgit v1.2.3 From 755082d36231c1b247bc0e1f13919dd9b5c362a8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 20 Dec 2013 22:13:32 -0800 Subject: altos: mma655x also needs ao_sensor_errors TeleMetrum has an MMA655X but no IMU, so it needs an explicit addition for sensor errors. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 4 ++-- src/core/ao_flight.h | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) (limited to 'src/core/ao_flight.c') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 5918b997..aac6880d 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -46,7 +46,7 @@ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ __pdata uint16_t ao_boost_tick; /* time of launch detect */ __pdata uint16_t ao_motor_number; /* number of motors burned so far */ -#if HAS_IMU +#if HAS_SENSOR_ERRORS /* Any sensor can set this to mark the flight computer as 'broken' */ __xdata uint8_t ao_sensor_errors; #endif @@ -150,7 +150,7 @@ ao_flight(void) } else { /* Set idle mode */ ao_flight_state = ao_flight_idle; -#if HAS_IMU +#if HAS_SENSOR_ERRORS if (ao_sensor_errors) ao_flight_state = ao_flight_invalid; #endif diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index c7c02ccf..01d21c11 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -41,7 +41,11 @@ extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_boost_tick; extern __pdata uint16_t ao_motor_number; -#if HAS_IMU +#if HAS_IMU || HAS_MMA655X +#define HAS_SENSOR_ERRORS 1 +#endif + +#if HAS_SENSOR_ERRORS extern __xdata uint8_t ao_sensor_errors; #endif -- cgit v1.2.3 From afc16e805145c3e9ab4ba948f9ab1d9aa2b27afb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 28 Dec 2013 10:18:53 -0800 Subject: altos: Add 'O' command for TeleMega orient testing Only present when HAS_FLIGHT_DEBUG is enabled, this command lets the user check the orientation tracking code by showing the current orientation and when the calibration values are reset. Signed-off-by: Keith Packard --- src/core/ao_flight.c | 9 +++++++++ src/core/ao_sample.c | 19 +++++++++++++++++++ 2 files changed, 28 insertions(+) (limited to 'src/core/ao_flight.c') diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index aac6880d..08302140 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -443,9 +443,18 @@ ao_gyro_test(void) ao_flight_state = ao_flight_idle; } +uint8_t ao_orient_test; + +static void +ao_orient_test_select(void) +{ + ao_orient_test = !ao_orient_test; +} + __code struct ao_cmds ao_flight_cmds[] = { { ao_flight_dump, "F\0Dump flight status" }, { ao_gyro_test, "G\0Test gyro code" }, + { ao_orient_test_select,"O\0Test orientation code" }, { 0, NULL }, }; #endif diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index adf8399d..34658951 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -92,6 +92,10 @@ __pdata int32_t ao_sample_roll_sum; static struct ao_quaternion ao_rotation; #endif +#if HAS_FLIGHT_DEBUG +extern uint8_t ao_orient_test; +#endif + static void ao_sample_preflight_add(void) { @@ -159,7 +163,11 @@ ao_sample_preflight_set(void) * that as the current rotation vector */ ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) + printf("\n\treset\n"); #endif +#endif nsamples = 0; } @@ -210,6 +218,17 @@ ao_sample_rotate(void) rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); + +#if HAS_FLIGHT_DEBUG + if (ao_orient_test) { + printf ("rot %d %d %d orient %d \r", + (int) (x * 1000), + (int) (y * 1000), + (int) (z * 1000), + ao_sample_orient); + } +#endif + } #endif -- cgit v1.2.3 From cc06242e882cba462791962c199b7c89e79adc65 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 14 Jan 2014 23:29:59 -0800 Subject: altos: Use factory calibration for all acceleration computations The ground acceleration value will vary depending on the tilt angle of the airframe, which will result in incorrect acceleration computations during flight. This also avoids accidental boost detect when moving the airframe around in pad mode. Signed-off-by: Keith Packard --- altoslib/AltosState.java | 12 ++---------- src/core/ao_flight.c | 2 +- src/core/ao_kalman.c | 2 +- 3 files changed, 4 insertions(+), 12 deletions(-) (limited to 'src/core/ao_flight.c') diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 134aeb4e..08dafbb4 100644 --- a/altoslib/AltosState.java +++ b/altoslib/AltosState.java @@ -949,14 +949,8 @@ public class AltosState implements Cloneable { } void update_accel() { - double ground = ground_accel; - - if (ground == AltosLib.MISSING) - ground = ground_accel_avg; if (accel == AltosLib.MISSING) return; - if (ground == AltosLib.MISSING) - return; if (accel_plus_g == AltosLib.MISSING) return; if (accel_minus_g == AltosLib.MISSING) @@ -964,7 +958,7 @@ public class AltosState implements Cloneable { double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0; double counts_per_mss = counts_per_g / 9.80665; - acceleration.set_measured((ground - accel) / counts_per_mss, time); + acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time); } public void set_accel_g(double accel_plus_g, double accel_minus_g) { @@ -976,10 +970,8 @@ public class AltosState implements Cloneable { } public void set_ground_accel(double ground_accel) { - if (ground_accel != AltosLib.MISSING) { + if (ground_accel != AltosLib.MISSING) this.ground_accel = ground_accel; - update_accel(); - } } public void set_accel(double accel) { diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 08302140..702c3403 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -401,7 +401,7 @@ ao_flight_dump(void) #if HAS_ACCEL int16_t accel; - accel = ((ao_ground_accel - ao_sample_accel) * ao_accel_scale) >> 16; + accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; #endif printf ("sample:\n"); diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 7fd4f889..9aea1f14 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -166,7 +166,7 @@ ao_kalman_err_accel(void) { int32_t accel; - accel = (ao_ground_accel - ao_sample_accel) * ao_accel_scale; + accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; /* Can't use ao_accel here as it is the pre-prediction value still */ ao_error_a = (accel - ao_k_accel) >> 16; -- cgit v1.2.3