sl-controls
71b46f51d5
- Mamba (STM32): add ORIN_CAN_ID_PID_SET (0x305) handler in orin_can.c. Receives kp/ki/kd as uint16*100 (BE), applies to running balance loop, replies with FC_PID_ACK (0x405) echoing clamped gains. Gains persist in RAM until reboot; not saved to flash. - Jetson: expose pid/kp, pid/ki, pid/kd as ROS2 parameters in can_bridge_node. Parameter changes trigger encode_pid_set_cmd() and send CAN frame 0x305 immediately. ACK frame 0x405 logged at DEBUG. - mamba_protocol.py: add ORIN_CAN_ID_PID_SET / FC_PID_ACK IDs, PidGains dataclass, encode_pid_set_cmd(), decode_pid_ack(). Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
1072 lines
46 KiB
C
1072 lines
46 KiB
C
#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 "baro.h"
|
||
#include "mag.h"
|
||
#include "bno055.h"
|
||
#include "jetson_cmd.h"
|
||
#include "jetson_uart.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 "pid_flash.h"
|
||
#include "fault_handler.h"
|
||
#include "can_driver.h"
|
||
#include "vesc_can.h"
|
||
#include "orin_can.h"
|
||
#include "imu_cal_flash.h"
|
||
#include "hw_button.h"
|
||
#include "servo_bus.h"
|
||
#include "gimbal.h"
|
||
#include "lvc.h"
|
||
#include "uart_protocol.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;
|
||
extern volatile uint8_t cdc_imu_cal_request; /* set by O command — mount cal (Issue #680) */
|
||
|
||
/* Direct motor test (set by W command in jetson_uart.c) */
|
||
volatile int16_t direct_test_speed = 0;
|
||
volatile int16_t direct_test_steer = 0;
|
||
|
||
/* 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();
|
||
|
||
/* Fault recovery handler (Issue #565) — must be first, before safety_init() */
|
||
fault_handler_init();
|
||
|
||
/* 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();
|
||
|
||
/* Load IMU mount angle offsets from flash if previously calibrated (Issue #680) */
|
||
{
|
||
float imu_pitch_off = 0.0f, imu_roll_off = 0.0f;
|
||
if (imu_cal_flash_load(&imu_pitch_off, &imu_roll_off)) {
|
||
mpu6000_set_mount_offset(imu_pitch_off, imu_roll_off);
|
||
printf("[IMU_CAL] Mount offsets loaded: pitch=%.1f° roll=%.1f°\n",
|
||
(double)imu_pitch_off, (double)imu_roll_off);
|
||
}
|
||
}
|
||
|
||
/* Init hoverboard ESC UART */
|
||
hoverboard_init();
|
||
|
||
/* Init balance controller */
|
||
balance_t bal;
|
||
balance_init(&bal);
|
||
|
||
/* Load PID from flash if previously auto-tuned (Issue #531) */
|
||
{
|
||
float flash_kp, flash_ki, flash_kd;
|
||
if (pid_flash_load(&flash_kp, &flash_ki, &flash_kd)) {
|
||
bal.kp = flash_kp;
|
||
bal.ki = flash_ki;
|
||
bal.kd = flash_kd;
|
||
printf("[PID] Loaded from flash: Kp=%.3f Ki=%.3f Kd=%.3f\n",
|
||
(double)flash_kp, (double)flash_ki, (double)flash_kd);
|
||
}
|
||
}
|
||
|
||
/* 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 CAN1 bus — PB8/PB9, 500 kbps (Issues #597/#676/#674).
|
||
* Register callbacks BEFORE can_driver_init() so both are ready when
|
||
* filter banks activate. */
|
||
can_driver_set_ext_cb(vesc_can_on_frame); /* VESC STATUS → FIFO1 */
|
||
can_driver_set_std_cb(orin_can_on_frame); /* Orin cmds → FIFO0 */
|
||
can_driver_init();
|
||
vesc_can_init(VESC_CAN_ID_LEFT, VESC_CAN_ID_RIGHT);
|
||
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) */
|
||
if (fault_get_last_type() != FAULT_NONE) {
|
||
fault_log_entry_t fle;
|
||
memset(&fle, 0, sizeof(fle));
|
||
jlink_tlm_fault_log_t ftlm;
|
||
memset(&ftlm, 0, sizeof(ftlm));
|
||
ftlm.entry_count = fault_log_get_count();
|
||
if (fault_log_read(0u, &fle)) {
|
||
ftlm.fault_type = fle.fault_type;
|
||
ftlm.reset_count = fle.reset_count;
|
||
ftlm.timestamp_ms = fle.timestamp_ms;
|
||
ftlm.pc = fle.pc;
|
||
ftlm.lr = fle.lr;
|
||
ftlm.cfsr = fle.cfsr;
|
||
ftlm.hfsr = fle.hfsr;
|
||
}
|
||
jlink_send_fault_log(&ftlm);
|
||
printf("[FAULT] Prior fault type=%u count=%u PC=0x%08lX\n",
|
||
(unsigned)ftlm.fault_type, (unsigned)ftlm.entry_count,
|
||
(unsigned long)ftlm.pc);
|
||
}
|
||
|
||
/* Init Jetson UART command interface on USART6 (PC6/PC7) at 921600 baud.
|
||
* Mirrors CDC command protocol over hardware UART (fixes USB CDC TX bug). */
|
||
jetson_uart_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 ST3215 serial bus servo driver (USART3 half-duplex PB10, Issue #547) */
|
||
servo_bus_init();
|
||
|
||
/* Init pan/tilt gimbal (ST3215 servos, centers both axes on boot) */
|
||
gimbal_t gimbal;
|
||
gimbal_init(&gimbal);
|
||
|
||
/* 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();
|
||
|
||
/* Init LVC: low voltage cutoff state machine (Issue #613) */
|
||
lvc_init();
|
||
|
||
/* Init UART command protocol for Jetson (UART5 PC12/PD2, 115200 baud, Issue #629) */
|
||
uart_protocol_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;
|
||
baro_init(baro_chip); /* Issue #672: 1 Hz baro read + JLink telemetry */
|
||
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 */
|
||
uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */
|
||
uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */
|
||
uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */
|
||
uint32_t uart_status_tick = 0; /* UART protocol STATUS frame timer (Issue #629) */
|
||
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);
|
||
|
||
/* Orin LED override (Issue #685): apply pattern from CAN 0x304 if updated.
|
||
* Safety states (estop, tilt fault) are applied later and always win. */
|
||
if (orin_can_led_updated) {
|
||
orin_can_led_updated = 0u;
|
||
/* pattern: 0=auto, 1=solid-green, 2=slow-blink, 3=fast-blink, 4=pulse */
|
||
switch (orin_can_led_override.pattern) {
|
||
case 1u: led_set_state(LED_STATE_ARMED); break;
|
||
case 2u: led_set_state(LED_STATE_LOW_BATT); break; /* slow blink */
|
||
case 3u: led_set_state(LED_STATE_ERROR); break; /* fast blink */
|
||
case 4u: led_set_state(LED_STATE_CHARGING); break; /* pulse */
|
||
default: break; /* 0=auto — let state machine below decide */
|
||
}
|
||
}
|
||
|
||
/* Advance LED animation sequencer (non-blocking, call every tick) */
|
||
led_tick(now);
|
||
|
||
/* Fault recovery LED blink code (Issue #565; self-disables after 10 s) */
|
||
fault_led_tick(now);
|
||
|
||
/* Servo pan-tilt animation tick — updates smooth sweeps */
|
||
servo_tick(now);
|
||
|
||
/* Gimbal ST3215 feedback polling tick (self-throttles to GIMBAL_TLM_HZ) */
|
||
gimbal_tick(&gimbal, now);
|
||
|
||
/* Accumulate coulombs for battery state-of-charge estimation (Issue #325) */
|
||
battery_accumulate_coulombs();
|
||
|
||
/* LVC: update low-voltage protection state machine (Issue #613) */
|
||
lvc_tick(now, battery_read_mv());
|
||
|
||
/* Baro: 1 Hz BME280 read + JLink telemetry (Issue #672) */
|
||
baro_tick(now);
|
||
|
||
/* 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();
|
||
|
||
/* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
|
||
can_driver_process();
|
||
|
||
/* UART command protocol RX: parse Jetson frames (Issue #629) */
|
||
uart_protocol_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) < 20.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();
|
||
}
|
||
/* GIMBAL_POS: forward new pan/tilt command to gimbal (Issue #547) */
|
||
if (jlink_state.gimbal_updated) {
|
||
jlink_state.gimbal_updated = 0u;
|
||
gimbal_set_pos(&gimbal,
|
||
(int16_t)jlink_state.gimbal_pan_x10,
|
||
(int16_t)jlink_state.gimbal_tilt_x10,
|
||
jlink_state.gimbal_speed);
|
||
}
|
||
|
||
/* PID_SAVE: persist current gains to flash, reply with result (Issue #531).
|
||
* Only allowed while disarmed -- flash sector erase stalls CPU for ~1 s. */
|
||
if (jlink_state.pid_save_req && bal.state != BALANCE_ARMED) {
|
||
jlink_state.pid_save_req = 0u;
|
||
bool save_ok = pid_flash_save(bal.kp, bal.ki, bal.kd);
|
||
jlink_tlm_pid_result_t pid_res;
|
||
pid_res.kp = bal.kp;
|
||
pid_res.ki = bal.ki;
|
||
pid_res.kd = bal.kd;
|
||
pid_res.saved_ok = save_ok ? 1u : 0u;
|
||
jlink_send_pid_result(&pid_res);
|
||
printf("[PID] Flash save %s: Kp=%.3f Ki=%.3f Kd=%.3f\n",
|
||
save_ok ? "OK" : "FAIL",
|
||
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
||
}
|
||
|
||
/* UART protocol: handle commands from Jetson (Issue #629) */
|
||
{
|
||
if (uart_prot_state.vel_updated) {
|
||
uart_prot_state.vel_updated = 0u;
|
||
if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
|
||
can_cmd_t ucmd_l = { uart_prot_state.left_rpm, 0 };
|
||
can_cmd_t ucmd_r = { uart_prot_state.right_rpm, 0 };
|
||
can_driver_send_cmd(CAN_NODE_LEFT, &ucmd_l);
|
||
can_driver_send_cmd(CAN_NODE_RIGHT, &ucmd_r);
|
||
}
|
||
}
|
||
if (uart_prot_state.pid_updated) {
|
||
uart_prot_state.pid_updated = 0u;
|
||
bal.kp = uart_prot_state.pid_kp;
|
||
bal.ki = uart_prot_state.pid_ki;
|
||
bal.kd = uart_prot_state.pid_kd;
|
||
}
|
||
if (uart_prot_state.estop_req) {
|
||
uart_prot_state.estop_req = 0u;
|
||
safety_remote_estop(ESTOP_REMOTE);
|
||
safety_arm_cancel();
|
||
balance_disarm(&bal);
|
||
motor_driver_estop(&motors);
|
||
}
|
||
if (uart_prot_state.estop_clear_req) {
|
||
uart_prot_state.estop_clear_req = 0u;
|
||
if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
|
||
safety_remote_estop_clear();
|
||
}
|
||
}
|
||
|
||
/* Orin CAN: handle commands from Orin over CAN bus (Issue #674).
|
||
* Estop and clear are latching; drive/mode are consumed each tick.
|
||
* Balance independence: if orin_can_is_alive() == false the loop
|
||
* simply does not inject any commands and holds upright in-place. */
|
||
if (orin_can_state.estop_req) {
|
||
orin_can_state.estop_req = 0u;
|
||
safety_remote_estop(ESTOP_REMOTE);
|
||
safety_arm_cancel();
|
||
balance_disarm(&bal);
|
||
motor_driver_estop(&motors);
|
||
}
|
||
if (orin_can_state.estop_clear_req) {
|
||
orin_can_state.estop_clear_req = 0u;
|
||
if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
|
||
safety_remote_estop_clear();
|
||
}
|
||
/* PID_SET (Issue #693): apply new gains from Orin immediately.
|
||
* Gains persist in RAM until reboot; not saved to flash. */
|
||
if (orin_can_state.pid_updated) {
|
||
orin_can_state.pid_updated = 0u;
|
||
float new_kp = (float)orin_can_state.pid_kp_x100 / 100.0f;
|
||
float new_ki = (float)orin_can_state.pid_ki_x100 / 100.0f;
|
||
float new_kd = (float)orin_can_state.pid_kd_x100 / 100.0f;
|
||
if (new_kp > 500.0f) new_kp = 500.0f;
|
||
if (new_ki > 50.0f) new_ki = 50.0f;
|
||
if (new_kd > 50.0f) new_kd = 50.0f;
|
||
bal.kp = new_kp;
|
||
bal.ki = new_ki;
|
||
bal.kd = new_kd;
|
||
orin_can_send_pid_ack(bal.kp, bal.ki, bal.kd);
|
||
printf("[PID] Orin set Kp=%.3f Ki=%.3f Kd=%.3f\n",
|
||
(double)bal.kp, (double)bal.ki, (double)bal.kd);
|
||
}
|
||
|
||
/* 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) */
|
||
if (jlink_state.fault_log_req) {
|
||
jlink_state.fault_log_req = 0u;
|
||
fault_log_entry_t fle;
|
||
memset(&fle, 0, sizeof(fle));
|
||
jlink_tlm_fault_log_t ftlm;
|
||
memset(&ftlm, 0, sizeof(ftlm));
|
||
ftlm.entry_count = fault_log_get_count();
|
||
if (fault_log_read(0u, &fle)) {
|
||
ftlm.fault_type = fle.fault_type;
|
||
ftlm.reset_count = fle.reset_count;
|
||
ftlm.timestamp_ms = fle.timestamp_ms;
|
||
ftlm.pc = fle.pc;
|
||
ftlm.lr = fle.lr;
|
||
ftlm.cfsr = fle.cfsr;
|
||
ftlm.hfsr = fle.hfsr;
|
||
}
|
||
jlink_send_fault_log(&ftlm);
|
||
}
|
||
|
||
/* CAN_STATS: send telemetry on demand or at CAN_TLM_HZ (1 Hz) (Issue #597) */
|
||
{
|
||
bool send_can = (jlink_state.can_stats_req != 0u);
|
||
if (!send_can && (now - can_tlm_tick >= (1000u / CAN_TLM_HZ))) {
|
||
send_can = true;
|
||
}
|
||
if (send_can) {
|
||
jlink_state.can_stats_req = 0u;
|
||
can_tlm_tick = now;
|
||
can_stats_t cs;
|
||
can_driver_get_stats(&cs);
|
||
can_feedback_t fb0, fb1;
|
||
jlink_tlm_can_stats_t ctlm;
|
||
memset(&ctlm, 0, sizeof(ctlm));
|
||
ctlm.tx_count = cs.tx_count;
|
||
ctlm.rx_count = cs.rx_count;
|
||
ctlm.err_count = cs.err_count;
|
||
ctlm.bus_off = cs.bus_off;
|
||
if (can_driver_get_feedback(CAN_NODE_LEFT, &fb0)) {
|
||
ctlm.vel0_rpm = fb0.velocity_rpm;
|
||
ctlm.node_faults |= (fb0.fault ? 1u : 0u);
|
||
}
|
||
if (can_driver_get_feedback(CAN_NODE_RIGHT, &fb1)) {
|
||
ctlm.vel1_rpm = fb1.velocity_rpm;
|
||
ctlm.node_faults |= (fb1.fault ? 2u : 0u);
|
||
}
|
||
jlink_send_can_stats(&ctlm);
|
||
}
|
||
}
|
||
|
||
/* 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: emergency stop if RC is alive and CH5 off.
|
||
* Issue #512: RC becomes optional override — kill switch triggers estop,
|
||
* not disarm, so Jetson-armed robots remain armed when RC disconnects.
|
||
* Emergency stop kills motors immediately but allows re-arm. */
|
||
if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
|
||
motor_driver_estop(&motors);
|
||
}
|
||
/* 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 with hold interlock.
|
||
* Issue #512: RC becomes optional override. Falling edge only disarms if RC
|
||
* explicitly requested it (CH5 off while RC alive). RC disconnect doesn't
|
||
* disarm Jetson-controlled robots; Jetson timeout disarm (in main loop) handles it. */
|
||
{
|
||
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) < 20.0f) {
|
||
safety_arm_start(now);
|
||
}
|
||
}
|
||
if (!rc_armed_now && s_rc_armed_prev && safety_rc_alive(now)) {
|
||
/* Falling edge with RC still alive: RC explicitly de-armed.
|
||
* Cancel pending arm or disarm if already armed. */
|
||
safety_arm_cancel();
|
||
if (bal.state == BALANCE_ARMED) balance_disarm(&bal);
|
||
}
|
||
/* Note: RC disconnect (crsf_state.last_rx_ms == 0 after being alive) is handled
|
||
* by failsafe timer below, NOT by this edge detector. */
|
||
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) < 20.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();
|
||
}
|
||
|
||
/* IMU mount angle calibration (Issue #680): capture current pitch/roll as
|
||
* mount offsets and save to flash. Disarmed only — flash erase stalls ~1s. */
|
||
if (cdc_imu_cal_request && bal.state != BALANCE_ARMED) {
|
||
cdc_imu_cal_request = 0;
|
||
float off_p = bal.pitch_deg;
|
||
float off_r = imu.roll;
|
||
mpu6000_set_mount_offset(off_p, off_r);
|
||
bool cal_ok = imu_cal_flash_save(off_p, off_r);
|
||
char reply[64];
|
||
snprintf(reply, sizeof(reply),
|
||
"{\"imu_cal\":%s,\"pitch_off\":%d,\"roll_off\":%d}\n",
|
||
cal_ok ? "ok" : "fail",
|
||
(int)(off_p * 10), (int)(off_r * 10));
|
||
CDC_Transmit((uint8_t *)reply, strlen(reply));
|
||
printf("[IMU_CAL] Mount offset saved: pitch=%.1f° roll=%.1f° (%s)\n",
|
||
(double)off_p, (double)off_r, cal_ok ? "OK" : "FAIL");
|
||
}
|
||
|
||
/* 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 (orin_can_is_alive(now))
|
||
bal.setpoint += ((float)orin_can_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);
|
||
}
|
||
|
||
/* LVC cutoff: disarm and estop on undervoltage latch (Issue #613) */
|
||
if (lvc_is_cutoff() && bal.state == BALANCE_ARMED) {
|
||
safety_arm_cancel();
|
||
balance_disarm(&bal);
|
||
motor_driver_estop(&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 (orin_can_is_alive(now))
|
||
mode_manager_set_auto_cmd(&mode, orin_can_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;
|
||
/* LVC power scaling: 100% normal, 50% critical, 0% cutoff (Issue #613) */
|
||
uint8_t lvc_scale = lvc_get_power_scale();
|
||
if (lvc_scale < 100u) {
|
||
speed = (speed * (int32_t)lvc_scale) / 100;
|
||
}
|
||
if (speed > 1000) speed = 1000;
|
||
if (speed < -1000) speed = -1000;
|
||
motor_driver_update(&motors, (int16_t)speed, steer, now);
|
||
} else if (direct_test_speed != 0 || direct_test_steer != 0) {
|
||
/* Direct motor test mode — set by W command */
|
||
esc_send(direct_test_speed, direct_test_steer);
|
||
} else {
|
||
/* Always send zero while disarmed to prevent ESC timeout */
|
||
motor_driver_update(&motors, 0, 0, now);
|
||
}
|
||
}
|
||
|
||
/* VESC: send RPM commands at CAN_TX_RATE_HZ (100 Hz) via 29-bit extended CAN.
|
||
* VESC does not need enable/disable frames — RPM=0 while disarmed holds brakes.
|
||
* Left wheel: speed_rpm + steer_rpm (differential drive)
|
||
* Right wheel: speed_rpm − steer_rpm (Issue #674) */
|
||
if (now - can_cmd_tick >= (1000u / CAN_TX_RATE_HZ)) {
|
||
can_cmd_tick = now;
|
||
int32_t speed_rpm = 0;
|
||
int32_t steer_rpm = 0;
|
||
if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) {
|
||
int16_t can_steer = mode_manager_get_steer(&mode);
|
||
speed_rpm = (int32_t)bal.motor_cmd * CAN_RPM_SCALE;
|
||
steer_rpm = (int32_t)can_steer * CAN_RPM_SCALE / 2;
|
||
}
|
||
vesc_can_send_rpm(VESC_CAN_ID_LEFT, speed_rpm + steer_rpm);
|
||
vesc_can_send_rpm(VESC_CAN_ID_RIGHT, speed_rpm - steer_rpm);
|
||
}
|
||
|
||
/* VESC telemetry: send JLINK_TLM_VESC_STATE at 1 Hz (Issue #674) */
|
||
vesc_can_send_tlm(now);
|
||
|
||
/* Orin CAN broadcast: FC_STATUS + FC_VESC at ORIN_TLM_HZ (10 Hz) (Issue #674) */
|
||
{
|
||
orin_can_fc_status_t fc_st;
|
||
fc_st.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||
fc_st.motor_cmd = bal.motor_cmd;
|
||
uint32_t _vbat = battery_read_mv();
|
||
fc_st.vbat_mv = (_vbat > 65535u) ? 65535u : (uint16_t)_vbat;
|
||
fc_st.balance_state = (uint8_t)bal.state;
|
||
fc_st.flags = (safety_remote_estop_active() ? 0x01u : 0u) |
|
||
(bal.state == BALANCE_ARMED ? 0x02u : 0u);
|
||
|
||
orin_can_fc_vesc_t fc_vesc;
|
||
vesc_state_t vl, vr;
|
||
bool vl_ok = vesc_can_get_state(VESC_CAN_ID_LEFT, &vl);
|
||
bool vr_ok = vesc_can_get_state(VESC_CAN_ID_RIGHT, &vr);
|
||
fc_vesc.left_rpm_x10 = vl_ok ? (int16_t)(vl.rpm / 10) : 0;
|
||
fc_vesc.right_rpm_x10 = vr_ok ? (int16_t)(vr.rpm / 10) : 0;
|
||
fc_vesc.left_current_x10 = vl_ok ? vl.current_x10 : 0;
|
||
fc_vesc.right_current_x10 = vr_ok ? vr.current_x10 : 0;
|
||
|
||
orin_can_broadcast(now, &fc_st, &fc_vesc);
|
||
}
|
||
|
||
/* FC_IMU (0x402): full IMU angles + calibration status at 50 Hz (Issue #680) */
|
||
{
|
||
orin_can_fc_imu_t fc_imu;
|
||
fc_imu.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||
fc_imu.roll_x10 = (int16_t)(imu.roll * 10.0f);
|
||
fc_imu.yaw_x10 = (int16_t)(imu.yaw * 10.0f);
|
||
/* cal_status: 0=uncal, 1=gyro_cal, 2=gyro+mount_cal */
|
||
if (!mpu6000_is_calibrated()) fc_imu.cal_status = 0u;
|
||
else if (mpu6000_has_mount_offset()) fc_imu.cal_status = 2u;
|
||
else fc_imu.cal_status = 1u;
|
||
fc_imu.balance_state = (uint8_t)bal.state;
|
||
orin_can_broadcast_imu(now, &fc_imu);
|
||
}
|
||
|
||
/* FC_BARO (0x403): barometer at 1 Hz (Issue #672) */
|
||
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);
|
||
orin_can_fc_baro_t fc_baro;
|
||
fc_baro.pressure_pa = _pres_pa;
|
||
fc_baro.temp_x10 = _temp_x10;
|
||
fc_baro.alt_cm = (_alt_cm > 32767) ? 32767 :
|
||
(_alt_cm < -32768) ? -32768 : (int16_t)_alt_cm;
|
||
orin_can_broadcast_baro(now, &fc_baro);
|
||
}
|
||
|
||
/* 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);
|
||
|
||
/* Send gimbal state at same 50 Hz cadence (Issue #547) */
|
||
jlink_tlm_gimbal_state_t gtlm;
|
||
gtlm.pan_x10 = gimbal.fb_pan_x10;
|
||
gtlm.tilt_x10 = gimbal.fb_tilt_x10;
|
||
gtlm.pan_speed_raw = gimbal.fb_pan_speed;
|
||
gtlm.tilt_speed_raw = gimbal.fb_tilt_speed;
|
||
gtlm.torque_en = gimbal.torque_enabled ? 1u : 0u;
|
||
uint32_t rx_total = gimbal.rx_ok + gimbal.rx_err;
|
||
gtlm.rx_err_pct = (rx_total > 0u)
|
||
? (uint8_t)(gimbal.rx_err * 100u / rx_total)
|
||
: 0u;
|
||
jlink_send_gimbal_state(>lm);
|
||
}
|
||
|
||
/* 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);
|
||
}
|
||
|
||
/* JLINK_TLM_LVC telemetry at LVC_TLM_HZ (1 Hz) -- battery voltage + protection state (Issue #613) */
|
||
if (now - lvc_tlm_tick >= (1000u / LVC_TLM_HZ)) {
|
||
lvc_tlm_tick = now;
|
||
uint32_t lvc_vbat = battery_read_mv();
|
||
jlink_tlm_lvc_t ltlm;
|
||
ltlm.voltage_mv = (lvc_vbat > 65535u) ? 65535u : (uint16_t)lvc_vbat;
|
||
ltlm.protection_state = (uint8_t)lvc_get_state();
|
||
if (lvc_vbat == 0u) {
|
||
ltlm.percent = 255u;
|
||
} else if (lvc_vbat <= LVC_CUTOFF_MV) {
|
||
ltlm.percent = 0u;
|
||
} else if (lvc_vbat >= LVC_WARNING_MV) {
|
||
ltlm.percent = 100u;
|
||
} else {
|
||
ltlm.percent = (uint8_t)(((lvc_vbat - LVC_CUTOFF_MV) * 100u) /
|
||
(LVC_WARNING_MV - LVC_CUTOFF_MV));
|
||
}
|
||
jlink_send_lvc_tlm(<lm);
|
||
}
|
||
|
||
/* UART protocol: send STATUS to Jetson at 10 Hz (Issue #629) */
|
||
if (now - uart_status_tick >= 100u) {
|
||
uart_status_tick = now;
|
||
uart_prot_status_t ups;
|
||
ups.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f);
|
||
ups.motor_cmd = bal.motor_cmd;
|
||
uint32_t _uv = battery_read_mv();
|
||
ups.vbat_mv = (_uv > 65535u) ? 65535u : (uint16_t)_uv;
|
||
ups.balance_state = (uint8_t)bal.state;
|
||
ups.estop_active = safety_remote_estop_active() ? 1u : 0u;
|
||
uart_protocol_send_status(&ups);
|
||
}
|
||
|
||
/* 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);
|
||
}
|
||
}
|