dunkirk.sh
🥑 meltybrain avocado
feat: add watchdog, bat low, and pid
author
Kieran Klukas
date
(Jul 8, 2026, 1:01 AM -0400)
commit
16c69150
16c69150b7a066a50c3d0ce8f7bcb2f277a97100
parent
bb2f7ef2
bb2f7ef2c0176ee888183150253551c41e3886c7
+509
-28
+5
firmware/CMakeLists.txt
+5
firmware/CMakeLists.txt
···
23
23
src/estimation/meas_model.c
24
24
src/estimation/ekf.c
25
25
src/estimation/imu_convert.c
26
+
src/estimation/accel_cal.c
26
27
src/control/drift.c
28
+
src/control/pid.c
27
29
src/app/control_loop.c
30
+
src/safety/watchdog.c
31
+
src/safety/battery.c
32
+
src/safety/rc_health.c
28
33
)
29
34
30
35
# Generate PIO header from .pio file
+1
firmware/sim/CMakeLists.txt
+1
firmware/sim/CMakeLists.txt
+1
-1
firmware/sim/drive.c
+1
-1
firmware/sim/drive.c
···
98
98
plant_read_imu(plant, &z);
99
99
last_z = z;
100
100
est = app_sensor_tick_si(&ekf, &z, TICK_DT);
101
-
motors = app_control_tick(&cfg, &cmd, &est);
101
+
motors = app_control_tick(&cfg, &cmd, &est, TICK_DT);
102
102
plant_step(plant, motors.throttle_a, motors.throttle_b, TICK_DT);
103
103
accumulator -= TICK_DT;
104
104
+36
-15
firmware/src/app/control_loop.c
+36
-15
firmware/src/app/control_loop.c
···
1
-
#include "app/control_loop.h"
1
+
#include "control_loop.h"
2
2
3
3
#include "math/linalg.h"
4
4
#include "estimation/imu_convert.h"
5
+
#include "control/pid.h"
6
+
7
+
/* PID state is module-level runtime state, not config. This keeps the
8
+
* shared config struct clean (serializable, no mutable state) and
9
+
* prevents callers from reaching into PID internals. */
10
+
static pid_t rpm_pid;
11
+
12
+
/* dt guard: if timestep is outside this band, fall back to CONTROL_DT. */
13
+
#define DT_MIN 0.0f
14
+
#define DT_MAX 0.1f
5
15
6
16
app_estimate_t app_sensor_tick(ekf_t *ekf, const imu_sample_t samples[IMU_COUNT],
7
17
float dt) {
···
11
21
}
12
22
13
23
app_estimate_t app_sensor_tick_si(ekf_t *ekf, const mat_t *z, float dt) {
14
-
if (dt <= 0.0f || dt > 0.1f) dt = 0.001f; /* guard bad deltas */
24
+
if (dt <= DT_MIN || dt > DT_MAX) dt = CONTROL_DT;
15
25
16
26
ekf_predict(ekf, dt);
17
27
ekf_update(ekf, z);
···
23
33
return est;
24
34
}
25
35
26
-
app_motors_t app_control_tick(const app_config_t *cfg, const app_command_t *cmd,
27
-
const app_estimate_t *est) {
36
+
app_motors_t app_control_tick(app_config_t *cfg, const app_command_t *cmd,
37
+
const app_estimate_t *est, float dt) {
28
38
app_motors_t out;
29
39
30
40
if (!cmd->armed) {
41
+
pid_reset(&rpm_pid);
31
42
out.throttle_a = 0.0f;
32
43
out.throttle_b = 0.0f;
33
44
return out;
34
45
}
35
46
47
+
/* Compute base throttle: either PID-regulated or direct from stick. */
48
+
float base = cmd->base;
49
+
if (cfg->pid_enabled) {
50
+
float target_rpm = cmd->base * cfg->target_rpm_max;
51
+
float current_rpm = est->omega * RAD_S_TO_RPM;
52
+
base = pid_compute(&rpm_pid, target_rpm, current_rpm, dt);
53
+
}
54
+
36
55
if (!cfg->drift_enabled) {
37
-
/* Open-loop spin: both motors follow the base throttle. */
38
-
out.throttle_a = cmd->base;
39
-
out.throttle_b = cmd->base;
56
+
out.throttle_a = base;
57
+
out.throttle_b = base;
40
58
return out;
41
59
}
42
60
43
61
float authority, phi;
44
62
drift_from_stick(cmd->stick_x, cmd->stick_y, cfg->max_authority,
45
63
&authority, &phi);
46
-
/* Add the drive-phase calibration: motor thrust is tangential, a
47
-
* quarter turn off the radial heading, so net translation lags the
48
-
* modulation peak by ~90 deg. Fold that plus the stored heading trim
49
-
* into the modulation phase so a commanded world direction actually
50
-
* translates that way. drift_phase is refined per-robot on the bench. */
51
64
float heading = est->heading - cfg->heading_trim;
52
65
phi -= cfg->drift_phase;
53
-
drift_throttles_t d = drift_compute(heading, cmd->base, authority, phi);
66
+
drift_throttles_t d = drift_compute(heading, base, authority, phi);
54
67
out.throttle_a = d.a;
55
68
out.throttle_b = d.b;
56
69
return out;
57
70
}
58
71
72
+
void app_pid_reset(void) {
73
+
pid_reset(&rpm_pid);
74
+
}
75
+
59
76
void app_config_default(app_config_t *cfg) {
60
77
cfg->max_authority = 0.5f;
61
-
cfg->drift_enabled = false; /* bring-up safe default: open-loop */
78
+
cfg->drift_enabled = false;
62
79
cfg->heading_trim = 0.0f;
63
-
cfg->drift_phase = -0.225f; /* residual heading-lag calibration (bench-tune per robot) */
80
+
cfg->drift_phase = -0.225f;
81
+
cfg->pid_enabled = false;
82
+
cfg->target_rpm_max = 3000.0f;
83
+
/* Initialize module-level PID with conservative gains. */
84
+
pid_init(&rpm_pid, 0.002f, 0.001f, 0.0f, 0.0f, 1.0f);
64
85
}
+17
-5
firmware/src/app/control_loop.h
+17
-5
firmware/src/app/control_loop.h
···
15
15
#include "imu/imu_spi.h"
16
16
#include "estimation/ekf.h"
17
17
#include "control/drift.h"
18
-
#include "math/linalg.h"
18
+
19
+
/* rad/s → RPM conversion factor. */
20
+
#define RAD_S_TO_RPM 9.5493f
21
+
22
+
/* Control loop timestep in seconds. Must match the sleep in core 1. */
23
+
#define CONTROL_DT 0.0005f
19
24
20
25
/* Estimator output, published sensor-side to control-side. */
21
26
typedef struct {
···
38
43
float throttle_b;
39
44
} app_motors_t;
40
45
41
-
/* Tunables shared by both callers. */
46
+
/* Tunables shared by both callers. PID state is intentionally NOT here;
47
+
* it's runtime state managed internally by control_loop.c. */
42
48
typedef struct {
43
49
float max_authority; /* drift authority cap [0,1] */
44
50
bool drift_enabled; /* false = open-loop spin (motors follow base) */
···
49
55
float drift_phase; /* rad; residual drive-phase calibration after
50
56
geometric +90 deg compensation in drift_compute.
51
57
0 nominal; bench-tune per robot. */
58
+
bool pid_enabled; /* true = PID regulates RPM from stick; false = direct throttle */
59
+
float target_rpm_max; /* RPM at full stick deflection when PID enabled */
52
60
} app_config_t;
53
61
54
62
/* Sensor-side tick: run the EKF on one IMU sample set over dt seconds
···
62
70
app_estimate_t app_sensor_tick_si(ekf_t *ekf, const mat_t *z, float dt);
63
71
64
72
/* Control-side tick: turn RC command + latest estimate into motor
65
-
* throttles using the drift controller. */
66
-
app_motors_t app_control_tick(const app_config_t *cfg, const app_command_t *cmd,
67
-
const app_estimate_t *est);
73
+
* throttles using the drift controller. dt is the time since last call
74
+
* (for PID integration). */
75
+
app_motors_t app_control_tick(app_config_t *cfg, const app_command_t *cmd,
76
+
const app_estimate_t *est, float dt);
77
+
78
+
/* Reset PID state (e.g., on disarm or mode change). */
79
+
void app_pid_reset(void);
68
80
69
81
/* Sensible defaults. */
70
82
void app_config_default(app_config_t *cfg);
+49
firmware/src/control/pid.c
+49
firmware/src/control/pid.c
···
1
+
#include "pid.h"
2
+
3
+
void pid_init(pid_t *p, float kp, float ki, float kd,
4
+
float out_min, float out_max) {
5
+
p->kp = kp;
6
+
p->ki = ki;
7
+
p->kd = kd;
8
+
p->integral = 0.0f;
9
+
p->prev_error = 0.0f;
10
+
p->out_min = out_min;
11
+
p->out_max = out_max;
12
+
}
13
+
14
+
static float clamp(float v, float lo, float hi) {
15
+
if (v < lo) return lo;
16
+
if (v > hi) return hi;
17
+
return v;
18
+
}
19
+
20
+
float pid_compute(pid_t *p, float setpoint, float measurement, float dt) {
21
+
if (dt <= 0.0f) return 0.0f;
22
+
23
+
float error = setpoint - measurement;
24
+
25
+
/* Proportional */
26
+
float p_term = p->kp * error;
27
+
28
+
/* Integral with anti-windup: only integrate when output isn't saturated */
29
+
p->integral += error * dt;
30
+
float i_term = p->ki * p->integral;
31
+
32
+
/* Derivative on error */
33
+
float d_term = p->kd * (error - p->prev_error) / dt;
34
+
p->prev_error = error;
35
+
36
+
float output = p_term + i_term + d_term;
37
+
38
+
/* Anti-windup: clamp integrator when output saturates */
39
+
if (output > p->out_max || output < p->out_min) {
40
+
p->integral -= error * dt; /* undo this step's integration */
41
+
}
42
+
43
+
return clamp(output, p->out_min, p->out_max);
44
+
}
45
+
46
+
void pid_reset(pid_t *p) {
47
+
p->integral = 0.0f;
48
+
p->prev_error = 0.0f;
49
+
}
+29
firmware/src/control/pid.h
+29
firmware/src/control/pid.h
···
1
+
#pragma once
2
+
3
+
/*
4
+
* Simple PID controller for RPM regulation.
5
+
*
6
+
* Takes a target RPM (from stick) and current RPM (from EKF/eRPM),
7
+
* outputs a throttle value [0, 1]. Used by core 1 to maintain consistent
8
+
* spin speed regardless of battery sag or friction changes.
9
+
*
10
+
* Tuning: start with P only (Ki=Kd=0), increase until oscillation,
11
+
* then back off ~30%. Add I to eliminate steady-state error. D is
12
+
* rarely needed for RPM control.
13
+
*/
14
+
15
+
typedef struct {
16
+
float kp, ki, kd;
17
+
float integral;
18
+
float prev_error;
19
+
float out_min, out_max;
20
+
} pid_t;
21
+
22
+
void pid_init(pid_t *p, float kp, float ki, float kd,
23
+
float out_min, float out_max);
24
+
25
+
/* Compute one PID step. dt in seconds. Returns clamped output. */
26
+
float pid_compute(pid_t *p, float setpoint, float measurement, float dt);
27
+
28
+
/* Reset integrator and derivative state (e.g., on mode change or disarm). */
29
+
void pid_reset(pid_t *p);
+56
firmware/src/estimation/accel_cal.c
+56
firmware/src/estimation/accel_cal.c
···
1
+
#include "accel_cal.h"
2
+
3
+
static float cal_table[ACCEL_CAL_MAX_POINTS * 2]; /* pairs: [raw_g, factor] */
4
+
static int cal_count = 0;
5
+
6
+
void accel_cal_init(void) {
7
+
cal_count = 0;
8
+
}
9
+
10
+
bool accel_cal_add(float raw_g, float correction_factor) {
11
+
if (cal_count >= ACCEL_CAL_MAX_POINTS) return false;
12
+
13
+
/* Sorted insert by raw_g. */
14
+
int pos = cal_count;
15
+
while (pos > 0 && raw_g < cal_table[(pos - 1) * 2]) {
16
+
cal_table[pos * 2] = cal_table[(pos - 1) * 2];
17
+
cal_table[pos * 2 + 1] = cal_table[(pos - 1) * 2 + 1];
18
+
pos--;
19
+
}
20
+
cal_table[pos * 2] = raw_g;
21
+
cal_table[pos * 2 + 1] = correction_factor;
22
+
cal_count++;
23
+
return true;
24
+
}
25
+
26
+
float accel_cal_get(float raw_g) {
27
+
if (cal_count == 0) return 1.0f;
28
+
29
+
/* Below table range: use first point's factor. */
30
+
if (raw_g <= cal_table[0]) return cal_table[1];
31
+
32
+
/* Above table range: use last point's factor. */
33
+
if (raw_g >= cal_table[(cal_count - 1) * 2])
34
+
return cal_table[(cal_count - 1) * 2 + 1];
35
+
36
+
/* Find bracketing interval and lerp. */
37
+
for (int i = 1; i < cal_count; i++) {
38
+
float g0 = cal_table[(i - 1) * 2];
39
+
float g1 = cal_table[i * 2];
40
+
if (raw_g <= g1) {
41
+
float t = (raw_g - g0) / (g1 - g0);
42
+
float f0 = cal_table[(i - 1) * 2 + 1];
43
+
float f1 = cal_table[i * 2 + 1];
44
+
return f0 + t * (f1 - f0);
45
+
}
46
+
}
47
+
return 1.0f; /* shouldn't reach here */
48
+
}
49
+
50
+
int accel_cal_count(void) {
51
+
return cal_count;
52
+
}
53
+
54
+
void accel_cal_clear(void) {
55
+
cal_count = 0;
56
+
}
+39
firmware/src/estimation/accel_cal.h
+39
firmware/src/estimation/accel_cal.h
···
1
+
#pragma once
2
+
3
+
#include <stdbool.h>
4
+
5
+
/*
6
+
* Accelerometer nonlinearity correction table.
7
+
*
8
+
* The H3LIS331DL sensors are not perfectly linear across their ±400G
9
+
* range. At high G (near saturation), the reported value deviates from
10
+
* true acceleration. This module stores a piecewise-linear correction
11
+
* curve built during bench calibration (spin at known RPMs, compare
12
+
* measured centrifugal to expected ω²r).
13
+
*
14
+
* The correction is applied as a multiplicative factor:
15
+
* corrected = raw * get_accel_correction(raw_g)
16
+
*
17
+
* Table format: pairs of (raw_g, correction_factor), sorted by raw_g.
18
+
* Linear interpolation between points. Outside the table range, the
19
+
* nearest endpoint factor is used.
20
+
*/
21
+
22
+
#define ACCEL_CAL_MAX_POINTS 8 /* max calibration points */
23
+
24
+
/* Initialize with an empty table (no correction). */
25
+
void accel_cal_init(void);
26
+
27
+
/* Add a calibration point. Points need not be added in order; the table
28
+
* is kept sorted. Returns false if table is full. */
29
+
bool accel_cal_add(float raw_g, float correction_factor);
30
+
31
+
/* Get the correction factor for a given raw G reading. Returns 1.0 if
32
+
* no calibration data exists. */
33
+
float accel_cal_get(float raw_g);
34
+
35
+
/* Number of calibration points currently stored. */
36
+
int accel_cal_count(void);
37
+
38
+
/* Clear all calibration data. */
39
+
void accel_cal_clear(void);
+43
-7
firmware/src/main.c
+43
-7
firmware/src/main.c
···
13
13
#include "crsf/crsf.h"
14
14
#include "estimation/ekf.h"
15
15
#include "app/control_loop.h"
16
+
#include "safety/watchdog.h"
17
+
#include "safety/battery.h"
18
+
#include "safety/rc_health.h"
19
+
20
+
/* rad → degrees for status display. */
21
+
#define RAD_TO_DEG 57.2958f
22
+
23
+
/* CRSF link timeout in ms (receiver-level failsafe). */
24
+
#define CRSF_LINK_TIMEOUT_MS 500
25
+
26
+
/* Status print interval in ms. */
27
+
#define STATUS_PRINT_INTERVAL_MS 100
28
+
29
+
/* Core 1 control loop sleep in microseconds. Must match CONTROL_DT. */
30
+
#define CONTROL_LOOP_SLEEP_US 500
16
31
17
32
/* ---- Shared sensor data (core 0 → core 1) ---- */
18
33
···
20
35
static spin_lock_t *imu_lock;
21
36
static app_config_t app_cfg;
22
37
38
+
/* Unified safety check: all conditions that must be true before motors
39
+
* are allowed to run. Centralizes the arming policy so it can't drift
40
+
* across multiple call sites. */
41
+
static bool is_safe_to_arm(const crsf_state_t *rc) {
42
+
return crsf_link_alive(rc, CRSF_LINK_TIMEOUT_MS)
43
+
&& rc_health_ok()
44
+
&& battery_ok();
45
+
}
46
+
23
47
/* ---- Core 1: Control (owns DSHOT + CRSF) ---- */
24
48
25
49
static void core1_control_loop(void) {
···
30
54
if (!dshot_init(&esc2, pio0, PIN_DSHOT2)) printf("DSHOT2 init failed\n");
31
55
32
56
crsf_init();
57
+
rc_health_init();
33
58
crsf_state_t rc = {0};
34
59
35
60
printf("Core 1 ready. Waiting for RC link...\n");
36
61
37
62
while (1) {
38
63
crsf_poll(&rc);
64
+
rc_health_update(rc.channels);
39
65
40
66
imu_packet_t imu;
41
67
uint32_t irq = spin_lock_blocking(imu_lock);
···
44
70
spin_unlock(imu_lock, irq);
45
71
46
72
app_command_t cmd = {0};
47
-
cmd.armed = crsf_link_alive(&rc, 500);
73
+
cmd.armed = is_safe_to_arm(&rc);
48
74
if (cmd.armed) {
49
75
int16_t thr = (int16_t)rc.channels[2];
50
76
if (thr > CRSF_CH_MIN) {
···
59
85
}
60
86
61
87
app_estimate_t est = { imu.heading, imu.omega, imu.alpha };
62
-
app_motors_t m = app_control_tick(&app_cfg, &cmd, &est);
88
+
app_motors_t m = app_control_tick(&app_cfg, &cmd, &est, CONTROL_DT);
63
89
64
90
dshot_set_throttle(&esc1, m.throttle_a);
65
91
dshot_set_throttle(&esc2, m.throttle_b);
···
69
95
70
96
static uint32_t last_print = 0;
71
97
uint32_t now = to_ms_since_boot(get_absolute_time());
72
-
if (now - last_print > 100) {
98
+
if (now - last_print > STATUS_PRINT_INTERVAL_MS) {
73
99
last_print = now;
74
-
printf("hdg=%6.1f deg w=%7.1f rad/s RPM=%lu/%lu a/b=%.2f/%.2f Link:%s\n",
75
-
(double)(imu.heading * 57.2958f), (double)imu.omega,
100
+
printf("hdg=%6.1f° w=%7.1f RPM=%lu/%lu a/b=%.2f/%.2f %s%s%s\n",
101
+
(double)(imu.heading * RAD_TO_DEG), (double)imu.omega,
76
102
(unsigned long)esc1.telemetry.rpm,
77
103
(unsigned long)esc2.telemetry.rpm,
78
104
(double)m.throttle_a, (double)m.throttle_b,
79
-
cmd.armed ? "OK" : "LOST");
105
+
cmd.armed ? "LINK" : "NO-LINK",
106
+
battery_low() ? " LOW-BAT" : "",
107
+
!rc_health_ok() ? " STALE-RC" : "");
80
108
}
81
109
82
-
sleep_us(500);
110
+
watchdog_feed_core1();
111
+
sleep_us(CONTROL_LOOP_SLEEP_US);
83
112
}
84
113
}
85
114
···
94
123
int lock_num = spin_lock_claim_unused(true);
95
124
imu_lock = spin_lock_instance(lock_num);
96
125
126
+
battery_init();
127
+
97
128
if (!imu_init()) {
98
129
printf("IMU init failed\n");
99
130
while (1) tight_loop_contents();
···
101
132
102
133
ekf_t ekf;
103
134
ekf_init(&ekf, 0.0f);
135
+
136
+
/* 2-second watchdog: either core hanging triggers a reset. */
137
+
watchdog_start(2000);
104
138
105
139
multicore_launch_core1(core1_control_loop);
106
140
···
126
160
spin_unlock(imu_lock, irq);
127
161
}
128
162
163
+
battery_sample();
164
+
watchdog_feed();
129
165
busy_wait_until(t + 1000); /* 1kHz sensor cadence */
130
166
}
131
167
}
+52
firmware/src/safety/battery.c
+52
firmware/src/safety/battery.c
···
1
+
#include "battery.h"
2
+
3
+
#include "hw_pins.h"
4
+
#include "hardware/adc.h"
5
+
6
+
/* VBAT divider: R9=120k high, R10=33.2k low.
7
+
* Pack voltage = adc_v * (120 + 33.2) / 33.2 = adc_v * 4.614 */
8
+
#define DIVIDER_RATIO 4.614f
9
+
10
+
/* ADC reference voltage on RP2350. */
11
+
#define ADC_VREF 3.3f
12
+
13
+
/* EMA filter coefficient. Lower = smoother but slower to respond.
14
+
* At 1kHz sampling, alpha=0.01 gives ~100ms time constant. */
15
+
#define FILTER_ALPHA 0.01f
16
+
17
+
static float filtered_pack_v = 0.0f;
18
+
static bool initialized = false;
19
+
20
+
void battery_init(void) {
21
+
adc_init();
22
+
adc_gpio_init(PIN_VBAT_ADC);
23
+
adc_select_input(ADC_VBAT_CHAN);
24
+
/* Prime the filter with an initial reading. */
25
+
uint16_t raw = adc_read();
26
+
filtered_pack_v = (float)raw / 4095.0f * ADC_VREF * DIVIDER_RATIO;
27
+
initialized = true;
28
+
}
29
+
30
+
void battery_sample(void) {
31
+
if (!initialized) return;
32
+
adc_select_input(ADC_VBAT_CHAN);
33
+
uint16_t raw = adc_read();
34
+
float pack_v = (float)raw / 4095.0f * ADC_VREF * DIVIDER_RATIO;
35
+
filtered_pack_v += FILTER_ALPHA * (pack_v - filtered_pack_v);
36
+
}
37
+
38
+
bool battery_ok(void) {
39
+
return battery_cell_voltage() >= BATTERY_HALT_V;
40
+
}
41
+
42
+
bool battery_low(void) {
43
+
return battery_cell_voltage() < BATTERY_WARN_V;
44
+
}
45
+
46
+
float battery_voltage(void) {
47
+
return filtered_pack_v;
48
+
}
49
+
50
+
float battery_cell_voltage(void) {
51
+
return filtered_pack_v / (float)BATTERY_CELLS;
52
+
}
+42
firmware/src/safety/battery.h
+42
firmware/src/safety/battery.h
···
1
+
#pragma once
2
+
3
+
#include <stdbool.h>
4
+
#include <stdint.h>
5
+
6
+
/*
7
+
* Battery voltage monitor with low-voltage alert and safety halt.
8
+
*
9
+
* Reads the VBAT ADC divider on core 0's sensor tick, filters with an
10
+
* exponential moving average, and exposes the current cell voltage.
11
+
* Core 1 checks battery_ok() before commanding motors; if voltage is
12
+
* critically low, motors are cut regardless of stick input.
13
+
*
14
+
* Thresholds are per-cell for a 2S pack (configurable). The alert level
15
+
* triggers a warning flag the LED/status system can display; the halt
16
+
* level cuts motors immediately.
17
+
*/
18
+
19
+
/* Per-cell voltage thresholds (volts). */
20
+
#define BATTERY_WARN_V 3.75f
21
+
#define BATTERY_HALT_V 3.60f
22
+
#define BATTERY_CELLS 2 /* 2S pack */
23
+
24
+
/* Initialize ADC and start background sampling. Call from core 0 init. */
25
+
void battery_init(void);
26
+
27
+
/* Sample the ADC and update the filtered voltage. Call once per sensor
28
+
* tick (~1kHz) from core 0. */
29
+
void battery_sample(void);
30
+
31
+
/* Returns true if battery voltage is above the halt threshold.
32
+
* Core 1 should check this before commanding motors. */
33
+
bool battery_ok(void);
34
+
35
+
/* Returns true if battery voltage is below the warning threshold. */
36
+
bool battery_low(void);
37
+
38
+
/* Current filtered pack voltage in volts. */
39
+
float battery_voltage(void);
40
+
41
+
/* Current filtered per-cell voltage in volts. */
42
+
float battery_cell_voltage(void);
+39
firmware/src/safety/rc_health.c
+39
firmware/src/safety/rc_health.c
···
1
+
#include "rc_health.h"
2
+
3
+
#include "pico/stdlib.h"
4
+
5
+
static uint32_t last_checksum = 0;
6
+
static uint32_t last_change_ms = 0;
7
+
static bool initialized = false;
8
+
9
+
/* Weighted checksum of channel values. Different weights per channel
10
+
* so that a change in any single channel is detectable even if others
11
+
* are static. From PotatoMelt's approach. */
12
+
static uint32_t compute_checksum(const uint16_t channels[16]) {
13
+
return (uint32_t)channels[0] * 64
14
+
+ (uint32_t)channels[1] * 16
15
+
+ (uint32_t)channels[2] * 4
16
+
+ (uint32_t)channels[3];
17
+
}
18
+
19
+
void rc_health_init(void) {
20
+
last_checksum = 0;
21
+
last_change_ms = to_ms_since_boot(get_absolute_time());
22
+
initialized = true;
23
+
}
24
+
25
+
void rc_health_update(const uint16_t channels[16]) {
26
+
if (!initialized) return;
27
+
28
+
uint32_t cs = compute_checksum(channels);
29
+
if (cs != last_checksum) {
30
+
last_checksum = cs;
31
+
last_change_ms = to_ms_since_boot(get_absolute_time());
32
+
}
33
+
}
34
+
35
+
bool rc_health_ok(void) {
36
+
if (!initialized) return false;
37
+
uint32_t now = to_ms_since_boot(get_absolute_time());
38
+
return (now - last_change_ms) < RC_STALE_TIMEOUT_MS;
39
+
}
+28
firmware/src/safety/rc_health.h
+28
firmware/src/safety/rc_health.h
···
1
+
#pragma once
2
+
3
+
#include <stdbool.h>
4
+
#include <stdint.h>
5
+
6
+
/*
7
+
* RC signal health monitor. Belt-and-suspenders safety layer on top of
8
+
* the receiver's own failsafe. Computes a checksum of all channel values;
9
+
* if the checksum hasn't changed for longer than the timeout, the link
10
+
* is considered stale even if the receiver reports "alive."
11
+
*
12
+
* Call rc_health_update() every time new CRSF data arrives. Check
13
+
* rc_health_ok() before trusting stick inputs.
14
+
*/
15
+
16
+
/* Timeout in ms. If no channel change is detected for this long, the
17
+
* link is considered stale. PotatoMelt uses 3000ms. */
18
+
#define RC_STALE_TIMEOUT_MS 3000
19
+
20
+
/* Initialize the health monitor. */
21
+
void rc_health_init(void);
22
+
23
+
/* Call with the current CRSF channel array whenever a frame arrives. */
24
+
void rc_health_update(const uint16_t channels[16]);
25
+
26
+
/* Returns true if the RC link appears healthy (channels changing or
27
+
* recently changed within the timeout). */
28
+
bool rc_health_ok(void);
+45
firmware/src/safety/watchdog.c
+45
firmware/src/safety/watchdog.c
···
1
+
#include "watchdog.h"
2
+
3
+
#include <stdint.h>
4
+
#include "hardware/watchdog.h"
5
+
#include "hardware/sync.h"
6
+
7
+
/* Dual-core watchdog coordination via spinlock. Both cores must signal
8
+
* alive within the timeout period or the chip resets. A spinlock
9
+
* serializes the flag exchange so there's no TOCTOU race between cores.
10
+
* Without this, ARM store buffering could cause both cores to miss each
11
+
* other's flag and trigger a spurious reset. */
12
+
13
+
static volatile bool core0_alive = false;
14
+
static volatile bool core1_alive = false;
15
+
static spin_lock_t *wdt_lock = NULL;
16
+
17
+
void watchdog_start(uint32_t timeout_ms) {
18
+
int lock_num = spin_lock_claim_unused(true);
19
+
wdt_lock = spin_lock_instance(lock_num);
20
+
watchdog_enable(timeout_ms, true); /* pause-on-debug */
21
+
}
22
+
23
+
void watchdog_feed(void) {
24
+
if (!wdt_lock) return;
25
+
uint32_t irq = spin_lock_blocking(wdt_lock);
26
+
core0_alive = true;
27
+
if (core1_alive) {
28
+
watchdog_update();
29
+
core0_alive = false;
30
+
core1_alive = false;
31
+
}
32
+
spin_unlock(wdt_lock, irq);
33
+
}
34
+
35
+
void watchdog_feed_core1(void) {
36
+
if (!wdt_lock) return;
37
+
uint32_t irq = spin_lock_blocking(wdt_lock);
38
+
core1_alive = true;
39
+
if (core0_alive) {
40
+
watchdog_update();
41
+
core0_alive = false;
42
+
core1_alive = false;
43
+
}
44
+
spin_unlock(wdt_lock, irq);
45
+
}
+25
firmware/src/safety/watchdog.h
+25
firmware/src/safety/watchdog.h
···
1
+
#pragma once
2
+
3
+
#include <stdint.h>
4
+
5
+
/*
6
+
* Hardware watchdog for the RP2350. If the main loop stops feeding the
7
+
* watchdog within the timeout period, the chip resets. This prevents a
8
+
* firmware hang from leaving motors spinning uncontrolled on a meltybrain.
9
+
*
10
+
* The watchdog must be updated from BOTH cores if both are running,
11
+
* since either core hanging is a failure condition. Core 0 updates via
12
+
* watchdog_feed(), core 1 via watchdog_feed_core1().
13
+
*/
14
+
15
+
/* Initialize and start the hardware watchdog with the given timeout in ms.
16
+
* Call once from core 1 (main) before launching core 0. */
17
+
void watchdog_start(uint32_t timeout_ms);
18
+
19
+
/* Feed the watchdog from core 0. Must be called at least once per
20
+
* timeout period. Place in the sensor loop. */
21
+
void watchdog_feed(void);
22
+
23
+
/* Feed the watchdog from core 1. Must be called at least once per
24
+
* timeout period. Place in the control loop. */
25
+
void watchdog_feed_core1(void);