dunkirk.sh
🥑 meltybrain avocado
5.2 kB
171 lines
1#include <stdio.h>
2#include <string.h>
3
4#include "pico/stdlib.h"
5#include "pico/multicore.h"
6#include "hardware/sync.h"
7#include "hardware/pio.h"
8
9#include "hw_pins.h"
10#include "estimation/heading_estimate.h"
11#include "dshot/dshot.h"
12#include "crsf/crsf.h"
13#include "sensor/sensor_dispatch.h"
14#include "app/control_loop.h"
15#include "safety/watchdog.h"
16#include "safety/battery.h"
17#include "safety/rc_health.h"
18#include "led/head_led.h"
19
20#define RAD_TO_DEG 57.2958f
21#define CRSF_LINK_TIMEOUT_MS 500
22#define STATUS_PRINT_INTERVAL_MS 100
23#define CONTROL_LOOP_SLEEP_US 500
24
25/* ---- Shared estimate (core 0 → core 1) ---- */
26
27typedef struct {
28 heading_estimate_t est;
29 uint32_t timestamp_us;
30 volatile bool ready;
31} shared_estimate_t;
32
33static shared_estimate_t shared_est;
34static spin_lock_t *est_lock;
35static app_config_t app_cfg;
36
37static bool is_safe_to_arm(const crsf_state_t *rc) {
38 return crsf_link_alive(rc, CRSF_LINK_TIMEOUT_MS)
39 && rc_health_ok()
40 && battery_ok();
41}
42
43/* ---- Core 1: Control (DSHOT + CRSF) ---- */
44
45static void core1_control_loop(void) {
46 if (!dshot_load_program(pio0)) printf("DSHOT PIO program load failed\n");
47
48 dshot_esc_t esc1, esc2;
49 if (!dshot_init(&esc1, pio0, PIN_DSHOT1)) printf("DSHOT1 init failed\n");
50 if (!dshot_init(&esc2, pio0, PIN_DSHOT2)) printf("DSHOT2 init failed\n");
51
52 crsf_init();
53 rc_health_init();
54 head_led_init();
55 crsf_state_t rc = {0};
56
57 printf("Core 1 ready [%s]. Waiting for RC link...\n", sensor_primary.name);
58
59 uint32_t c1_last_us = time_us_32();
60
61 while (1) {
62 crsf_poll(&rc);
63 rc_health_update(rc.channels);
64
65 /* Read latest estimate from core 0. */
66 heading_estimate_t est = {0};
67 uint32_t irq = spin_lock_blocking(est_lock);
68 if (shared_est.ready) {
69 est = shared_est.est;
70 shared_est.ready = false;
71 head_led_on_estimate(&est, time_us_32());
72 }
73 spin_unlock(est_lock, irq);
74
75 app_command_t cmd = {0};
76 cmd.armed = is_safe_to_arm(&rc);
77 if (cmd.armed) {
78 int16_t thr = (int16_t)rc.channels[2];
79 if (thr > CRSF_CH_MIN) {
80 cmd.base = (float)(thr - CRSF_CH_MIN) /
81 (float)(CRSF_CH_MAX - CRSF_CH_MIN);
82 if (cmd.base > 1.0f) cmd.base = 1.0f;
83 }
84 cmd.stick_x = (float)((int16_t)rc.channels[0] - CRSF_CH_MID) /
85 (float)(CRSF_CH_MAX - CRSF_CH_MID);
86 cmd.stick_y = (float)((int16_t)rc.channels[1] - CRSF_CH_MID) /
87 (float)(CRSF_CH_MAX - CRSF_CH_MID);
88 }
89
90 uint32_t c1_now_us = time_us_32();
91 float c1_dt = (float)(c1_now_us - c1_last_us) * 1e-6f;
92 c1_last_us = c1_now_us;
93 app_motors_t m = app_control_tick(&app_cfg, &cmd, &est, c1_dt);
94
95 dshot_set_throttle(&esc1, m.throttle_a);
96 dshot_set_throttle(&esc2, m.throttle_b);
97
98 dshot_read_telemetry(&esc1);
99 dshot_read_telemetry(&esc2);
100
101 static uint32_t last_print = 0;
102 uint32_t now = to_ms_since_boot(get_absolute_time());
103 if (now - last_print > STATUS_PRINT_INTERVAL_MS) {
104 last_print = now;
105 printf("hdg=%6.1f° w=%7.1f RPM=%lu/%lu a/b=%.2f/%.2f %s%s%s\n",
106 (double)(est.heading * RAD_TO_DEG), (double)est.omega,
107 (unsigned long)esc1.telemetry.rpm,
108 (unsigned long)esc2.telemetry.rpm,
109 (double)m.throttle_a, (double)m.throttle_b,
110 cmd.armed ? "LINK" : "NO-LINK",
111 battery_low() ? " LOW-BAT" : "",
112 !rc_health_ok() ? " STALE-RC" : "");
113 }
114
115 head_led_tick(time_us_32());
116
117 watchdog_feed_core1();
118 sleep_us(CONTROL_LOOP_SLEEP_US);
119 }
120}
121
122/* ---- Core 0: Sensor acquisition (main) ---- */
123
124int main(void) {
125 stdio_init_all();
126 printf("Holy Guacamole firmware starting [%s]...\n", sensor_primary.name);
127
128 app_config_default(&app_cfg);
129
130 int lock_num = spin_lock_claim_unused(true);
131 est_lock = spin_lock_instance(lock_num);
132
133 battery_init();
134
135 /* Primary sensor must init first (owns EKF in fusion mode). */
136 if (!sensor_primary.init()) {
137 printf("%s init failed\n", sensor_primary.name);
138 while (1) tight_loop_contents();
139 }
140
141 /* Auxiliary sensor (e.g., RSSI fusion into IMU EKF). */
142 if (sensor_aux) {
143 if (!sensor_aux->init())
144 printf("%s aux init failed (continuing without)\n", sensor_aux->name);
145 else
146 printf("Aux sensor: %s\n", sensor_aux->name);
147 }
148
149 watchdog_start(2000);
150 multicore_launch_core1(core1_control_loop);
151
152 uint32_t last_us = time_us_32();
153
154 while (1) {
155 uint32_t t = time_us_32();
156 float dt = (float)(t - last_us) * 1e-6f;
157 last_us = t;
158
159 heading_estimate_t est = sensor_primary.tick(dt);
160
161 uint32_t irq = spin_lock_blocking(est_lock);
162 shared_est.est = est;
163 shared_est.timestamp_us = t;
164 shared_est.ready = true;
165 spin_unlock(est_lock, irq);
166
167 battery_sample();
168 watchdog_feed();
169 busy_wait_until(t + 1000); /* 1kHz sensor cadence */
170 }
171}