Compare commits
6 Commits
aa90ea2fa7
...
170c64eec1
| Author | SHA1 | Date | |
|---|---|---|---|
| 170c64eec1 | |||
| 93756f5248 | |||
| 3c93a72d01 | |||
| 844504e92e | |||
| 985e03a26d | |||
| d52e7af554 |
@ -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
79
include/esc_backend.h
Normal 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 */
|
||||
@ -0,0 +1,83 @@
|
||||
"""
|
||||
uart_bridge.launch.py — FC↔Orin 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 THS0–THS3):
|
||||
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 (THS0–THS3)",
|
||||
),
|
||||
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
417
jetson/scripts/uart_test.py
Executable file
@ -0,0 +1,417 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
uart_test.py — FC↔Orin 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
58
src/esc_backend.c
Normal 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
127
src/esc_hoverboard.c
Normal 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
70
src/esc_vesc.c
Normal 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);
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
13
src/main.c
13
src/main.c
@ -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;
|
||||
|
||||
|
||||
@ -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) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user