diff options
| author | Keith Packard <keithp@keithp.com> | 2018-04-21 16:17:26 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2018-04-26 19:24:21 -0700 |
| commit | b478d3c3569d2f9df50b0030197468d14af67688 (patch) | |
| tree | 723356b0d6d58312f2bc86d318215900e97e91b7 /src/kernel/ao_pyro.c | |
| parent | a414a32f86c9d8a2c5f576898c0f0dc75263ff85 (diff) | |
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 <keithp@keithp.com>
Diffstat (limited to 'src/kernel/ao_pyro.c')
| -rw-r--r-- | src/kernel/ao_pyro.c | 26 |
1 files changed, 22 insertions, 4 deletions
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 |