From: Keith Packard Date: Sat, 21 Apr 2018 23:17:26 +0000 (-0700) Subject: altos: Use max of 64 previous orient values when checking pyro limits X-Git-Tag: 1.8.6~1^2~104 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=b478d3c3569d2f9df50b0030197468d14af67688 altos: Use max of 64 previous orient values when checking pyro limits Instead of checking just a single measurement to see if the orientation is outside of the desired limits, use the maximum of 64 previous values to that rapidly changing orientation won't accidentally enable a pyro channel if sampled at the 'wrong time'. Signed-off-by: Keith Packard --- diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index e5c30eec..5a556d59 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -81,6 +81,19 @@ int pyro_dbg; #define DBG(...) #endif +static angle_t +ao_sample_max_orient(void) +{ + uint8_t i; + angle_t max = ao_sample_orients[0]; + + for (i = 1; i < AO_NUM_ORIENT; i++) { + angle_t a = ao_sample_orients[i]; + if (a > max) + max = a; + } + return max; +} /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -90,6 +103,9 @@ static uint8_t ao_pyro_ready(struct ao_pyro *pyro) { enum ao_pyro_flag flag, flags; +#if HAS_GYRO + angle_t max_orient; +#endif flags = pyro->flags; while (flags != ao_pyro_none) { @@ -130,14 +146,16 @@ ao_pyro_ready(struct ao_pyro *pyro) #if HAS_GYRO case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) + max_orient = ao_sample_max_orient(); + if (max_orient <= pyro->orient_less) continue; - DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); + DBG("orient %d > %d\n", max_orient, pyro->orient_less); break; case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) + max_orient = ao_sample_max_orient(); + if (max_orient >= pyro->orient_greater) continue; - DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); + DBG("orient %d < %d\n", max_orient, pyro->orient_greater); break; #endif diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 61519478..f8012e34 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -50,6 +50,8 @@ __pdata gyro_t ao_sample_roll; __pdata gyro_t ao_sample_pitch; __pdata gyro_t ao_sample_yaw; __pdata angle_t ao_sample_orient; +__pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +__pdata uint8_t ao_sample_orient_pos; #endif __data uint8_t ao_sample_data; @@ -115,6 +117,53 @@ ao_sample_preflight_add(void) ++nsamples; } +#if HAS_GYRO +static void +ao_sample_set_all_orients(void) +{ + int i; + for (i = 0; i < AO_NUM_ORIENT; i++) + ao_sample_orients[i] = ao_sample_orient; + ao_sample_orient_pos = 0; +} + +static void +ao_sample_set_one_orient(void) +{ + ao_sample_orients[ao_sample_orient_pos] = ao_sample_orient; + ao_sample_orient_pos = (ao_sample_orient_pos + 1) % AO_NUM_ORIENT; +} + +static void +ao_sample_compute_orient(void) +{ + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + */ + + float rotz; + 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); +} +#endif /* HAS_GYRO */ + static void ao_sample_preflight_set(void) { @@ -138,7 +187,7 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_orient = 0; + ao_sample_set_all_orients(); struct ao_quaternion orient; @@ -168,6 +217,9 @@ ao_sample_preflight_set(void) if (ao_orient_test) printf("\n\treset\n"); #endif + + ao_sample_compute_orient(); + ao_sample_set_all_orients(); #endif nsamples = 0; } @@ -195,31 +247,6 @@ ao_sample_rotate(void) /* And normalize to make sure it remains a unit vector */ ao_quaternion_normalize(&ao_rotation, &ao_rotation); - /* Compute pitch angle from vertical by taking the pad - * orientation vector and rotating it by the current total - * rotation value. That will be a unit vector pointing along - * the airframe axis. The Z value will be the cosine of the - * change in the angle from vertical since boost. - * - * rot = ao_rotation * vertical * ao_rotation° - * rot = ao_rotation * (0,0,0,1) * ao_rotation° - * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) - * = a.z² - a.y² - a.x² + a.r² - * - * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° - * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z - * - * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) - * = -a.z² + a.y² + a.x² - a.r² - */ - - float rotz; - 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", @@ -229,7 +256,8 @@ ao_sample_rotate(void) ao_sample_orient); } #endif - + ao_sample_compute_orient(); + ao_sample_set_one_orient(); } #endif @@ -367,6 +395,7 @@ ao_sample_init(void) ao_sample_yaw = 0; ao_sample_roll = 0; ao_sample_orient = 0; + ao_sample_set_all_orients(); #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index fbef031d..5ae389be 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -146,7 +146,10 @@ extern __pdata accel_t ao_sample_accel_through; extern __pdata gyro_t ao_sample_roll; extern __pdata gyro_t ao_sample_pitch; extern __pdata gyro_t ao_sample_yaw; +#define AO_NUM_ORIENT 64 extern __pdata angle_t ao_sample_orient; +extern __pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +extern __pdata uint8_t ao_sample_orient_pos; #endif void ao_sample_init(void);