seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity
saltylab-firmware/src/main.c
sl-mechanical 170c64eec1 feat: Add watchdog reset detection and status reporting (Issue #300) 
- Detect if MCU was reset by IWDG watchdog timeout at startup
- Log watchdog reset events to debug terminal (USB CDC)
- Store watchdog reset flag for status reporting to Jetson
- Watchdog timer configured with 2-second timeout in safety_init()
- Main loop calls safety_refresh() to kick the watchdog every iteration

The IWDG (Independent Watchdog) resets the MCU if the main loop
hangs and fails to call safety_refresh() within the timeout window.
This provides hardware-enforced detection of software failures.

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-04 12:17:56 -05:00

617 lines
25 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "stm32f7xx_hal.h"
#include "usbd_core.h"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include "usbd_desc.h"
#include "mpu6000.h"
#include "balance.h"
#include "hoverboard.h"
#include "motor_driver.h"
#include "mode_manager.h"
#include "config.h"
#include "status.h"
#include "safety.h"
#include "crsf.h"
#include "i2c1.h"
#include "bmp280.h"
#include "mag.h"
#include "bno055.h"
#include "jetson_cmd.h"
#include "jlink.h"
#include "ota.h"
#include "audio.h"
#include "buzzer.h"
#include "led.h"
#include "servo.h"
#include "ina219.h"
#include "ultrasonic.h"
#include "power_mgmt.h"
#include "battery.h"
#include "coulomb_counter.h"
#include "watchdog.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
extern volatile uint8_t cdc_streaming; /* set by S command in CDC RX */
extern volatile uint8_t cdc_arm_request; /* set by A command */
extern volatile uint8_t cdc_disarm_request; /* set by D command */
extern volatile uint8_t cdc_recal_request; /* set by G command */
extern volatile uint32_t cdc_rx_count; /* total CDC packets received */
extern volatile uint8_t cdc_estop_request;
extern volatile uint8_t cdc_estop_clear_request;
/* BNO055 active flag (set if BNO055 initialized successfully) */
static bool bno055_active = false;
/* Watchdog reset flag (set if MCU was reset by IWDG timeout) */
static bool g_watchdog_reset_detected = false;
/*
* Apply a PID tuning command string from the USB terminal.
* Format: P<kp> I<ki> D<kd> T<setpoint_deg> M<max_speed> ?
* Returns a short ack string written into `reply` (max reply_sz bytes).
*/
static void apply_pid_cmd(balance_t *b, const char *cmd, char *reply, int reply_sz) {
float val = 0.0f;
/* Simple ASCII float parse — avoids sscanf in hot path */
if (cmd[0] == '?') {
snprintf(reply, reply_sz,
"{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
(int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
(int)(b->setpoint * 10), (int)b->max_speed);
return;
}
/* Parse float after the command letter */
if (sscanf(cmd + 1, "%f", &val) != 1) {
snprintf(reply, reply_sz, "{\"err\":\"bad_fmt\"}\n");
return;
}
switch (cmd[0]) {
case 'P': if (val >= 0.0f && val <= 500.0f) b->kp = val; break;
case 'I': if (val >= 0.0f && val <= 50.0f) b->ki = val; break;
case 'K': /* alias for D */ /* fall through */
case 'D': if (val >= 0.0f && val <= 50.0f) b->kd = val; break;
case 'T': if (val >= -20.0f && val <= 20.0f) b->setpoint = val; break;
case 'M': if (val >= 0.0f && val <= 1000.0f) b->max_speed = (int16_t)val; break;
}
snprintf(reply, reply_sz,
"{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
(int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
(int)(b->setpoint * 10), (int)b->max_speed);
}
USBD_HandleTypeDef hUsbDevice;
static void SystemClock_Config(void) {
RCC_OscInitTypeDef osc = {0};
RCC_ClkInitTypeDef clk = {0};
RCC_PeriphCLKInitTypeDef pclk = {0};
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
osc.OscillatorType = RCC_OSCILLATORTYPE_HSE;
osc.HSEState = RCC_HSE_ON;
osc.PLL.PLLState = RCC_PLL_ON;
osc.PLL.PLLSource = RCC_PLLSOURCE_HSE;
osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9;
if (HAL_RCC_OscConfig(&osc) != HAL_OK) {
osc.OscillatorType = RCC_OSCILLATORTYPE_HSI;
osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16;
if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1);
}
HAL_PWREx_EnableOverDrive();
pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1;
pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP;
pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8;
pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1);
clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
clk.AHBCLKDivider = RCC_SYSCLK_DIV1;
clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2;
HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7);
}
extern PCD_HandleTypeDef hpcd;
void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); }
void SysTick_Handler(void) { HAL_IncTick(); }
int main(void) {
SCB_EnableICache();
/* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */
checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */
HAL_Init();
SystemClock_Config();
/* Detect watchdog reset (Issue #300) — must be before safety_init() */
g_watchdog_reset_detected = watchdog_was_reset_by_watchdog();
/* USB CDC */
USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0);
USBD_RegisterClass(&hUsbDevice, &USBD_CDC);
USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops);
USBD_Start(&hUsbDevice);
status_init();
HAL_Delay(3000); /* Wait for USB host to enumerate */
/* Log watchdog reset event if it occurred (Issue #300) */
if (g_watchdog_reset_detected) {
printf("[WATCHDOG] MCU reset by IWDG timeout detected. Main loop may have hung.\n");
}
/* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */
int imu_ret = mpu6000_init() ? 0 : -1;
/*
* Gyro bias calibration — hold board still for ~1s.
* Must run before safety_init() (IWDG not yet started).
* Blocks arming and streaming until complete.
*/
if (imu_ret == 0) mpu6000_calibrate();
/* Init hoverboard ESC UART */
hoverboard_init();
/* Init balance controller */
balance_t bal;
balance_init(&bal);
/* Init motor driver */
motor_driver_t motors;
motor_driver_init(&motors);
/* Init CRSF/ELRS receiver on UART4 (PA0/PA1) with DMA */
crsf_init();
/* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
jlink_init();
/* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */
audio_init();
audio_play_tone(AUDIO_TONE_STARTUP);
/* Init piezo buzzer driver (TIM4_CH3 PWM on PB2, Issue #189) */
buzzer_init();
buzzer_play_melody(MELODY_STARTUP);
/* Init WS2812B NeoPixel LED ring (TIM3_CH1 PWM on PB4, Issue #193) */
led_init();
led_set_state(LED_STATE_BOOT);
/* Init power management — STOP-mode sleep/wake, wake EXTIs configured */
power_mgmt_init();
/* Init servo pan-tilt driver for camera head (TIM4 PWM on PB6/PB7, Issue #206) */
servo_init();
/* Init HC-SR04 ultrasonic distance sensor (TIM1 input capture on PA1, Issue #243) */
ultrasonic_init();
/* Init mode manager (RC/autonomous blend; CH6 mode switch) */
mode_manager_t mode;
mode_manager_init(&mode);
/* Init battery ADC (PC1/ADC3 — Vbat divider 11:1) for CRSF telemetry */
battery_init();
/* Probe I2C1 for optional sensors — skip gracefully if not found */
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
mag_type_t mag_type = MAG_NONE;
if (i2c1_init() == 0) {
int chip = bmp280_init();
baro_chip = (chip > 0) ? chip : 0;
mag_type = mag_init();
ina219_init(); /* Init INA219 dual motor current monitoring (Issue #214) */
}
/*
* IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init()
* which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
* Once started, safety_refresh() MUST be called every WATCHDOG_TIMEOUT_MS
* or MCU resets. IWDG cannot be stopped once started (hardware enforced).
*/
safety_init();
char buf[320];
int len;
uint32_t tx_count = 0; /* telemetry frames sent to host */
IMUData imu;
uint32_t send_tick = 0;
uint32_t balance_tick = 0;
uint32_t esc_tick = 0;
uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */
uint32_t jlink_tlm_tick = 0; /* Jetson binary telemetry TX timer */
uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
while (1) {
if (imu_ret == 0) mpu6000_read(&imu);
uint32_t now = HAL_GetTick();
/* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
safety_refresh();
/* Advance audio tone sequencer (non-blocking, call every tick) */
audio_tick(now);
/* Advance buzzer pattern sequencer (non-blocking, call every tick) */
buzzer_tick(now);
/* Advance LED animation sequencer (non-blocking, call every tick) */
led_tick(now);
/* Servo pan-tilt animation tick — updates smooth sweeps */
servo_tick(now);
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
battery_accumulate_coulombs();
/* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
* pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
pm_pwm_phase++;
{
uint8_t pm_bright = power_mgmt_led_brightness();
if (pm_bright > 0u) {
bool led_on = (pm_pwm_phase < pm_bright);
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN,
led_on ? GPIO_PIN_RESET : GPIO_PIN_SET);
}
}
/* Power manager tick — may block in WFI (STOP mode) when disarmed */
if (bal.state != BALANCE_ARMED) {
power_mgmt_tick(now);
}
/* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
mode_manager_update(&mode, now);
if (cdc_estop_request) {
EstopSource src = (cdc_estop_request == 1) ? ESTOP_REMOTE : ESTOP_CELLULAR_TIMEOUT;
cdc_estop_request = 0;
safety_remote_estop(src);
safety_arm_cancel();
balance_disarm(&bal);
motor_driver_estop(&motors);
}
if (cdc_estop_clear_request) {
cdc_estop_clear_request = 0;
if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
safety_remote_estop_clear();
}
/* Drain JLink DMA circular buffer and parse frames */
jlink_process();
/* Handle JLink one-shot flags from Jetson binary protocol */
if (jlink_state.estop_req) {
jlink_state.estop_req = 0u;
safety_remote_estop(ESTOP_REMOTE);
safety_arm_cancel();
balance_disarm(&bal);
motor_driver_estop(&motors);
}
if (jlink_state.arm_req) {
jlink_state.arm_req = 0u;
if (!safety_remote_estop_active() &&
mpu6000_is_calibrated() &&
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
safety_arm_start(now);
}
}
if (jlink_state.disarm_req) {
jlink_state.disarm_req = 0u;
safety_arm_cancel();
balance_disarm(&bal);
}
if (jlink_state.pid_updated) {
jlink_state.pid_updated = 0u;
bal.kp = jlink_state.pid_kp;
bal.ki = jlink_state.pid_ki;
bal.kd = jlink_state.pid_kd;
}
if (jlink_state.dfu_req) {
jlink_state.dfu_req = 0u;
/* ota_enter_dfu() is a no-op (returns false) when armed;
* never returns when disarmed — MCU resets into DFU mode. */
ota_enter_dfu(bal.state == BALANCE_ARMED);
}
if (jlink_state.sleep_req) {
jlink_state.sleep_req = 0u;
power_mgmt_request_sleep();
}
/* Power management: CRSF/JLink activity or armed state resets idle timer */
if ((crsf_state.last_rx_ms != 0 && (now - crsf_state.last_rx_ms) < 500) ||
jlink_is_active(now) ||
bal.state == BALANCE_ARMED) {
power_mgmt_activity();
}
/* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
* Applies regardless of active mode (CH5 always has kill authority). */
if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
safety_arm_cancel();
balance_disarm(&bal);
}
/* RC failsafe: disarm if signal lost AFTER RC was previously alive.
* Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */
if (bal.state == BALANCE_ARMED &&
crsf_state.last_rx_ms != 0 &&
(now - crsf_state.last_rx_ms) > CRSF_FAILSAFE_MS) {
safety_arm_cancel();
balance_disarm(&bal);
}
/* Tilt fault buzzer alert (one-shot on fault edge) */
safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT);
/* RC arm/disarm via CH5 switch (CRSF) — edge detect, same hold interlock */
{
static bool s_rc_armed_prev = false;
bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed;
if (rc_armed_now && !s_rc_armed_prev) {
/* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
if (!safety_remote_estop_active() &&
mpu6000_is_calibrated() &&
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
safety_arm_start(now);
}
}
if (!rc_armed_now && s_rc_armed_prev) {
/* Falling edge: cancel pending arm or disarm if already armed */
safety_arm_cancel();
if (bal.state == BALANCE_ARMED) balance_disarm(&bal);
}
s_rc_armed_prev = rc_armed_now;
}
/* Handle arm/disarm requests from USB with arm-hold interlock */
if (cdc_arm_request) {
cdc_arm_request = 0;
if (!safety_remote_estop_active() &&
mpu6000_is_calibrated() &&
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
safety_arm_start(now);
}
}
/* Complete arming once hold timer expires */
if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) {
safety_arm_cancel();
balance_arm(&bal);
audio_play_tone(AUDIO_TONE_ARM);
}
if (cdc_disarm_request) {
cdc_disarm_request = 0;
safety_arm_cancel();
balance_disarm(&bal);
}
/* Gyro recalibration — disarm first, then re-sample bias (~1s blocked) */
if (cdc_recal_request) {
cdc_recal_request = 0;
safety_arm_cancel();
balance_disarm(&bal);
if (imu_ret == 0) mpu6000_calibrate();
}
/* Handle PID tuning commands from USB (P/I/D/T/M/?) */
if (cdc_cmd_ready) {
cdc_cmd_ready = 0;
char reply[96];
apply_pid_cmd(&bal, (const char *)cdc_cmd_buf, reply, sizeof(reply));
CDC_Transmit((uint8_t *)reply, strlen(reply));
}
/* Handle Jetson C<speed>,<steer> command (parsed here, not in ISR) */
if (jetson_cmd_ready) {
jetson_cmd_ready = 0;
jetson_cmd_process();
}
/*
* Balance PID — 1kHz with MPU6000/SPI, ~250Hz with BNO055/I2C.
* BNO055 read (~3ms) is placed inside the gate so it doesn't block
* the full main loop; the gap between ticks is naturally longer.
* jlink takes priority for Jetson speed offset; falls back to CDC.
*/
if (imu_ret == 0 && now - balance_tick >= 1) {
if (bno055_active) bno055_read(&imu); /* I2C read inside gate */
balance_tick = now;
float base_sp = bal.setpoint;
if (jlink_is_active(now))
bal.setpoint += ((float)jlink_state.speed / 1000.0f) * JETSON_SPEED_MAX_DEG;
else if (jetson_cmd_is_active(now))
bal.setpoint += jetson_cmd_sp_offset();
balance_update(&bal, &imu, dt);
bal.setpoint = base_sp;
}
/* Latch estop on tilt fault or disarm */
{
static uint8_t s_prev_tilt_fault = 0;
uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u;
if (tilt_now && !s_prev_tilt_fault)
audio_play_tone(AUDIO_TONE_FAULT);
s_prev_tilt_fault = tilt_now;
}
if (bal.state == BALANCE_TILT_FAULT) {
motor_driver_estop(&motors);
} else if (bal.state == BALANCE_DISARMED && motors.estop &&
!safety_remote_estop_active()) {
motor_driver_estop_clear(&motors);
}
/* Feed autonomous steer from Jetson into mode manager.
* jlink takes priority over legacy CDC jetson_cmd.
* mode_manager_get_steer() blends it with RC steer per active mode. */
if (jlink_is_active(now))
mode_manager_set_auto_cmd(&mode, jlink_state.steer, 0);
else if (jetson_cmd_is_active(now))
mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
/* Send to hoverboard ESC at 50Hz (every 20ms) */
if (now - esc_tick >= 20) {
esc_tick = now;
if (bal.state == BALANCE_ARMED) {
/* Blended steer (RC ↔ auto per mode) + RC speed bias */
int16_t steer = mode_manager_get_steer(&mode);
int16_t spd_bias = mode_manager_get_speed_bias(&mode);
int32_t speed = (int32_t)bal.motor_cmd + spd_bias;
if (speed > 1000) speed = 1000;
if (speed < -1000) speed = -1000;
motor_driver_update(&motors, (int16_t)speed, steer, now);
} else {
/* Always send zero while disarmed to prevent ESC timeout */
motor_driver_update(&motors, 0, 0, now);
}
}
/* CRSF telemetry uplink — battery voltage + arm state at 1 Hz.
* Sends battery sensor frame (0x08) and flight mode frame (0x21)
* back to ELRS TX module so the pilot's handset OSD shows Vbat + state. */
if (now - crsf_telem_tick >= (1000u / CRSF_TELEMETRY_HZ)) {
crsf_telem_tick = now;
uint32_t vbat_mv = battery_read_mv();
/* Use coulomb-based SoC if available, fallback to voltage-based */
uint8_t soc_pct = battery_get_soc_coulomb();
if (soc_pct == 255) {
soc_pct = battery_estimate_pct(vbat_mv);
}
crsf_send_battery(vbat_mv, coulomb_counter_get_remaining_mah(), soc_pct);
crsf_send_flight_mode(bal.state == BALANCE_ARMED);
}
/* JLink STATUS telemetry at JLINK_TLM_HZ (50Hz) back to Jetson */
if (now - jlink_tlm_tick >= (1000u / JLINK_TLM_HZ)) {
jlink_tlm_tick = now;
jlink_tlm_status_t tlm;
tlm.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
tlm.roll_x10 = (int16_t)(imu.roll * 10.0f);
tlm.yaw_x10 = (int16_t)(imu.yaw * 10.0f);
tlm.motor_cmd = bal.motor_cmd;
uint32_t vbat = battery_read_mv();
tlm.vbat_mv = (vbat > 65535u) ? 65535u : (uint16_t)vbat;
tlm.rssi_dbm = crsf_state.rssi_dbm;
tlm.link_quality = crsf_state.link_quality;
tlm.balance_state = (uint8_t)bal.state;
tlm.rc_armed = crsf_state.armed ? 1u : 0u;
tlm.mode = (uint8_t)mode_manager_active(&mode);
EstopSource _es = safety_get_estop();
tlm.estop = (uint8_t)_es;
/* Use coulomb-based SoC if available, fallback to voltage-based */
uint8_t soc = battery_get_soc_coulomb();
tlm.soc_pct = (soc == 255) ? battery_estimate_pct(vbat) : soc;
tlm.fw_major = FW_MAJOR;
tlm.fw_minor = FW_MINOR;
tlm.fw_patch = FW_PATCH;
jlink_send_telemetry(&tlm);
}
/* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */
if (now - pm_tlm_tick >= (1000u / PM_TLM_HZ)) {
pm_tlm_tick = now;
jlink_tlm_power_t pow;
pow.power_state = (uint8_t)power_mgmt_state();
pow.est_total_ma = power_mgmt_current_ma();
pow.est_audio_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_AUDIO_MA);
pow.est_osd_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_OSD_MA);
pow.idle_ms = power_mgmt_idle_ms();
jlink_send_power_telemetry(&pow);
}
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
send_tick = now;
if (imu_ret == 0) {
float err = bal.setpoint - bal.pitch_deg;
/* Build JSON incrementally; append optional sensor fields */
char *p = buf;
int rem = (int)sizeof(buf);
int n;
n = snprintf(p, rem,
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d",
(int)(bal.pitch_deg * 10), /* pitch degrees x10 */
(int)(imu.roll * 10), /* roll degrees x10 */
(int)(err * 10), /* PID error x10 */
(int)(bal.integral * 10), /* integral x10 (windup monitor) */
(int)bal.motor_cmd, /* ESC command -1000..+1000 */
(int)bal.state,
(int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated) */
p += n; rem -= n;
n = snprintf(p, rem, ",\"md\":%d",
(int)mode_manager_active(&mode)); /* 0=MANUAL,1=ASSISTED,2=AUTO */
p += n; rem -= n;
if (mag_type != MAG_NONE) {
int16_t hd = mag_read_heading();
if (hd >= 0)
n = snprintf(p, rem, ",\"hd\":%d", hd); /* heading deg×10 */
else
n = snprintf(p, rem, ",\"hd\":-1"); /* not ready */
p += n; rem -= n;
}
if (baro_chip) {
int32_t pres_pa; int16_t temp_x10;
bmp280_read(&pres_pa, &temp_x10);
int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa);
/* alt cm, temp °C×10, pressure hPa×10 (Pa÷10 = hPa×10) */
n = snprintf(p, rem, ",\"alt\":%ld,\"t\":%d,\"pa\":%ld",
(long)alt_cm, (int)temp_x10, (long)(pres_pa / 10));
p += n; rem -= n;
if (baro_chip == 0x60) { /* BME280: add humidity %RH×10 */
int16_t hum_x10 = bmp280_read_humidity();
if (hum_x10 >= 0) {
n = snprintf(p, rem, ",\"h\":%d", (int)hum_x10);
p += n; rem -= n;
}
}
}
if (safety_rc_alive(now)) {
/* RSSI, link quality, and CH1–CH4 mapped to µs (1000–2000) */
n = snprintf(p, rem, ",\"rssi\":%d,\"lq\":%d"
",\"ch1\":%d,\"ch2\":%d,\"ch3\":%d,\"ch4\":%d",
(int)crsf_state.rssi_dbm, (int)crsf_state.link_quality,
(int)crsf_to_range(crsf_state.channels[0], 1000, 2000),
(int)crsf_to_range(crsf_state.channels[1], 1000, 2000),
(int)crsf_to_range(crsf_state.channels[2], 1000, 2000),
(int)crsf_to_range(crsf_state.channels[3], 1000, 2000));
p += n; rem -= n;
}
/* Jetson active flag, USB TX/RX packet counters */
int es; EstopSource _rs = safety_get_estop();
if (_rs >= ESTOP_REMOTE) es = (int)_rs;
else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT;
else es = ESTOP_CLEAR;
/* BNO055 calibration status and temperature when active */
if (bno055_active) {
uint8_t cs = bno055_calib_status();
n = snprintf(p, rem, ",\"bno_cs\":%d,\"bno_t\":%d",
(int)cs, (int)bno055_temperature());
p += n; rem -= n;
}
n = snprintf(p, rem, ",\"ja\":%d,\"jl\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
jetson_cmd_is_active(now) ? 1 : 0,
jlink_is_active(now) ? 1 : 0,
(unsigned)tx_count,
(unsigned)cdc_rx_count, es);
len = (int)(p + n - buf);
} else {
len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
}
CDC_Transmit((uint8_t *)buf, len);
tx_count++;
}
status_update(now, (imu_ret == 0),
(bal.state == BALANCE_ARMED),
(bal.state == BALANCE_TILT_FAULT),
safety_remote_estop_active());
HAL_Delay(1);
}
}
Reference in New Issue View Git Blame Copy Permalink