dunkirk.sh
馃 meltybrain avocado
1.8 kB
53 lines
1#include "head_led.h"
2#include "hw_pins.h"
3#include "hardware/gpio.h"
4#include <math.h>
5
6/* Tunables. */
7#define SPIN_OMEGA_MIN 10.0f /* rad/s; below this the robot is "idle" */
8#define STROBE_WINDOW 0.35f /* rad (~20 deg) total width of the dot */
9#define REF_HEADING 0.0f /* heading at which the dot appears */
10#define STALE_US 200000u /* no estimate for this long => idle */
11#define IDLE_BLINK_US 250000u /* idle heartbeat half-period (2 Hz) */
12
13static float s_heading;
14static float s_omega;
15static uint32_t s_update_us;
16static bool s_have;
17
18void head_led_init(void) {
19 gpio_init(PIN_HEAD_LED);
20 gpio_set_dir(PIN_HEAD_LED, GPIO_OUT);
21 gpio_put(PIN_HEAD_LED, 0);
22 s_have = false;
23}
24
25void head_led_on_estimate(const heading_estimate_t *est, uint32_t now_us) {
26 s_heading = est->heading;
27 s_omega = est->omega;
28 s_update_us = now_us;
29 s_have = true;
30}
31
32static float wrap_pi(float a) {
33 const float pi = (float)M_PI;
34 while (a >= pi) a -= 2.0f * pi;
35 while (a < -pi) a += 2.0f * pi;
36 return a;
37}
38
39void head_led_tick(uint32_t now_us) {
40 bool stale = !s_have || (now_us - s_update_us) > STALE_US;
41
42 if (!stale && fabsf(s_omega) > SPIN_OMEGA_MIN) {
43 /* Extrapolate heading to "now" so the dot stays put between the
44 * (slower) estimate updates, then light the LED within a small
45 * angular window around the reference heading. */
46 float dt = (float)(now_us - s_update_us) * 1e-6f;
47 float err = wrap_pi(s_heading + s_omega * dt - REF_HEADING);
48 gpio_put(PIN_HEAD_LED, fabsf(err) < (STROBE_WINDOW * 0.5f));
49 } else {
50 /* Idle heartbeat. */
51 gpio_put(PIN_HEAD_LED, ((now_us / IDLE_BLINK_US) & 1u) == 0u);
52 }
53}