sl-firmware
d41a9dfe10
STM32 firmware: - safety.h/c: EstopSource enum, safety_remote_estop/clear/get/active() CDC 'E'=ESTOP_REMOTE, 'F'=ESTOP_CELLULAR_TIMEOUT, 'Z'=clear latch - usbd_cdc_if: cdc_estop_request/cdc_estop_clear_request volatile flags - status: status_update() +remote_estop param; both LEDs fast-blink 200ms - main.c: immediate motor cutoff highest-priority; arming gated by !safety_remote_estop_active(); motor estop auto-clear gated; telemetry 'es' field 0-4; status_update() updated to 5 args Safety: IMMEDIATE motor cutoff, latched until explicit Z + DISARMED, cannot re-arm via MQTT alone (requires RC arm hold). IWDG-safe. Jetson bridge: - remote_estop_node.py: paho-mqtt + pyserial, cellular watchdog 5s - estop_params.yaml, remote_estop.launch.py - setup.py / package.xml: register node + paho-mqtt dep - docker-compose.yml: remote-estop service - test_remote_estop.py: kill/clear/watchdog/latency unit tests Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
70 lines
2.4 KiB
C
70 lines
2.4 KiB
C
#include "stm32f7xx_hal.h"
|
|
#include "config.h"
|
|
#include "status.h"
|
|
|
|
void status_init(void) {
|
|
__HAL_RCC_GPIOB_CLK_ENABLE();
|
|
__HAL_RCC_GPIOC_CLK_ENABLE();
|
|
|
|
GPIO_InitTypeDef gpio = {0};
|
|
gpio.Mode = GPIO_MODE_OUTPUT_PP;
|
|
gpio.Pull = GPIO_NOPULL;
|
|
gpio.Speed = GPIO_SPEED_FREQ_LOW;
|
|
|
|
/* LED1 - PC15 */
|
|
gpio.Pin = LED1_PIN;
|
|
HAL_GPIO_Init(LED1_PORT, &gpio);
|
|
|
|
/* LED2 - PC14 */
|
|
gpio.Pin = LED2_PIN;
|
|
HAL_GPIO_Init(LED2_PORT, &gpio);
|
|
|
|
/* Buzzer - PB2 (inverted: HIGH = off) */
|
|
gpio.Pin = BEEPER_PIN;
|
|
HAL_GPIO_Init(BEEPER_PORT, &gpio);
|
|
|
|
/* Start with both LEDs ON (active low) */
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
|
|
|
|
/* Buzzer off */
|
|
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
|
|
}
|
|
|
|
void status_boot_beep(void) {
|
|
/* Two quick beeps */
|
|
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET);
|
|
HAL_Delay(80);
|
|
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
|
|
HAL_Delay(80);
|
|
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_SET);
|
|
HAL_Delay(80);
|
|
HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
|
|
}
|
|
|
|
void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop) {
|
|
GPIO_PinState fast_blink = ((tick / 200) % 2) ? GPIO_PIN_RESET : GPIO_PIN_SET;
|
|
GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET /* ON half */
|
|
: GPIO_PIN_SET; /* OFF half */
|
|
if (remote_estop) {
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, fast_blink);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, fast_blink);
|
|
} else if (!imu_ok) {
|
|
/* IMU error: LED1 blinking (attention), LED2 solid ON */
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
|
|
} else if (tilt_fault) {
|
|
/* Tilt fault: both LEDs slow blink */
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, blink);
|
|
} else if (armed) {
|
|
/* Armed: both LEDs solid ON */
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);
|
|
} else {
|
|
/* Normal disarmed: LED1 solid ON, LED2 off */
|
|
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, GPIO_PIN_RESET);
|
|
HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_SET);
|
|
}
|
|
}
|