Compare commits

...

6 Commits

Author SHA1 Message Date
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
93756f5248 feat: Add FC↔Orin UART verification (Issue #362)
Implements UART bridge verification between Flight Controller (STM32F722)
and Jetson Orin.

Changes:
1. jetson/scripts/uart_test.py (12.7 KB)
   - Opens /dev/ttyTHS1 at 921600 baud
   - Sends jlink binary test frames (PING, VERSION, ECHO)
   - Verifies CRC16-CCITT frame integrity
   - Logs transactions with timestamps
   - JSON result export and optional MQTT publishing

2. jetson/ros2_ws/src/saltybot_bridge/launch/uart_bridge.launch.py
   - ROS2 launch file for serial_bridge_node on UART port
   - Configurable port (default /dev/ttyTHS1), baud rate (921600)
   - Bridges FC telemetry to /saltybot/imu, /saltybot/balance_state
   - Publishes diagnostics to /diagnostics

Usage:
  Test: sudo python3 jetson/scripts/uart_test.py
  Launch: ros2 launch saltybot_bridge uart_bridge.launch.py

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-04 12:17:56 -05:00
3c93a72d01 Merge pull request 'refactor: ESC abstraction layer with pluggable backends (Issue #388)' (#389) from sl-firmware/issue-388-esc-abstraction into main 2026-03-04 11:36:24 -05:00
844504e92e refactor: ESC abstraction layer with pluggable backends (Issue #388)
BREAKING CHANGE: Hoverboard implementation moved to pluggable vtable architecture.

## Implementation

### New Files
- include/esc_backend.h: Abstract interface (vtable) with:
  - esc_telemetry_t struct (voltage, current, temp, speed, steer, fault)
  - esc_backend_t vtable (init, send, estop, resume, get_telemetry)
  - Runtime registration (esc_backend_register/get)
  - Convenience wrappers (esc_init, esc_send, esc_estop, etc)

- src/esc_backend.c: Backend registry and wrapper implementations

- src/esc_hoverboard.c: Hoverboard backend implementing vtable
  - USART2 @ 115200 baud configuration
  - EFeru FOC packet encoding (0xABCD start, XOR checksum)
  - Backward-compatible hoverboard_init/send wrappers
  - Telemetry stub (future: add RX feedback parsing)

- src/esc_vesc.c: VESC backend stub (filled by Issue #383)
  - Placeholder functions for FSESC 4.20 Plus integration
  - Public vesc_backend_register_impl() for runtime registration
  - Ready for pyvesc protocol implementation

### Modified Files
- src/motor_driver.c: Changed from direct hoverboard_send() calls to esc_send()
  - No logic changes, ESC-agnostic via vtable

- include/config.h: Added ESC_BACKEND define
  - Compile-time selection (default: HOVERBOARD)
  - Comments document architecture for future VESC support

### Removed Files
- src/hoverboard.c: Original implementation merged into esc_hoverboard.c

## Architecture Benefits
1. **Backend Pluggability**: Support multiple ESC types without code duplication
2. **Zero Direct Dependencies**: motor_driver.c never calls hoverboard functions directly
3. **Clean Testing**: Each backend can be tested/stubbed independently
4. **Future-Ready**: VESC integration (Issue #383) just implements the vtable
5. **Backward Compatible**: Existing code calling hoverboard_init/send still works

## Testing
- pio run:  PASS (55.4KB Flash, 16.9KB RAM)
- Hoverboard backend tested via existing balance tests (unchanged logic)
- VESC backend stub compiles and links (no-op until #383 fills implementation)

## Blocks
- Issue #383 (VESC integration) — ready to implement vtable functions
- Issue #384 (pan/tilt servo) — may use independent PWM (not blocked)

## Dependencies
- None — this is pure refactoring, no API changes for callers

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-04 10:36:35 -05:00
985e03a26d Merge pull request 'fix: add missing bno055.h include in main.c' (#387) from sl-firmware/fix-bno055-include into main 2026-03-04 09:54:56 -05:00
d52e7af554 fix: Add missing bno055.h include to resolve implicit declaration warnings
Adds #include "bno055.h" to src/main.c to resolve implicit declaration
warnings for bno055_read(), bno055_calib_status(), and bno055_temperature().
Functions were properly implemented but header was missing from includes.

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-04 08:45:51 -05:00
10 changed files with 856 additions and 44 deletions

View File

@ -157,6 +157,12 @@
// Jetson: USART6 (PC6=TX, PC7=RX)
// Debug: UART5 (PC12=TX, PD2=RX)
// --- ESC Backend Selection (Issue #388) ---
// Pluggable ESC abstraction layer — supports multiple backends:
// HOVERBOARD: EFeru FOC (USART2 @ 115200) — current default
// VESC: FSESC 4.20 Plus (USART6 @ 921600, balance mode) — future
#define ESC_BACKEND HOVERBOARD /* HOVERBOARD or VESC */
// --- CRSF / ExpressLRS ---
// CH1[0]=steer CH2[1]=throttle CH5[4]=arm CH6[5]=mode
#define CRSF_ARM_THRESHOLD 1750 /* CH5 raw value; > threshold = armed */

79
include/esc_backend.h Normal file
View File

@ -0,0 +1,79 @@
#ifndef ESC_BACKEND_H
#define ESC_BACKEND_H
#include <stdint.h>
#include <stdbool.h>
/*
* ESC Backend Abstraction Layer
*
* Provides a pluggable interface for different ESC implementations:
* - Hoverboard (EFeru FOC firmware, UART @ 115200)
* - VESC (via UART @ 921600, with balance mode) future
*
* Allows motor_driver.c to remain ESC-agnostic. Backend selection
* via ESC_BACKEND compile-time define in config.h.
*
* Issue #388: ESC abstraction layer
* Blocks Issue #383: VESC integration
*/
/* Telemetry snapshot from ESC (polled on-demand) */
typedef struct {
int16_t speed; /* Motor speed (PWM duty or RPM, backend-dependent) */
int16_t steer; /* Steering position (0 = centered) */
uint16_t voltage_mv; /* Battery voltage in millivolts */
int16_t current_ma; /* Motor current in milliamps (signed: discharge/charge) */
int16_t temperature_c; /* ESC temperature in °C */
uint16_t fault; /* Fault code (backend-specific) */
} esc_telemetry_t;
/* Virtual function table for ESC backends */
typedef struct {
/* Initialize ESC hardware and UART (called once at startup) */
void (*init)(void);
/* Send motor command to ESC (called at ~50Hz from motor_driver_update)
* speed: -1000..+1000 (forward/reverse)
* steer: -1000..+1000 (left/right)
*/
void (*send)(int16_t speed, int16_t steer);
/* Emergency stop: send zero and disable output
* (called from safety or mode manager)
*/
void (*estop)(void);
/* Query current ESC state
* Returns latest telemetry snapshot (may be cached/stale on some backends).
* Safe to call from any context (non-blocking).
*/
void (*get_telemetry)(esc_telemetry_t *out);
/* Optional: resume from estop (not all backends use this) */
void (*resume)(void);
} esc_backend_t;
/*
* Register a backend implementation at runtime.
* Typically called during init sequence before motor_driver_init().
*/
void esc_backend_register(const esc_backend_t *backend);
/*
* Get the currently active backend.
* Returns pointer to vtable; nullptr if no backend registered.
*/
const esc_backend_t *esc_backend_get(void);
/*
* High-level convenience wrappers (match motor_driver.c interface).
* These call through the active backend if registered.
*/
void esc_init(void);
void esc_send(int16_t speed, int16_t steer);
void esc_estop(void);
void esc_resume(void);
void esc_get_telemetry(esc_telemetry_t *out);
#endif /* ESC_BACKEND_H */

View File

@ -0,0 +1,83 @@
"""
uart_bridge.launch.py FCOrin UART bridge (Issue #362)
Launches serial_bridge_node configured for Jetson Orin UART port.
Bridges Flight Controller (STM32F722) telemetry from /dev/ttyTHS1 into ROS2.
Published topics (same as USB CDC bridge):
/saltybot/imu sensor_msgs/Imu pitch/roll/yaw as angular velocity
/saltybot/balance_state std_msgs/String (JSON) full PID diagnostics
/diagnostics diagnostic_msgs/DiagnosticArray system health
Usage:
ros2 launch saltybot_bridge uart_bridge.launch.py
# Override UART port (Orin has THS0THS3):
ros2 launch saltybot_bridge uart_bridge.launch.py uart_port:=/dev/ttyTHS0
# Override baud rate:
ros2 launch saltybot_bridge uart_bridge.launch.py baud_rate:=115200
Prerequisites:
- Flight Controller connected to /dev/ttyTHS1 @ 921600 baud
- STM32 firmware transmitting JSON telemetry frames (50 Hz)
- ROS2 environment sourced (source install/setup.bash)
Note:
/dev/ttyTHS1 is the native UART1 on Jetson Orin. Verify connectivity:
$ cat /dev/ttyTHS1 | head -5
{"p":123,"r":-45,"e":0,"ig":0,"m":0,"s":1,"y":0}
...
Troubleshooting:
- "Permission denied" run with sudo or add user to dialout group
- No frames received check Flight Controller baud rate, verify UART cable
- Garbled output check baud rate mismatch, check cable shield/termination
"""
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
# ── Launch arguments ───────────────────────────────────────────────────
DeclareLaunchArgument(
"uart_port",
default_value="/dev/ttyTHS1",
description="Jetson Orin UART device (THS0THS3)",
),
DeclareLaunchArgument(
"baud_rate",
default_value="921600",
description="Serial baud rate (Flight Controller standard)",
),
DeclareLaunchArgument(
"timeout",
default_value="0.1",
description="Serial read timeout in seconds",
),
DeclareLaunchArgument(
"reconnect_delay",
default_value="2.0",
description="Delay before reconnect attempt on serial error (seconds)",
),
# ── Serial bridge node ─────────────────────────────────────────────────
Node(
package="saltybot_bridge",
executable="serial_bridge_node",
name="fc_uart_bridge",
output="screen",
parameters=[
{
"serial_port": LaunchConfiguration("uart_port"),
"baud_rate": LaunchConfiguration("baud_rate"),
"timeout": LaunchConfiguration("timeout"),
"reconnect_delay": LaunchConfiguration("reconnect_delay"),
},
],
),
])

417
jetson/scripts/uart_test.py Executable file
View File

@ -0,0 +1,417 @@
#!/usr/bin/env python3
"""
uart_test.py FCOrin UART verification (Issue #362)
Opens /dev/ttyTHS1 at 921600 baud, sends jlink test frames, logs responses.
Jlink frame format (binary):
[0xAA] [0x55] [len] [cmd] [payload...] [crc16]
cmd = 0x01: PING request
cmd = 0x02: VERSION request
cmd = 0x03: ECHO request (with payload)
Test procedure:
1. Open serial port with 921600 baud
2. Send PING frame expect PONG response
3. Send VERSION frame expect version string
4. Send ECHO frame with test data expect echo back
5. Log all transactions with timestamps
6. Report results to MQTT (max topic)
Usage:
sudo python3 uart_test.py
# Override port/baud:
python3 uart_test.py --port /dev/ttyTHS0 --baud 115200
# MQTT report (requires paho-mqtt):
python3 uart_test.py --mqtt-host localhost --mqtt-topic saltybot/uart_test
Prerequisites:
- pyserial: pip install pyserial
- paho-mqtt (optional): pip install paho-mqtt
- Orin UART device configured and Flight Controller connected to /dev/ttyTHS1
"""
import argparse
import json
import logging
import serial
import sys
import time
from datetime import datetime
from typing import Optional, Tuple
# Configure logging
logging.basicConfig(
level=logging.INFO,
format="[%(asctime)s] %(levelname)s: %(message)s",
datefmt="%Y-%m-%d %H:%M:%S",
)
logger = logging.getLogger(__name__)
class JlinkFrame:
"""Jlink binary frame codec."""
HEADER = (0xAA, 0x55)
CMD_PING = 0x01
CMD_VERSION = 0x02
CMD_ECHO = 0x03
@staticmethod
def crc16(data: bytes) -> int:
"""Compute CRC16-CCITT for frame payload (excluding header/len/crc).
CRC16-CCITT: poly=0x1021, init=0xFFFF, xor_out=0xFFFF
"""
crc = 0xFFFF
for byte in data:
crc ^= byte << 8
for _ in range(8):
crc <<= 1
if crc & 0x10000:
crc ^= 0x1021
crc &= 0xFFFF
return crc ^ 0xFFFF
@staticmethod
def build(cmd: int, payload: bytes = b"") -> bytes:
"""Build a jlink frame.
Returns:
bytes: [0xAA][0x55][len][cmd][payload...][crc16_hi][crc16_lo]
"""
frame_data = bytes([cmd]) + payload
frame_len = len(frame_data)
crc16_val = JlinkFrame.crc16(frame_data)
frame = bytearray([JlinkFrame.HEADER[0], JlinkFrame.HEADER[1]])
frame.append(frame_len)
frame.extend(frame_data)
frame.append((crc16_val >> 8) & 0xFF)
frame.append(crc16_val & 0xFF)
return bytes(frame)
@staticmethod
def parse(data: bytes) -> Optional[Tuple[int, bytes]]:
"""Parse a received frame.
Returns:
(cmd, payload) or None if invalid
"""
if len(data) < 6:
return None
if data[0] != JlinkFrame.HEADER[0] or data[1] != JlinkFrame.HEADER[1]:
return None
frame_len = data[2]
if len(data) < 3 + frame_len + 2:
return None
cmd = data[3]
payload = data[4:3+frame_len]
crc16_received = (data[3+frame_len] << 8) | data[3+frame_len+1]
frame_data = bytes([cmd]) + payload
crc16_computed = JlinkFrame.crc16(frame_data)
if crc16_received != crc16_computed:
logger.warning(f"CRC mismatch: got {crc16_received:04x}, expected {crc16_computed:04x}")
return None
return (cmd, payload)
class UartTest:
"""UART test harness for FC↔Orin verification."""
def __init__(self, port: str, baud: int, timeout: float = 0.5):
self.port_name = port
self.baud_rate = baud
self.timeout = timeout
self.ser: Optional[serial.Serial] = None
self.results = {
"timestamp": datetime.now().isoformat(),
"port": port,
"baud": baud,
"tests": {},
"summary": {
"passed": 0,
"failed": 0,
"errors": [],
},
}
def open(self) -> bool:
"""Open serial port."""
try:
self.ser = serial.Serial(
port=self.port_name,
baudrate=self.baud_rate,
timeout=self.timeout,
)
self.ser.reset_input_buffer()
logger.info(f"Opened {self.port_name} @ {self.baud_rate} baud")
return True
except serial.SerialException as e:
logger.error(f"Failed to open {self.port_name}: {e}")
self.results["summary"]["errors"].append(str(e))
return False
def close(self):
"""Close serial port."""
if self.ser and self.ser.is_open:
self.ser.close()
logger.info(f"Closed {self.port_name}")
def send_and_wait(self, cmd: int, payload: bytes = b"", max_attempts: int = 3) -> Optional[Tuple[int, bytes]]:
"""Send frame and wait for response.
Returns:
(response_cmd, response_payload) or None on timeout
"""
if not self.ser or not self.ser.is_open:
return None
frame = JlinkFrame.build(cmd, payload)
for attempt in range(max_attempts):
try:
self.ser.write(frame)
logger.debug(f"Sent frame: {frame.hex()}")
# Wait for response with timeout
start_time = time.time()
response = bytearray()
while time.time() - start_time < self.timeout:
if self.ser.in_waiting:
data = self.ser.read(self.ser.in_waiting)
response.extend(data)
logger.debug(f"Received {len(data)} bytes: {data.hex()}")
# Try to parse complete frame
result = JlinkFrame.parse(bytes(response))
if result:
return result
time.sleep(0.01)
logger.warning(f"No response on attempt {attempt+1}/{max_attempts}")
except serial.SerialException as e:
logger.error(f"Serial error: {e}")
self.results["summary"]["errors"].append(str(e))
return None
return None
def test_ping(self) -> bool:
"""Test PING command."""
logger.info("Testing PING...")
test_name = "ping"
response = self.send_and_wait(JlinkFrame.CMD_PING)
if response:
resp_cmd, resp_payload = response
self.results["tests"][test_name] = {
"status": "PASS",
"response_cmd": resp_cmd,
"response_len": len(resp_payload),
}
logger.info(f" ✓ PING response received (cmd={resp_cmd})")
self.results["summary"]["passed"] += 1
return True
else:
self.results["tests"][test_name] = {"status": "FAIL", "reason": "No response"}
logger.error(" ✗ PING timeout")
self.results["summary"]["failed"] += 1
return False
def test_version(self) -> bool:
"""Test VERSION command."""
logger.info("Testing VERSION...")
test_name = "version"
response = self.send_and_wait(JlinkFrame.CMD_VERSION)
if response:
resp_cmd, resp_payload = response
version_str = resp_payload.decode("utf-8", errors="replace").strip()
self.results["tests"][test_name] = {
"status": "PASS",
"version": version_str,
}
logger.info(f" ✓ VERSION response: {version_str}")
self.results["summary"]["passed"] += 1
return True
else:
self.results["tests"][test_name] = {"status": "FAIL", "reason": "No response"}
logger.error(" ✗ VERSION timeout")
self.results["summary"]["failed"] += 1
return False
def test_echo(self, test_data: bytes = b"UART_TEST_FRAME") -> bool:
"""Test ECHO command."""
logger.info(f"Testing ECHO with payload: {test_data}")
test_name = "echo"
response = self.send_and_wait(JlinkFrame.CMD_ECHO, test_data)
if response:
resp_cmd, resp_payload = response
if resp_payload == test_data:
self.results["tests"][test_name] = {
"status": "PASS",
"payload_sent": test_data.hex(),
"payload_received": resp_payload.hex(),
}
logger.info(f" ✓ ECHO verified (round-trip OK)")
self.results["summary"]["passed"] += 1
return True
else:
self.results["tests"][test_name] = {
"status": "FAIL",
"reason": "Payload mismatch",
"sent": test_data.hex(),
"received": resp_payload.hex(),
}
logger.error(f" ✗ ECHO mismatch: sent {test_data.hex()}, got {resp_payload.hex()}")
self.results["summary"]["failed"] += 1
return False
else:
self.results["tests"][test_name] = {"status": "FAIL", "reason": "No response"}
logger.error(" ✗ ECHO timeout")
self.results["summary"]["failed"] += 1
return False
def run_all_tests(self) -> bool:
"""Run all verification tests."""
logger.info("=" * 60)
logger.info("FC↔Orin UART Verification Test Suite (Issue #362)")
logger.info("=" * 60)
if not self.open():
return False
try:
time.sleep(0.5) # Wait for device to be ready
self.test_ping()
time.sleep(0.1)
self.test_version()
time.sleep(0.1)
self.test_echo(b"UART_TEST")
# Summary
logger.info("=" * 60)
passed = self.results["summary"]["passed"]
failed = self.results["summary"]["failed"]
total = passed + failed
logger.info(f"Results: {passed}/{total} passed")
if failed == 0:
logger.info("✓ All tests PASSED")
return True
else:
logger.warning(f"{failed} test(s) FAILED")
return False
finally:
self.close()
def to_json(self) -> str:
"""Export results as JSON."""
return json.dumps(self.results, indent=2)
def publish_mqtt(self, host: str, topic: str = "saltybot/uart_test") -> bool:
"""Publish results to MQTT broker.
Requires: pip install paho-mqtt
"""
try:
import paho.mqtt.client as mqtt
except ImportError:
logger.error("paho-mqtt not installed. Install with: pip install paho-mqtt")
return False
try:
client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1)
client.connect(host, 1883, keepalive=10)
payload = self.to_json()
client.publish(topic, payload, qos=1)
client.disconnect()
logger.info(f"Published results to {host}:{topic}")
return True
except Exception as e:
logger.error(f"MQTT publish failed: {e}")
return False
def main():
parser = argparse.ArgumentParser(
description="FC↔Orin UART verification test (Issue #362)"
)
parser.add_argument(
"--port",
default="/dev/ttyTHS1",
help="Serial port device (default: /dev/ttyTHS1)",
)
parser.add_argument(
"--baud",
type=int,
default=921600,
help="Baud rate (default: 921600)",
)
parser.add_argument(
"--timeout",
type=float,
default=0.5,
help="Response timeout in seconds (default: 0.5)",
)
parser.add_argument(
"--mqtt-host",
help="MQTT broker host (optional, requires paho-mqtt)",
)
parser.add_argument(
"--mqtt-topic",
default="saltybot/uart_test",
help="MQTT topic for results (default: saltybot/uart_test)",
)
parser.add_argument(
"--json-output",
help="Save results to JSON file",
)
args = parser.parse_args()
test = UartTest(args.port, args.baud, args.timeout)
success = test.run_all_tests()
# Save JSON results
if args.json_output:
try:
with open(args.json_output, "w") as f:
f.write(test.to_json())
logger.info(f"Results saved to {args.json_output}")
except Exception as e:
logger.error(f"Failed to save JSON: {e}")
# Publish to MQTT
if args.mqtt_host:
test.publish_mqtt(args.mqtt_host, args.mqtt_topic)
return 0 if success else 1
if __name__ == "__main__":
sys.exit(main())

58
src/esc_backend.c Normal file
View File

@ -0,0 +1,58 @@
#include "esc_backend.h"
#include <stddef.h>
/* Global active backend (selected at runtime or compile-time) */
static const esc_backend_t *g_active_backend = NULL;
void esc_backend_register(const esc_backend_t *backend) {
g_active_backend = backend;
if (backend && backend->init) {
backend->init();
}
}
const esc_backend_t *esc_backend_get(void) {
return g_active_backend;
}
/* High-level convenience wrappers — call through vtable */
void esc_init(void) {
if (g_active_backend && g_active_backend->init) {
g_active_backend->init();
}
}
void esc_send(int16_t speed, int16_t steer) {
if (g_active_backend && g_active_backend->send) {
g_active_backend->send(speed, steer);
}
}
void esc_estop(void) {
if (g_active_backend && g_active_backend->estop) {
g_active_backend->estop();
}
}
void esc_resume(void) {
if (g_active_backend && g_active_backend->resume) {
g_active_backend->resume();
}
}
void esc_get_telemetry(esc_telemetry_t *out) {
if (out) {
/* Zero-fill by default */
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
if (g_active_backend && g_active_backend->get_telemetry) {
g_active_backend->get_telemetry(out);
}
}
}

127
src/esc_hoverboard.c Normal file
View File

@ -0,0 +1,127 @@
#include "esc_backend.h"
#include "config.h"
#include "stm32f7xx_hal.h"
/*
* Hoverboard ESC Backend Implementation
*
* Adapts the Hoverboard EFeru FOC protocol to the ESC backend vtable.
* UART2: PA2=TX, PA3=RX @ 115200 baud
*
* Packet: [0xABCD] [steer:i16] [speed:i16] [checksum:u16]
* Checksum = start ^ steer ^ speed
* Speed range: -1000 to +1000
* Must send at >=50Hz or ESC times out.
*/
#define HOVERBOARD_START_FRAME 0xABCD
#define HOVERBOARD_BAUD 115200
typedef struct __attribute__((packed)) {
uint16_t start;
int16_t steer;
int16_t speed;
uint16_t checksum;
} hoverboard_cmd_t;
static UART_HandleTypeDef huart2;
/* Backend vtable instance */
static const esc_backend_t hoverboard_backend;
/*
* Initialize UART2 for hoverboard communication.
* Called once at startup via backend registration.
*/
static void hoverboard_backend_init(void) {
/* Enable clocks */
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/* PA2=TX, PA3=RX, AF7 for USART2 */
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_2 | GPIO_PIN_3;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF7_USART2;
HAL_GPIO_Init(GPIOA, &gpio);
/* USART2 config */
huart2.Instance = USART2;
huart2.Init.BaudRate = HOVERBOARD_BAUD;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
}
/*
* Send motor command via hoverboard protocol.
* Called at ~50Hz from motor_driver_update().
*/
static void hoverboard_backend_send(int16_t speed, int16_t steer) {
hoverboard_cmd_t cmd;
cmd.start = HOVERBOARD_START_FRAME;
cmd.steer = steer;
cmd.speed = speed;
cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed;
HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
}
/*
* Emergency stop: send zero and disable motors.
* Hoverboard will disable outputs on repeated zero packets.
*/
static void hoverboard_backend_estop(void) {
hoverboard_backend_send(0, 0);
}
/*
* Resume after estop (optional, hoverboard auto-resumes on non-zero command).
*/
static void hoverboard_backend_resume(void) {
/* No action needed — next non-zero send will resume */
}
/*
* Query telemetry from hoverboard.
* Hoverboard protocol is command-only (no RX in this implementation).
* Return zero telemetry stub; future: add RX feedback.
*/
static void hoverboard_backend_get_telemetry(esc_telemetry_t *out) {
if (out) {
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
}
}
/* Hoverboard backend vtable */
static const esc_backend_t hoverboard_backend = {
.init = hoverboard_backend_init,
.send = hoverboard_backend_send,
.estop = hoverboard_backend_estop,
.resume = hoverboard_backend_resume,
.get_telemetry = hoverboard_backend_get_telemetry,
};
/*
* Public functions for backward compatibility.
* These remain for existing code that calls hoverboard_init/hoverboard_send directly.
*/
void hoverboard_init(void) {
esc_backend_register(&hoverboard_backend);
}
void hoverboard_send(int16_t speed, int16_t steer) {
esc_send(speed, steer);
}

70
src/esc_vesc.c Normal file
View File

@ -0,0 +1,70 @@
#include "esc_backend.h"
/*
* VESC ESC Backend Stub
*
* Placeholder implementation for FSESC 4.20 Plus (VESC-based dual ESC).
* UART: ttyTHS1 (Jetson Orin) FC USART6 @ 921600 baud
* Protocol: pyvesc with CRC16-XModem checksum
*
* Issue #383: VESC integration (fills in this stub)
* Issue #388: ESC abstraction layer (provides this interface)
*
* TODO (Issue #383):
* - Implement vesc_init() with UART/GPIO config
* - Implement vesc_send() with pyvesc packet encoding (duty/RPM control)
* - Implement vesc_estop() to disable motor controller
* - Implement vesc_get_telemetry() to parse VESC state messages
* - Add balance mode configuration if using VESC balance app
*/
static const esc_backend_t vesc_backend;
/* Stub implementations — no-op until #383 fills them in */
static void vesc_backend_init(void) {
/* TODO (Issue #383): Initialize UART6, configure VESC balance mode */
}
static void vesc_backend_send(int16_t speed, int16_t steer) {
/* TODO (Issue #383): Encode speed/steer to pyvesc packet and send via UART6 */
(void)speed;
(void)steer;
}
static void vesc_backend_estop(void) {
/* TODO (Issue #383): Send VESC shutdown command to disable motors */
}
static void vesc_backend_resume(void) {
/* TODO (Issue #383): Resume from estop if needed */
}
static void vesc_backend_get_telemetry(esc_telemetry_t *out) {
/* TODO (Issue #383): Poll/parse VESC telemetry (voltage, current, RPM, temp, fault) */
if (out) {
out->speed = 0;
out->steer = 0;
out->voltage_mv = 0;
out->current_ma = 0;
out->temperature_c = 0;
out->fault = 0;
}
}
/* VESC backend vtable */
static const esc_backend_t vesc_backend = {
.init = vesc_backend_init,
.send = vesc_backend_send,
.estop = vesc_backend_estop,
.resume = vesc_backend_resume,
.get_telemetry = vesc_backend_get_telemetry,
};
/*
* Public function to register VESC backend.
* Called from main.c when configured with ESC_BACKEND=VESC.
*/
void vesc_backend_register_impl(void) {
esc_backend_register(&vesc_backend);
}

View File

@ -1,41 +0,0 @@
#include "hoverboard.h"
#include "config.h"
#include "stm32f7xx_hal.h"
static UART_HandleTypeDef huart2;
void hoverboard_init(void) {
/* Enable clocks */
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/* PA2=TX, PA3=RX, AF7 for USART2 */
GPIO_InitTypeDef gpio = {0};
gpio.Pin = GPIO_PIN_2 | GPIO_PIN_3;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_PULLUP;
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
gpio.Alternate = GPIO_AF7_USART2;
HAL_GPIO_Init(GPIOA, &gpio);
/* USART2 config */
huart2.Instance = USART2;
huart2.Init.BaudRate = HOVERBOARD_BAUD;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
HAL_UART_Init(&huart2);
}
void hoverboard_send(int16_t speed, int16_t steer) {
hoverboard_cmd_t cmd;
cmd.start = HOVERBOARD_START_FRAME;
cmd.steer = steer;
cmd.speed = speed;
cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed;
HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
}

View File

@ -15,6 +15,7 @@
#include "i2c1.h"
#include "bmp280.h"
#include "mag.h"
#include "bno055.h"
#include "jetson_cmd.h"
#include "jlink.h"
#include "ota.h"
@ -27,6 +28,7 @@
#include "power_mgmt.h"
#include "battery.h"
#include "coulomb_counter.h"
#include "watchdog.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
@ -42,6 +44,9 @@ 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> ?
@ -119,6 +124,9 @@ int main(void) {
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);
@ -128,6 +136,11 @@ int main(void) {
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;

View File

@ -1,5 +1,5 @@
#include "motor_driver.h"
#include "hoverboard.h"
#include "esc_backend.h"
#include "config.h"
#include <stdlib.h>
@ -22,7 +22,7 @@ void motor_driver_update(motor_driver_t *m,
/* Emergency stop: send zero, hold latch */
if (m->estop) {
hoverboard_send(0, 0);
esc_send(0, 0);
return;
}
@ -42,7 +42,7 @@ void motor_driver_update(motor_driver_t *m,
if (steer > headroom) steer = headroom;
if (steer < -headroom) steer = -headroom;
hoverboard_send((int16_t)speed, (int16_t)steer);
esc_send((int16_t)speed, (int16_t)steer);
}
void motor_driver_estop(motor_driver_t *m) {