sl-firmware
fa75c442a7
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
59 lines
1.9 KiB
C
59 lines
1.9 KiB
C
#include "motor_driver.h"
|
|
#include "esc_backend.h"
|
|
#include "config.h"
|
|
#include <stdlib.h>
|
|
|
|
void motor_driver_init(motor_driver_t *m) {
|
|
m->steer_actual = 0;
|
|
m->estop = false;
|
|
}
|
|
|
|
void motor_driver_update(motor_driver_t *m,
|
|
int16_t balance_cmd,
|
|
int16_t steer_cmd,
|
|
uint32_t now) {
|
|
static uint32_t s_last_tick = 0;
|
|
|
|
/* Delta-time for ramp (cap at 100ms to handle first call / long gaps).
|
|
* Always update tick so ramp starts fresh when estop clears. */
|
|
int32_t dt_ms = (int32_t)(now - s_last_tick);
|
|
if (dt_ms > 100) dt_ms = 100;
|
|
s_last_tick = now;
|
|
|
|
/* Emergency stop: send zero, hold latch */
|
|
if (m->estop) {
|
|
esc_send(0, 0);
|
|
return;
|
|
}
|
|
|
|
/* Ramp steer toward target at MOTOR_STEER_RAMP_RATE counts/ms */
|
|
int32_t max_step = (int32_t)MOTOR_STEER_RAMP_RATE * dt_ms;
|
|
int32_t steer_error = (int32_t)steer_cmd - (int32_t)m->steer_actual;
|
|
if (steer_error > max_step) steer_error = max_step;
|
|
else if (steer_error < -max_step) steer_error = -max_step;
|
|
m->steer_actual = (int16_t)((int32_t)m->steer_actual + steer_error);
|
|
|
|
/* Headroom clamp: ensure |speed| + |steer| <= MOTOR_CMD_MAX
|
|
* Reduce steer (not balance) to preserve PID authority. */
|
|
int32_t speed = (int32_t)balance_cmd;
|
|
int32_t steer = (int32_t)m->steer_actual;
|
|
int32_t headroom = MOTOR_CMD_MAX - abs((int)speed);
|
|
if (headroom < 0) headroom = 0;
|
|
if (steer > headroom) steer = headroom;
|
|
if (steer < -headroom) steer = -headroom;
|
|
|
|
esc_send((int16_t)speed, (int16_t)steer);
|
|
}
|
|
|
|
void motor_driver_estop(motor_driver_t *m) {
|
|
m->estop = true;
|
|
m->steer_actual = 0;
|
|
/* Don't call hoverboard_send here — caller must drive sends via
|
|
* motor_driver_update() at the normal 50Hz ESC rate to avoid
|
|
* flooding the UART with 1kHz zero packets. */
|
|
}
|
|
|
|
void motor_driver_estop_clear(motor_driver_t *m) {
|
|
m->estop = false;
|
|
}
|