feat: Hardware button park/disarm/re-arm (Issue #682) #688
@ -14,9 +14,10 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */
|
BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */
|
||||||
BALANCE_ARMED, /* Active balancing */
|
BALANCE_ARMED = 1, /* Active balancing */
|
||||||
BALANCE_TILT_FAULT, /* Tilt exceeded limit, motors killed */
|
BALANCE_TILT_FAULT = 2, /* Tilt exceeded limit, motors killed */
|
||||||
|
BALANCE_PARKED = 3, /* PID frozen, motors off — quick re-arm via button (Issue #682) */
|
||||||
} balance_state_t;
|
} balance_state_t;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
@ -46,5 +47,7 @@ void balance_init(balance_t *b);
|
|||||||
void balance_update(balance_t *b, const IMUData *imu, float dt);
|
void balance_update(balance_t *b, const IMUData *imu, float dt);
|
||||||
void balance_arm(balance_t *b);
|
void balance_arm(balance_t *b);
|
||||||
void balance_disarm(balance_t *b);
|
void balance_disarm(balance_t *b);
|
||||||
|
void balance_park(balance_t *b); /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
|
||||||
|
void balance_unpark(balance_t *b); /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -296,4 +296,13 @@
|
|||||||
#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2
|
#define ENC_RIGHT_CH2_PIN GPIO_PIN_7 // TIM3_CH2, AF2
|
||||||
#define ENC_RIGHT_AF GPIO_AF2_TIM3
|
#define ENC_RIGHT_AF GPIO_AF2_TIM3
|
||||||
|
|
||||||
|
// --- Hardware Button (Issue #682) ---
|
||||||
|
// Active-low push button on PC2 (internal pull-up)
|
||||||
|
#define BTN_PORT GPIOC
|
||||||
|
#define BTN_PIN GPIO_PIN_2
|
||||||
|
#define BTN_DEBOUNCE_MS 20u // ms debounce window
|
||||||
|
#define BTN_LONG_MIN_MS 1500u // ms threshold: LONG press
|
||||||
|
#define BTN_COMMIT_MS 500u // ms quiet after lone SHORT -> PARK event
|
||||||
|
#define BTN_SEQ_TIMEOUT_MS 3000u // ms: sequence window; expired buffer abandoned
|
||||||
|
|
||||||
#endif // CONFIG_H
|
#endif // CONFIG_H
|
||||||
|
|||||||
61
include/hw_button.h
Normal file
61
include/hw_button.h
Normal file
@ -0,0 +1,61 @@
|
|||||||
|
#ifndef HW_BUTTON_H
|
||||||
|
#define HW_BUTTON_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* hw_button — hardware button debounce + gesture detection (Issue #682).
|
||||||
|
*
|
||||||
|
* Debounce FSM:
|
||||||
|
* IDLE → (raw press detected) → DEBOUNCING
|
||||||
|
* DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD
|
||||||
|
* HELD → (released) → classify press type, back to IDLE
|
||||||
|
*
|
||||||
|
* Press types:
|
||||||
|
* SHORT held < BTN_LONG_MIN_MS from confirmed start
|
||||||
|
* LONG held >= BTN_LONG_MIN_MS
|
||||||
|
*
|
||||||
|
* Sequence detection:
|
||||||
|
* [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release)
|
||||||
|
* [SHORT] + BTN_COMMIT_MS quiet timeout -> BTN_EVENT_PARK
|
||||||
|
*
|
||||||
|
* Config constants (can be overridden in config.h):
|
||||||
|
* BTN_DEBOUNCE_MS 20 ms debounce window
|
||||||
|
* BTN_LONG_MIN_MS 1500 ms threshold for LONG press
|
||||||
|
* BTN_COMMIT_MS 500 ms quiet after lone SHORT -> PARK
|
||||||
|
* BTN_SEQ_TIMEOUT_MS 3000 ms sequence window; expired sequence is abandoned
|
||||||
|
* BTN_PORT GPIOC
|
||||||
|
* BTN_PIN GPIO_PIN_2
|
||||||
|
*/
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
BTN_EVENT_NONE = 0,
|
||||||
|
BTN_EVENT_PARK = 1, /* single short press + quiet */
|
||||||
|
BTN_EVENT_REARM_COMBO = 2, /* SHORT + SHORT + LONG */
|
||||||
|
} hw_btn_event_t;
|
||||||
|
|
||||||
|
/*
|
||||||
|
* hw_button_init() — configure GPIO (active-low pull-up), zero FSM state.
|
||||||
|
* Call once at startup.
|
||||||
|
*/
|
||||||
|
void hw_button_init(void);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* hw_button_tick(now_ms) — advance debounce FSM and sequence detector.
|
||||||
|
* Call every ms from the main loop. Returns BTN_EVENT_NONE unless a
|
||||||
|
* complete gesture was recognised this tick.
|
||||||
|
*/
|
||||||
|
hw_btn_event_t hw_button_tick(uint32_t now_ms);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* hw_button_is_pressed() — true while button is confirmed held (post-debounce).
|
||||||
|
*/
|
||||||
|
bool hw_button_is_pressed(void);
|
||||||
|
|
||||||
|
#ifdef TEST_HOST
|
||||||
|
/* Inject a simulated raw pin state for host-side unit tests. */
|
||||||
|
void hw_button_inject(bool pressed);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* HW_BUTTON_H */
|
||||||
@ -38,6 +38,7 @@
|
|||||||
#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */
|
#define ORIN_CAN_ID_FC_VESC 0x401u /* VESC RPM + current at 10 Hz */
|
||||||
#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */
|
#define ORIN_CAN_ID_FC_IMU 0x402u /* full IMU angles + cal status at 50 Hz (Issue #680) */
|
||||||
#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
|
#define ORIN_CAN_ID_FC_BARO 0x403u /* barometer pressure/temp/altitude at 1 Hz (Issue #672) */
|
||||||
|
#define ORIN_CAN_ID_FC_BTN 0x404u /* button event on-demand (Issue #682) */
|
||||||
|
|
||||||
/* ---- Timing ---- */
|
/* ---- Timing ---- */
|
||||||
#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */
|
#define ORIN_HB_TIMEOUT_MS 500u /* Orin offline after 500 ms without any frame */
|
||||||
@ -92,6 +93,13 @@ typedef struct __attribute__((packed)) {
|
|||||||
int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */
|
int16_t alt_cm; /* altitude in cm (reference = pressure at boot) */
|
||||||
} orin_can_fc_baro_t; /* 8 bytes */
|
} orin_can_fc_baro_t; /* 8 bytes */
|
||||||
|
|
||||||
|
/* FC_BTN (0x404) — button event, sent on demand (Issue #682)
|
||||||
|
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED (pitch too large) */
|
||||||
|
typedef struct __attribute__((packed)) {
|
||||||
|
uint8_t event_id; /* 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED */
|
||||||
|
uint8_t balance_state; /* balance_state_t value after the event */
|
||||||
|
} orin_can_fc_btn_t; /* 2 bytes */
|
||||||
|
|
||||||
/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
|
/* LED_CMD (0x304) — Orin → FC LED pattern override (Issue #685)
|
||||||
* duration_ms = 0: hold until next state change; >0: revert after duration */
|
* duration_ms = 0: hold until next state change; >0: revert after duration */
|
||||||
typedef struct __attribute__((packed)) {
|
typedef struct __attribute__((packed)) {
|
||||||
@ -149,4 +157,11 @@ void orin_can_broadcast_imu(uint32_t now_ms,
|
|||||||
void orin_can_broadcast_baro(uint32_t now_ms,
|
void orin_can_broadcast_baro(uint32_t now_ms,
|
||||||
const orin_can_fc_baro_t *baro_tlm);
|
const orin_can_fc_baro_t *baro_tlm);
|
||||||
|
|
||||||
|
/*
|
||||||
|
* orin_can_send_btn_event(event_id, balance_state) — send FC_BTN (0x404)
|
||||||
|
* immediately. Call on button park/unpark events. Issue #682.
|
||||||
|
* event_id: 1=PARKED, 2=UNPARKED, 3=UNPARK_FAILED.
|
||||||
|
*/
|
||||||
|
void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state);
|
||||||
|
|
||||||
#endif /* ORIN_CAN_H */
|
#endif /* ORIN_CAN_H */
|
||||||
|
|||||||
@ -96,3 +96,26 @@ void balance_disarm(balance_t *b) {
|
|||||||
b->integral = 0.0f;
|
b->integral = 0.0f;
|
||||||
slope_estimator_reset(&b->slope);
|
slope_estimator_reset(&b->slope);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void balance_park(balance_t *b) {
|
||||||
|
/* Suspend balancing from ARMED state only — keeps robot stationary on flat ground */
|
||||||
|
if (b->state == BALANCE_ARMED) {
|
||||||
|
b->state = BALANCE_PARKED;
|
||||||
|
b->motor_cmd = 0;
|
||||||
|
b->integral = 0.0f;
|
||||||
|
b->prev_error = 0.0f;
|
||||||
|
slope_estimator_reset(&b->slope);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void balance_unpark(balance_t *b) {
|
||||||
|
/* Quick re-arm from PARKED — only if pitch is safe (< 20 deg) */
|
||||||
|
if (b->state == BALANCE_PARKED) {
|
||||||
|
if (fabsf(b->pitch_deg) < 20.0f) {
|
||||||
|
b->motor_cmd = 0;
|
||||||
|
b->prev_error = 0.0f;
|
||||||
|
b->state = BALANCE_ARMED;
|
||||||
|
}
|
||||||
|
/* If pitch too large, stay PARKED — caller checks resulting state */
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|||||||
179
src/hw_button.c
Normal file
179
src/hw_button.c
Normal file
@ -0,0 +1,179 @@
|
|||||||
|
/* hw_button.c — hardware button debounce + gesture detection (Issue #682)
|
||||||
|
*
|
||||||
|
* Debounce FSM:
|
||||||
|
* IDLE → (raw press detected) → DEBOUNCING
|
||||||
|
* DEBOUNCING → (still pressed after BTN_DEBOUNCE_MS) → HELD
|
||||||
|
* HELD → (released) → classify press type, back to IDLE
|
||||||
|
*
|
||||||
|
* Press types:
|
||||||
|
* SHORT held < BTN_LONG_MIN_MS from confirmed start
|
||||||
|
* LONG held >= BTN_LONG_MIN_MS
|
||||||
|
*
|
||||||
|
* Sequence detection (operates on classified presses):
|
||||||
|
* Buffer up to 3 presses. Recognised patterns:
|
||||||
|
* [SHORT, SHORT, LONG] -> BTN_EVENT_REARM_COMBO (fires on LONG release)
|
||||||
|
* [SHORT] + BTN_COMMIT_MS timeout -> BTN_EVENT_PARK
|
||||||
|
* Sequence reset after BTN_SEQ_TIMEOUT_MS from first press.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "hw_button.h"
|
||||||
|
#include "config.h"
|
||||||
|
|
||||||
|
#ifndef TEST_HOST
|
||||||
|
#include "stm32f7xx_hal.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* ---- Timing defaults (override in config.h) ---- */
|
||||||
|
#ifndef BTN_DEBOUNCE_MS
|
||||||
|
#define BTN_DEBOUNCE_MS 20u
|
||||||
|
#endif
|
||||||
|
#ifndef BTN_LONG_MIN_MS
|
||||||
|
#define BTN_LONG_MIN_MS 1500u
|
||||||
|
#endif
|
||||||
|
#ifndef BTN_COMMIT_MS
|
||||||
|
#define BTN_COMMIT_MS 500u
|
||||||
|
#endif
|
||||||
|
#ifndef BTN_SEQ_TIMEOUT_MS
|
||||||
|
#define BTN_SEQ_TIMEOUT_MS 3000u
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* ---- Press type ---- */
|
||||||
|
typedef enum {
|
||||||
|
_PT_SHORT = 1u,
|
||||||
|
_PT_LONG = 2u,
|
||||||
|
} _press_type_t;
|
||||||
|
|
||||||
|
/* ---- Debounce state ---- */
|
||||||
|
typedef enum {
|
||||||
|
_BTN_IDLE,
|
||||||
|
_BTN_DEBOUNCING,
|
||||||
|
_BTN_HELD,
|
||||||
|
} _btn_state_t;
|
||||||
|
|
||||||
|
static _btn_state_t s_state = _BTN_IDLE;
|
||||||
|
static uint32_t s_trans_ms = 0u; /* timestamp of last FSM transition */
|
||||||
|
static bool s_pressed = false;
|
||||||
|
|
||||||
|
/* ---- Sequence buffer ---- */
|
||||||
|
#define _SEQ_MAX 3u
|
||||||
|
static _press_type_t s_seq[_SEQ_MAX];
|
||||||
|
static uint8_t s_seq_len = 0u;
|
||||||
|
static uint32_t s_seq_first_ms = 0u;
|
||||||
|
static uint32_t s_seq_last_ms = 0u;
|
||||||
|
|
||||||
|
/* ---- GPIO read ---- */
|
||||||
|
#ifdef TEST_HOST
|
||||||
|
static bool s_test_raw = false;
|
||||||
|
void hw_button_inject(bool pressed) { s_test_raw = pressed; }
|
||||||
|
static bool _read_raw(void) { return s_test_raw; }
|
||||||
|
#else
|
||||||
|
static bool _read_raw(void)
|
||||||
|
{
|
||||||
|
return HAL_GPIO_ReadPin(BTN_PORT, BTN_PIN) == GPIO_PIN_RESET; /* active-low */
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
void hw_button_init(void)
|
||||||
|
{
|
||||||
|
#ifndef TEST_HOST
|
||||||
|
__HAL_RCC_GPIOC_CLK_ENABLE(); /* BTN_PORT assumed GPIOC; adjust if needed */
|
||||||
|
GPIO_InitTypeDef g = {0};
|
||||||
|
g.Pin = BTN_PIN;
|
||||||
|
g.Mode = GPIO_MODE_INPUT;
|
||||||
|
g.Pull = GPIO_PULLUP;
|
||||||
|
HAL_GPIO_Init(BTN_PORT, &g);
|
||||||
|
#endif
|
||||||
|
s_state = _BTN_IDLE;
|
||||||
|
s_seq_len = 0u;
|
||||||
|
s_pressed = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool hw_button_is_pressed(void)
|
||||||
|
{
|
||||||
|
return s_pressed;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Record a classified press into the sequence buffer and check for patterns. */
|
||||||
|
static hw_btn_event_t _record_press(_press_type_t pt, uint32_t now_ms)
|
||||||
|
{
|
||||||
|
if (s_seq_len == 0u) {
|
||||||
|
s_seq_first_ms = now_ms;
|
||||||
|
}
|
||||||
|
s_seq_last_ms = now_ms;
|
||||||
|
|
||||||
|
if (s_seq_len < _SEQ_MAX) {
|
||||||
|
s_seq[s_seq_len++] = pt;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Check REARM_COMBO: SHORT + SHORT + LONG */
|
||||||
|
if (s_seq_len == 3u &&
|
||||||
|
s_seq[0] == _PT_SHORT &&
|
||||||
|
s_seq[1] == _PT_SHORT &&
|
||||||
|
s_seq[2] == _PT_LONG) {
|
||||||
|
s_seq_len = 0u;
|
||||||
|
return BTN_EVENT_REARM_COMBO;
|
||||||
|
}
|
||||||
|
|
||||||
|
return BTN_EVENT_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
hw_btn_event_t hw_button_tick(uint32_t now_ms)
|
||||||
|
{
|
||||||
|
bool raw = _read_raw();
|
||||||
|
|
||||||
|
/* ---- Debounce FSM ---- */
|
||||||
|
switch (s_state) {
|
||||||
|
case _BTN_IDLE:
|
||||||
|
if (raw) {
|
||||||
|
s_state = _BTN_DEBOUNCING;
|
||||||
|
s_trans_ms = now_ms;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case _BTN_DEBOUNCING:
|
||||||
|
if (!raw) {
|
||||||
|
/* Released before debounce elapsed — bounce, ignore */
|
||||||
|
s_state = _BTN_IDLE;
|
||||||
|
} else if ((now_ms - s_trans_ms) >= BTN_DEBOUNCE_MS) {
|
||||||
|
s_state = _BTN_HELD;
|
||||||
|
s_trans_ms = now_ms; /* record confirmed press-start time */
|
||||||
|
s_pressed = true;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case _BTN_HELD:
|
||||||
|
if (!raw) {
|
||||||
|
s_pressed = false;
|
||||||
|
uint32_t held_ms = now_ms - s_trans_ms;
|
||||||
|
_press_type_t pt = (held_ms >= BTN_LONG_MIN_MS) ? _PT_LONG : _PT_SHORT;
|
||||||
|
s_state = _BTN_IDLE;
|
||||||
|
hw_btn_event_t ev = _record_press(pt, now_ms);
|
||||||
|
if (ev != BTN_EVENT_NONE) {
|
||||||
|
return ev;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ---- Sequence timeout / commit check (only when not currently held) ---- */
|
||||||
|
if (s_state == _BTN_IDLE && s_seq_len > 0u) {
|
||||||
|
uint32_t since_first = now_ms - s_seq_first_ms;
|
||||||
|
uint32_t since_last = now_ms - s_seq_last_ms;
|
||||||
|
|
||||||
|
/* Whole sequence window expired — abandon */
|
||||||
|
if (since_first >= BTN_SEQ_TIMEOUT_MS) {
|
||||||
|
s_seq_len = 0u;
|
||||||
|
return BTN_EVENT_NONE;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Single short press + BTN_COMMIT_MS of quiet -> PARK */
|
||||||
|
if (s_seq_len == 1u &&
|
||||||
|
s_seq[0] == _PT_SHORT &&
|
||||||
|
since_last >= BTN_COMMIT_MS) {
|
||||||
|
s_seq_len = 0u;
|
||||||
|
return BTN_EVENT_PARK;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return BTN_EVENT_NONE;
|
||||||
|
}
|
||||||
29
src/main.c
29
src/main.c
@ -36,6 +36,7 @@
|
|||||||
#include "vesc_can.h"
|
#include "vesc_can.h"
|
||||||
#include "orin_can.h"
|
#include "orin_can.h"
|
||||||
#include "imu_cal_flash.h"
|
#include "imu_cal_flash.h"
|
||||||
|
#include "hw_button.h"
|
||||||
#include "servo_bus.h"
|
#include "servo_bus.h"
|
||||||
#include "gimbal.h"
|
#include "gimbal.h"
|
||||||
#include "lvc.h"
|
#include "lvc.h"
|
||||||
@ -218,6 +219,9 @@ int main(void) {
|
|||||||
vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
|
vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
|
||||||
orin_can_init();
|
orin_can_init();
|
||||||
|
|
||||||
|
/* Init hardware button debounce/gesture driver (Issue #682) */
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
/* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
|
/* Send fault log summary on boot if a prior fault was recorded (Issue #565) */
|
||||||
if (fault_get_last_type() != FAULT_NONE) {
|
if (fault_get_last_type() != FAULT_NONE) {
|
||||||
fault_log_entry_t fle;
|
fault_log_entry_t fle;
|
||||||
@ -520,6 +524,31 @@ int main(void) {
|
|||||||
safety_remote_estop_clear();
|
safety_remote_estop_clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Hardware button park/re-arm (Issue #682).
|
||||||
|
* Short press -> park (ARMED only): freeze PID, stop motors, amber LED.
|
||||||
|
* SHORT+SHORT+LONG combo -> unpark (PARKED only): resume if upright. */
|
||||||
|
{
|
||||||
|
hw_btn_event_t btn_ev = hw_button_tick(now);
|
||||||
|
if (btn_ev == BTN_EVENT_PARK) {
|
||||||
|
if (bal.state == BALANCE_ARMED) {
|
||||||
|
balance_park(&bal);
|
||||||
|
led_set_state(LED_STATE_LOW_BATT);
|
||||||
|
orin_can_send_btn_event(1u, (uint8_t)bal.state);
|
||||||
|
}
|
||||||
|
} else if (btn_ev == BTN_EVENT_REARM_COMBO) {
|
||||||
|
if (bal.state == BALANCE_PARKED) {
|
||||||
|
balance_unpark(&bal);
|
||||||
|
if (bal.state == BALANCE_ARMED) {
|
||||||
|
led_set_state(LED_STATE_ARMED);
|
||||||
|
orin_can_send_btn_event(2u, (uint8_t)bal.state);
|
||||||
|
} else {
|
||||||
|
/* Pitch too large — unpark refused, stay parked */
|
||||||
|
orin_can_send_btn_event(3u, (uint8_t)bal.state);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
/* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
|
||||||
if (jlink_state.fault_log_req) {
|
if (jlink_state.fault_log_req) {
|
||||||
jlink_state.fault_log_req = 0u;
|
jlink_state.fault_log_req = 0u;
|
||||||
|
|||||||
@ -140,3 +140,13 @@ void orin_can_broadcast_baro(uint32_t now_ms,
|
|||||||
memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t));
|
memcpy(buf, baro_tlm, sizeof(orin_can_fc_baro_t));
|
||||||
can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t));
|
can_driver_send_std(ORIN_CAN_ID_FC_BARO, buf, (uint8_t)sizeof(orin_can_fc_baro_t));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void orin_can_send_btn_event(uint8_t event_id, uint8_t balance_state)
|
||||||
|
{
|
||||||
|
orin_can_fc_btn_t btn;
|
||||||
|
btn.event_id = event_id;
|
||||||
|
btn.balance_state = balance_state;
|
||||||
|
uint8_t buf[2];
|
||||||
|
memcpy(buf, &btn, sizeof(orin_can_fc_btn_t));
|
||||||
|
can_driver_send_std(ORIN_CAN_ID_FC_BTN, buf, (uint8_t)sizeof(orin_can_fc_btn_t));
|
||||||
|
}
|
||||||
|
|||||||
283
test/test_hw_button.c
Normal file
283
test/test_hw_button.c
Normal file
@ -0,0 +1,283 @@
|
|||||||
|
/*
|
||||||
|
* test_hw_button.c — Unit tests for hw_button debounce/gesture driver (Issue #682).
|
||||||
|
*
|
||||||
|
* Build (host, no hardware):
|
||||||
|
* gcc -I include -I test/stubs -DTEST_HOST \
|
||||||
|
* -o /tmp/test_hw_button src/hw_button.c test/test_hw_button.c
|
||||||
|
*
|
||||||
|
* All tests use hw_button_inject() to drive the simulated GPIO pin.
|
||||||
|
* All timing is driven through the now_ms argument passed to hw_button_tick().
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* ---- Stubs: prevent STM32 HAL from being included in non-TEST_HOST path ---- */
|
||||||
|
#define STM32F7XX_HAL_H
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
/* ---- Include implementation directly ---- */
|
||||||
|
#include "../src/hw_button.c"
|
||||||
|
|
||||||
|
/* ---- Test framework ---- */
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
static int g_pass = 0;
|
||||||
|
static int g_fail = 0;
|
||||||
|
|
||||||
|
#define ASSERT(cond, msg) \
|
||||||
|
do { \
|
||||||
|
if (cond) { g_pass++; } \
|
||||||
|
else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
|
||||||
|
} while (0)
|
||||||
|
|
||||||
|
/* ---- Helpers ---- */
|
||||||
|
|
||||||
|
/* Advance time by `ms` ticks with the pin held at `pressed`.
|
||||||
|
* Returns the last non-NONE event seen, or BTN_EVENT_NONE. */
|
||||||
|
static hw_btn_event_t _tick_n(uint32_t *now, uint32_t ms, bool pressed)
|
||||||
|
{
|
||||||
|
hw_btn_event_t last = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(pressed);
|
||||||
|
for (uint32_t i = 0; i < ms; i++) {
|
||||||
|
(*now)++;
|
||||||
|
hw_btn_event_t ev = hw_button_tick(*now);
|
||||||
|
if (ev != BTN_EVENT_NONE) last = ev;
|
||||||
|
}
|
||||||
|
return last;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Reset driver state between tests. */
|
||||||
|
static void _reset(uint32_t *now)
|
||||||
|
{
|
||||||
|
*now = 0;
|
||||||
|
hw_button_inject(false);
|
||||||
|
hw_button_init();
|
||||||
|
/* Drain any pending sequence state */
|
||||||
|
for (int i = 0; i < 100; i++) hw_button_tick((*now)++);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ---- Tests ---- */
|
||||||
|
|
||||||
|
static void test_idle_no_event(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (int i = 0; i < 1000; i++) {
|
||||||
|
hw_btn_event_t ev = hw_button_tick(now++);
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "idle: no event while pin released");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_debounce_bounce_ignored(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press for only 10 ms (< BTN_DEBOUNCE_MS=20) then release */
|
||||||
|
hw_btn_event_t ev = _tick_n(&now, 10, true);
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "bounce < debounce: no event during press");
|
||||||
|
ev = _tick_n(&now, BTN_COMMIT_MS + 50, false);
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "bounce < debounce: no event after release");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_debounce_confirmed(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press for BTN_DEBOUNCE_MS + 1 ms — debounce should confirm */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 1, true);
|
||||||
|
ASSERT(hw_button_is_pressed(), "debounce passed: is_pressed() == true");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_short_press_park(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press for BTN_DEBOUNCE_MS + 10 ms (short: well < BTN_LONG_MIN_MS) */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
/* Release */
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
ASSERT(!hw_button_is_pressed(), "short press: released");
|
||||||
|
|
||||||
|
/* Wait BTN_COMMIT_MS quiet -> should fire PARK */
|
||||||
|
hw_btn_event_t ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) {
|
||||||
|
now++;
|
||||||
|
hw_btn_event_t e = hw_button_tick(now);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_PARK, "short press + quiet: BTN_EVENT_PARK");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_long_press_no_park(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press for BTN_LONG_MIN_MS + 100 ms (long press) */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 100, true);
|
||||||
|
/* Release and wait */
|
||||||
|
hw_btn_event_t ev = _tick_n(&now, BTN_COMMIT_MS + 10, false);
|
||||||
|
/* Lone LONG press has no defined gesture */
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "lone long press: no event");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_rearm_combo_fires(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press 1: SHORT (30 ms hold + release) */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
/* Brief gap between presses (< BTN_COMMIT_MS so no PARK fires) */
|
||||||
|
_tick_n(&now, 100, false);
|
||||||
|
|
||||||
|
/* Press 2: SHORT */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
_tick_n(&now, 100, false);
|
||||||
|
|
||||||
|
/* Press 3: LONG — combo fires on release */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 50, true);
|
||||||
|
hw_btn_event_t ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (int i = 0; i < 5; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_REARM_COMBO, "SHORT+SHORT+LONG: BTN_EVENT_REARM_COMBO");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_rearm_combo_resets_buffer(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Fire combo once */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 100, false);
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 100, false);
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + BTN_LONG_MIN_MS + 50, true);
|
||||||
|
hw_button_inject(false);
|
||||||
|
hw_btn_event_t ev = BTN_EVENT_NONE;
|
||||||
|
for (int i = 0; i < 5; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_REARM_COMBO, "first combo fires");
|
||||||
|
|
||||||
|
/* After combo, a lone short press should produce PARK */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_PARK, "after combo: short press fires PARK");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_seq_timeout_clears_buffer(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* TWO short presses — seq_len=2 (no PARK since it's not a lone SHORT).
|
||||||
|
* Then wait BTN_SEQ_TIMEOUT_MS — buffer should be abandoned silently. */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
_tick_n(&now, 100, false); /* gap between presses (< BTN_COMMIT_MS) */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
|
||||||
|
/* Wait BTN_SEQ_TIMEOUT_MS — seq_len=2 buffer expires silently */
|
||||||
|
hw_btn_event_t ev = _tick_n(&now, BTN_SEQ_TIMEOUT_MS + 10, false);
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "seq timeout (seq_len=2): buffer cleared, no event");
|
||||||
|
|
||||||
|
/* Fresh lone short press should produce PARK as a new sequence */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_PARK, "after timeout: fresh short press fires PARK");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_is_pressed_tracks_held(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
ASSERT(!hw_button_is_pressed(), "initially not pressed");
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 1, true);
|
||||||
|
ASSERT(hw_button_is_pressed(), "after debounce: is_pressed true");
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
ASSERT(!hw_button_is_pressed(), "after release: is_pressed false");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_short_press_before_commit_no_park(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Press and release */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true);
|
||||||
|
_tick_n(&now, 1, false);
|
||||||
|
|
||||||
|
/* Wait less than BTN_COMMIT_MS — no PARK yet */
|
||||||
|
hw_btn_event_t ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (uint32_t i = 0; i < BTN_COMMIT_MS - 50; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "before commit window: no PARK yet");
|
||||||
|
}
|
||||||
|
|
||||||
|
static void test_two_shorts_no_park(void)
|
||||||
|
{
|
||||||
|
uint32_t now = 0;
|
||||||
|
hw_button_init();
|
||||||
|
|
||||||
|
/* Two short presses close together — seq_len=2 does not trigger PARK */
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 1, false);
|
||||||
|
_tick_n(&now, 100, false);
|
||||||
|
_tick_n(&now, BTN_DEBOUNCE_MS + 10, true); _tick_n(&now, 1, false);
|
||||||
|
|
||||||
|
hw_btn_event_t ev = BTN_EVENT_NONE;
|
||||||
|
hw_button_inject(false);
|
||||||
|
for (uint32_t i = 0; i < BTN_COMMIT_MS + 10; i++) {
|
||||||
|
hw_btn_event_t e = hw_button_tick(now++);
|
||||||
|
if (e != BTN_EVENT_NONE) ev = e;
|
||||||
|
}
|
||||||
|
ASSERT(ev == BTN_EVENT_NONE, "two shorts + quiet: no PARK (seq_len=2, not lone SHORT)");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ---- main ---- */
|
||||||
|
int main(void)
|
||||||
|
{
|
||||||
|
printf("=== test_hw_button ===\n");
|
||||||
|
|
||||||
|
test_idle_no_event();
|
||||||
|
test_debounce_bounce_ignored();
|
||||||
|
test_debounce_confirmed();
|
||||||
|
test_short_press_park();
|
||||||
|
test_long_press_no_park();
|
||||||
|
test_rearm_combo_fires();
|
||||||
|
test_rearm_combo_resets_buffer();
|
||||||
|
test_seq_timeout_clears_buffer();
|
||||||
|
test_is_pressed_tracks_held();
|
||||||
|
test_short_press_before_commit_no_park();
|
||||||
|
test_two_shorts_no_park();
|
||||||
|
|
||||||
|
printf("\nResults: %d passed, %d failed\n", g_pass, g_fail);
|
||||||
|
return g_fail ? 1 : 0;
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user