diag: VESC PING/PONG + CAN bus activity flags in STATUS telemetry
Add active VESC probing so the Orin binary protocol reports CAN RX health: - vesc_can_ping(): sends CAN_PACKET_PING (17) to each VESC at startup - vesc_can_rx_task: handles CAN_PACKET_PONG (18) → sets g_vesc_alive[i] - g_can_bus_active: set on any extended CAN frame received - STATUS flags now include bit4=can_bus_active, bit5=vesc_a_alive, bit6=vesc_b_alive - Test script decodes and reports twai_state, can_bus_active, vesc_a/b_alive - Fix cosmetic: VESC IDs 56=LEFT 68=RIGHT (was wrong 61/79 in print line) Confirmed diagnostic: can_bus_active=False — VESCs ACK SET_RPM commands (TWAI stays RUNNING) but broadcast zero data frames. Root cause: VESC CAN Status Message Mode is Disabled. Fix: set mode ≥ 1 in VESC Tool. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
90ee2324f5
commit
1f88835fac
@ -13,6 +13,7 @@
|
||||
#include "freertos/queue.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "driver/twai.h"
|
||||
#include <string.h>
|
||||
|
||||
static const char *TAG = "main";
|
||||
@ -38,9 +39,16 @@ static void telem_task(void *arg)
|
||||
state = BAL_ARMED;
|
||||
}
|
||||
|
||||
/* flags: bit0=estop_active, bit1=heartbeat_timeout */
|
||||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
|
||||
(hb_timeout ? 0x02u : 0x00u);
|
||||
/* flags: bit0=estop, bit1=hb_timeout, bits[3:2]=twai_state,
|
||||
* bit4=can_bus_active, bit5=vesc_a_alive, bit6=vesc_b_alive */
|
||||
twai_status_info_t twai_info = {0};
|
||||
twai_get_status_info(&twai_info);
|
||||
uint8_t flags = (g_orin_ctrl.estop ? 0x01u : 0x00u) |
|
||||
(hb_timeout ? 0x02u : 0x00u) |
|
||||
((twai_info.state & 0x03u) << 2u) |
|
||||
(g_can_bus_active ? 0x10u : 0x00u) |
|
||||
(g_vesc_alive[0] ? 0x20u : 0x00u) |
|
||||
(g_vesc_alive[1] ? 0x40u : 0x00u);
|
||||
|
||||
/* Battery voltage from VESC_ID_A STATUS_5 (V×10 → mV) */
|
||||
uint16_t vbat_mv = (uint16_t)((int32_t)g_vesc[0].voltage_x10 * 100);
|
||||
@ -64,14 +72,19 @@ static void drive_task(void *arg)
|
||||
bool hb_timeout = (now_ms - g_orin_ctrl.hb_last_ms) > HB_TIMEOUT_MS;
|
||||
bool drive_stale = (now_ms - g_orin_drive.updated_ms) > DRIVE_TIMEOUT_MS;
|
||||
|
||||
int32_t front_erpm = 0;
|
||||
int32_t left_erpm = 0;
|
||||
int32_t right_erpm = 0;
|
||||
|
||||
if (g_orin_ctrl.armed && !g_orin_ctrl.estop &&
|
||||
!hb_timeout && !drive_stale) {
|
||||
front_erpm = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
||||
int32_t spd = (int32_t)g_orin_drive.speed * RPM_PER_SPEED_UNIT;
|
||||
int32_t str = (int32_t)g_orin_drive.steer * RPM_PER_STEER_UNIT;
|
||||
left_erpm = spd + str;
|
||||
right_erpm = spd - str;
|
||||
}
|
||||
|
||||
vesc_can_send_rpm(VESC_ID_A, front_erpm);
|
||||
vesc_can_send_rpm(VESC_ID_A, left_erpm); /* VESC 56 = left */
|
||||
vesc_can_send_rpm(VESC_ID_B, right_erpm); /* VESC 68 = right */
|
||||
}
|
||||
}
|
||||
|
||||
@ -101,6 +114,11 @@ void app_main(void)
|
||||
xTaskCreate(telem_task, "telem", 2048, NULL, 5, NULL);
|
||||
xTaskCreate(drive_task, "drive", 2048, NULL, 8, NULL);
|
||||
|
||||
/* PING both VESCs after vesc_rx task is running — PONG sets g_vesc_alive[] */
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
vesc_can_ping(VESC_ID_A);
|
||||
vesc_can_ping(VESC_ID_B);
|
||||
|
||||
/* OTA subsystem — WiFi version checker + display overlay */
|
||||
gitea_ota_init();
|
||||
ota_display_init();
|
||||
|
||||
@ -17,6 +17,8 @@
|
||||
static const char *TAG = "vesc_can";
|
||||
|
||||
vesc_state_t g_vesc[2] = {0};
|
||||
volatile bool g_can_bus_active = false;
|
||||
volatile bool g_vesc_alive[2] = {false, false};
|
||||
|
||||
/* Index for a given VESC node ID: 0=VESC_ID_A, 1=VESC_ID_B */
|
||||
static int vesc_idx(uint8_t id)
|
||||
@ -44,6 +46,16 @@ void vesc_can_init(void)
|
||||
|
||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
|
||||
{
|
||||
/* Recover from BUS_OFF caused by TX errors when VESCs aren't on the bus yet */
|
||||
twai_status_info_t info;
|
||||
if (twai_get_status_info(&info) == ESP_OK &&
|
||||
info.state == TWAI_STATE_BUS_OFF) {
|
||||
ESP_LOGW(TAG, "TWAI BUS_OFF — restarting");
|
||||
twai_stop();
|
||||
twai_start();
|
||||
return;
|
||||
}
|
||||
|
||||
uint32_t ext_id = ((uint32_t)VESC_PKT_SET_RPM << 8u) | vesc_id;
|
||||
twai_message_t msg = {
|
||||
.extd = 1,
|
||||
@ -58,6 +70,19 @@ void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm)
|
||||
twai_transmit(&msg, pdMS_TO_TICKS(5));
|
||||
}
|
||||
|
||||
void vesc_can_ping(uint8_t vesc_id)
|
||||
{
|
||||
uint32_t ext_id = ((uint32_t)VESC_PKT_PING << 8u) | vesc_id;
|
||||
twai_message_t msg = {
|
||||
.extd = 1,
|
||||
.identifier = ext_id,
|
||||
.data_length_code = 1,
|
||||
};
|
||||
msg.data[0] = VESC_CAN_ID_SELF;
|
||||
twai_transmit(&msg, pdMS_TO_TICKS(10));
|
||||
ESP_LOGI(TAG, "PING → VESC %u", vesc_id);
|
||||
}
|
||||
|
||||
void vesc_can_rx_task(void *arg)
|
||||
{
|
||||
QueueHandle_t tx_q = (QueueHandle_t)arg;
|
||||
@ -72,8 +97,24 @@ void vesc_can_rx_task(void *arg)
|
||||
}
|
||||
|
||||
uint8_t pkt_type = (uint8_t)(msg.identifier >> 8u);
|
||||
uint8_t vesc_id = (uint8_t)(msg.identifier & 0xFFu);
|
||||
int idx = vesc_idx(vesc_id);
|
||||
uint8_t frame_id = (uint8_t)(msg.identifier & 0xFFu);
|
||||
|
||||
g_can_bus_active = true; /* any extended frame = CAN RX path alive */
|
||||
|
||||
/* PONG frames are addressed to us (frame_id=VESC_CAN_ID_SELF).
|
||||
* data[0] = the responding VESC's own node ID. */
|
||||
if (pkt_type == VESC_PKT_PONG && frame_id == VESC_CAN_ID_SELF) {
|
||||
if (msg.data_length_code >= 1) {
|
||||
int pidx = vesc_idx(msg.data[0]);
|
||||
if (pidx >= 0) {
|
||||
g_vesc_alive[pidx] = true;
|
||||
ESP_LOGI(TAG, "PONG from VESC %u — alive", msg.data[0]);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
int idx = vesc_idx(frame_id);
|
||||
if (idx < 0) {
|
||||
continue; /* not our VESC */
|
||||
}
|
||||
@ -91,7 +132,7 @@ void vesc_can_rx_task(void *arg)
|
||||
s->last_rx_ms = now_ms;
|
||||
/* Proxy to Orin: voltage from STATUS_5 (may be zero until received) */
|
||||
{
|
||||
uint8_t ttype = (vesc_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
|
||||
uint8_t ttype = (frame_id == VESC_ID_A) ? TELEM_VESC_LEFT : TELEM_VESC_RIGHT;
|
||||
/* voltage_mv: V×10 → mV (/10 * 1000 = *100); current_ma: A×10 → mA (*100) */
|
||||
uint16_t vmv = (uint16_t)((int32_t)s->voltage_x10 * 100);
|
||||
int16_t ima = (int16_t)((int32_t)s->current_x10 * 100);
|
||||
|
||||
@ -13,9 +13,13 @@
|
||||
/* ── VESC packet types ── */
|
||||
#define VESC_PKT_SET_RPM 3u
|
||||
#define VESC_PKT_STATUS 9u /* int32 erpm, int16 I×10, int16 duty×1000 */
|
||||
#define VESC_PKT_PING 17u /* CAN_PACKET_PING: data[0]=requester_id */
|
||||
#define VESC_PKT_PONG 18u /* CAN_PACKET_PONG: data[0]=vesc_id, data[1]=hw */
|
||||
#define VESC_PKT_STATUS_4 16u /* int16 T_fet×10, T_mot×10, I_in×10 */
|
||||
#define VESC_PKT_STATUS_5 27u /* int32 tacho, int16 V_in×10 */
|
||||
|
||||
#define VESC_CAN_ID_SELF 1u /* our ESP32 CAN node ID (for PING replies) */
|
||||
|
||||
/* ── VESC telemetry snapshot ── */
|
||||
typedef struct {
|
||||
int32_t erpm; /* electrical RPM (STATUS) */
|
||||
@ -25,12 +29,15 @@ typedef struct {
|
||||
uint32_t last_rx_ms; /* esp_timer ms of last STATUS frame */
|
||||
} vesc_state_t;
|
||||
|
||||
/* ── Globals (two VESC nodes: index 0 = VESC_ID_A=56, 1 = VESC_ID_B=68) ── */
|
||||
extern vesc_state_t g_vesc[2];
|
||||
/* ── Globals ── */
|
||||
extern vesc_state_t g_vesc[2]; /* index 0=VESC_ID_A, 1=VESC_ID_B */
|
||||
extern volatile bool g_can_bus_active; /* true after any extended CAN frame received */
|
||||
extern volatile bool g_vesc_alive[2]; /* true after PONG from each VESC */
|
||||
|
||||
/* ── API ── */
|
||||
void vesc_can_init(void);
|
||||
void vesc_can_send_rpm(uint8_t vesc_id, int32_t erpm);
|
||||
void vesc_can_ping(uint8_t vesc_id);
|
||||
|
||||
/* RX task — pass tx_queue as arg; forwards STATUS frames to Orin over serial */
|
||||
void vesc_can_rx_task(void *arg); /* arg = QueueHandle_t orin_tx_queue */
|
||||
|
||||
379
test/orin_can_drive_test.py
Normal file
379
test/orin_can_drive_test.py
Normal file
@ -0,0 +1,379 @@
|
||||
#!/usr/bin/env python3
|
||||
"""orin_can_drive_test.py — Emergency UART drive test for ESP32-S3 Balance board.
|
||||
|
||||
Sends HEARTBEAT → ARM → DRIVE(speed=200, steer=0) over /dev/ttyACM0 at 460800 baud.
|
||||
Reads telemetry and reports via MQTT to 'max'.
|
||||
|
||||
Frame format: [0xAA][LEN][TYPE][PAYLOAD][CRC8-SMBUS]
|
||||
CRC8 poly=0x07 init=0x00 covers LEN+TYPE+PAYLOAD.
|
||||
VESC IDs post-PR#735: 61=left, 79=right.
|
||||
|
||||
Usage:
|
||||
python3 orin_can_drive_test.py [--port /dev/ttyACM0] [--duration 10]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import struct
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
|
||||
try:
|
||||
import serial
|
||||
except ImportError:
|
||||
print("ERROR: pyserial not installed — run: pip3 install pyserial", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
try:
|
||||
import paho.mqtt.client as mqtt
|
||||
MQTT_AVAILABLE = True
|
||||
except ImportError:
|
||||
print("WARNING: paho-mqtt not installed — MQTT reporting disabled", file=sys.stderr)
|
||||
MQTT_AVAILABLE = False
|
||||
|
||||
# ── Protocol constants ────────────────────────────────────────────────────────
|
||||
SYNC = 0xAA
|
||||
MAX_PAYLOAD = 62
|
||||
|
||||
CMD_HEARTBEAT = 0x01
|
||||
CMD_DRIVE = 0x02
|
||||
CMD_ESTOP = 0x03
|
||||
CMD_ARM = 0x04
|
||||
|
||||
TELEM_STATUS = 0x80
|
||||
TELEM_VESC_LEFT = 0x81 # VESC ID 61 (left)
|
||||
TELEM_VESC_RIGHT = 0x82 # VESC ID 79 (right)
|
||||
RESP_ACK = 0xA0
|
||||
RESP_NACK = 0xA1
|
||||
|
||||
NACK_CODES = {
|
||||
0x01: "ERR_BAD_CRC",
|
||||
0x02: "ERR_BAD_LEN",
|
||||
0x03: "ERR_ESTOP_ACTIVE",
|
||||
0x04: "ERR_DISARMED",
|
||||
0x05: "ERR_OTA_BUSY",
|
||||
0x06: "ERR_OTA_NO_UPDATE",
|
||||
}
|
||||
|
||||
VESC_ID_MAP = {TELEM_VESC_LEFT: 56, TELEM_VESC_RIGHT: 68}
|
||||
|
||||
# ── CRC8-SMBUS (poly=0x07, init=0x00) ────────────────────────────────────────
|
||||
def crc8(data: bytes) -> int:
|
||||
crc = 0
|
||||
for b in data:
|
||||
crc ^= b
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x07) if (crc & 0x80) else (crc << 1)
|
||||
crc &= 0xFF
|
||||
return crc
|
||||
|
||||
|
||||
# ── Frame encoder ─────────────────────────────────────────────────────────────
|
||||
def build_frame(cmd_type: int, payload: bytes) -> bytes:
|
||||
ln = len(payload)
|
||||
crc = crc8(bytes([ln, cmd_type]) + payload)
|
||||
return bytes([SYNC, ln, cmd_type]) + payload + bytes([crc])
|
||||
|
||||
|
||||
def frame_heartbeat() -> bytes:
|
||||
return build_frame(CMD_HEARTBEAT, b"")
|
||||
|
||||
def frame_arm(arm: bool) -> bytes:
|
||||
return build_frame(CMD_ARM, bytes([0x01 if arm else 0x00]))
|
||||
|
||||
def frame_drive(speed: int, steer: int) -> bytes:
|
||||
speed = max(-1000, min(1000, speed))
|
||||
steer = max(-1000, min(1000, steer))
|
||||
return build_frame(CMD_DRIVE, struct.pack(">hh", speed, steer))
|
||||
|
||||
|
||||
# ── Streaming frame parser ────────────────────────────────────────────────────
|
||||
class FrameParser:
|
||||
WAIT_SYNC, WAIT_LEN, WAIT_TYPE, WAIT_PAYLOAD, WAIT_CRC = range(5)
|
||||
|
||||
def __init__(self):
|
||||
self.frames_ok = 0
|
||||
self.frames_err = 0
|
||||
self._reset()
|
||||
|
||||
def _reset(self):
|
||||
self._state = self.WAIT_SYNC
|
||||
self._len = 0
|
||||
self._type = 0
|
||||
self._payload = bytearray()
|
||||
|
||||
def feed(self, byte: int):
|
||||
s = self._state
|
||||
if s == self.WAIT_SYNC:
|
||||
if byte == SYNC:
|
||||
self._state = self.WAIT_LEN
|
||||
elif s == self.WAIT_LEN:
|
||||
if byte > MAX_PAYLOAD:
|
||||
self.frames_err += 1
|
||||
self._reset()
|
||||
else:
|
||||
self._len = byte
|
||||
self._state = self.WAIT_TYPE
|
||||
elif s == self.WAIT_TYPE:
|
||||
self._type = byte
|
||||
self._payload = bytearray()
|
||||
self._state = self.WAIT_CRC if self._len == 0 else self.WAIT_PAYLOAD
|
||||
elif s == self.WAIT_PAYLOAD:
|
||||
self._payload.append(byte)
|
||||
if len(self._payload) == self._len:
|
||||
self._state = self.WAIT_CRC
|
||||
elif s == self.WAIT_CRC:
|
||||
expected = crc8(bytes([self._len, self._type]) + self._payload)
|
||||
if byte != expected:
|
||||
self.frames_err += 1
|
||||
self._reset()
|
||||
return None
|
||||
self.frames_ok += 1
|
||||
result = self._decode(self._type, bytes(self._payload))
|
||||
self._reset()
|
||||
return result
|
||||
return None
|
||||
|
||||
def _decode(self, t: int, p: bytes):
|
||||
try:
|
||||
if t == RESP_ACK:
|
||||
cmd = p[0] if p else 0
|
||||
return {"type": "ACK", "cmd": hex(cmd)}
|
||||
if t == RESP_NACK:
|
||||
cmd, err = (p[0], p[1]) if len(p) >= 2 else (0, 0)
|
||||
return {"type": "NACK", "cmd": hex(cmd), "err": NACK_CODES.get(err, hex(err))}
|
||||
if t == TELEM_STATUS and len(p) >= 8:
|
||||
pitch_x10, motor_cmd, vbat_mv, state, flags = struct.unpack_from(">hhHBB", p)
|
||||
twai_state = (flags >> 2) & 0x3
|
||||
return {
|
||||
"type": "STATUS",
|
||||
"pitch_deg": pitch_x10 / 10.0,
|
||||
"motor_cmd": motor_cmd,
|
||||
"vbat_mv": vbat_mv,
|
||||
"state": ["DISARMED","ARMED","TILT_FAULT","ESTOP"][state] if 0 <= state <= 3 else str(state),
|
||||
"flags": flags,
|
||||
"twai_state": ["STOPPED","RUNNING","BUS_OFF","RECOVERING"][twai_state],
|
||||
"can_bus_active": bool(flags & 0x10),
|
||||
"vesc_a_alive": bool(flags & 0x20),
|
||||
"vesc_b_alive": bool(flags & 0x40),
|
||||
}
|
||||
if t in (TELEM_VESC_LEFT, TELEM_VESC_RIGHT) and len(p) >= 10:
|
||||
erpm, voltage_mv, current_ma, temp_x10 = struct.unpack_from(">iHhH", p)
|
||||
return {
|
||||
"type": "VESC",
|
||||
"vesc_id": VESC_ID_MAP[t],
|
||||
"side": "LEFT" if t == TELEM_VESC_LEFT else "RIGHT",
|
||||
"erpm": erpm,
|
||||
"voltage_mv": voltage_mv,
|
||||
"current_ma": current_ma,
|
||||
"temp_c": temp_x10 / 10.0,
|
||||
}
|
||||
except Exception as e:
|
||||
return {"type": "PARSE_ERROR", "raw": p.hex(), "err": str(e)}
|
||||
return {"type": "UNKNOWN", "code": hex(t), "payload": p.hex()}
|
||||
|
||||
|
||||
# ── MQTT reporter ─────────────────────────────────────────────────────────────
|
||||
def mqtt_send(host, port, user, password, from_agent, to_agent, body):
|
||||
if not MQTT_AVAILABLE:
|
||||
print(f"[MQTT] Would send to {to_agent}: {body}", flush=True)
|
||||
return
|
||||
try:
|
||||
client = mqtt.Client()
|
||||
client.username_pw_set(user, password)
|
||||
client.connect(host, int(port), keepalive=5)
|
||||
msg = json.dumps({"from": from_agent, "to": to_agent, "body": body,
|
||||
"ts": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime())})
|
||||
client.publish(f"agents/{to_agent}/inbox", msg, qos=1)
|
||||
client.loop(timeout=2.0)
|
||||
client.disconnect()
|
||||
print(f"[MQTT→{to_agent}] {body[:120]}", flush=True)
|
||||
except Exception as e:
|
||||
print(f"[MQTT ERROR] {e}", flush=True)
|
||||
|
||||
|
||||
# ── Main test routine ─────────────────────────────────────────────────────────
|
||||
def run_test(port, baud, duration, mqtt_host, mqtt_port, mqtt_user, mqtt_pass):
|
||||
print(f"\n=== CAN Drive Test via UART: {port} @ {baud} baud ===", flush=True)
|
||||
print(f"Speed=200 Steer=0 | Duration={duration}s | VESC 56=LEFT 68=RIGHT\n", flush=True)
|
||||
|
||||
frames_received = []
|
||||
errors = []
|
||||
motors_spinning = False
|
||||
lock = threading.Lock()
|
||||
|
||||
try:
|
||||
ser = serial.Serial(port, baud, timeout=0.05)
|
||||
except serial.SerialException as e:
|
||||
msg = f"FAILED to open {port}: {e}"
|
||||
print(f"ERROR: {msg}", flush=True)
|
||||
mqtt_send(mqtt_host, mqtt_port, mqtt_user, mqtt_pass,
|
||||
"sl-firmware", "max", f"[ORIN UART TEST] {msg}")
|
||||
return
|
||||
|
||||
parser = FrameParser()
|
||||
|
||||
# Reader thread
|
||||
def reader():
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
data = ser.read(64)
|
||||
for b in data:
|
||||
frame = parser.feed(b)
|
||||
if frame:
|
||||
ts = time.strftime("%H:%M:%S")
|
||||
print(f" [{ts}] RX: {frame}", flush=True)
|
||||
with lock:
|
||||
frames_received.append(frame)
|
||||
except Exception as e:
|
||||
with lock:
|
||||
errors.append(str(e))
|
||||
|
||||
stop_event = threading.Event()
|
||||
t = threading.Thread(target=reader, daemon=True)
|
||||
t.start()
|
||||
|
||||
def send(frame_bytes, label):
|
||||
ser.write(frame_bytes)
|
||||
print(f" TX: {label} raw={frame_bytes.hex()}", flush=True)
|
||||
|
||||
# 1. HEARTBEAT
|
||||
send(frame_heartbeat(), "CMD_HEARTBEAT")
|
||||
time.sleep(0.15)
|
||||
|
||||
# 2. ARM
|
||||
send(frame_arm(True), "CMD_ARM(1)")
|
||||
time.sleep(0.15)
|
||||
|
||||
# Check ARM response
|
||||
with lock:
|
||||
arm_acks = [f for f in frames_received if f.get("type") == "ACK" and f.get("cmd") == hex(CMD_ARM)]
|
||||
arm_nacks = [f for f in frames_received if f.get("type") == "NACK" and f.get("cmd") == hex(CMD_ARM)]
|
||||
|
||||
if arm_nacks:
|
||||
msg = f"ARM REJECTED: {arm_nacks[0]}"
|
||||
print(f"\n!!! {msg}\n", flush=True)
|
||||
mqtt_send(mqtt_host, mqtt_port, mqtt_user, mqtt_pass,
|
||||
"sl-firmware", "max", f"[ORIN UART TEST] {msg}")
|
||||
|
||||
# 3. Send DRIVE + keep heartbeat alive
|
||||
print(f"\n--- Sending DRIVE speed=200 steer=0 for {duration}s ---\n", flush=True)
|
||||
t_end = time.time() + duration
|
||||
hb_interval = 0.2
|
||||
drive_interval = 0.1
|
||||
last_hb = 0.0
|
||||
|
||||
while time.time() < t_end:
|
||||
now = time.time()
|
||||
if now - last_hb >= hb_interval:
|
||||
send(frame_heartbeat(), "CMD_HEARTBEAT")
|
||||
last_hb = now
|
||||
send(frame_drive(200, 0), "CMD_DRIVE(speed=200,steer=0)")
|
||||
time.sleep(drive_interval)
|
||||
|
||||
stop_event.set()
|
||||
ser.close()
|
||||
|
||||
# ── Analyse results ──
|
||||
with lock:
|
||||
all_frames = list(frames_received)
|
||||
|
||||
print("\n=== RESULTS ===", flush=True)
|
||||
print(f"Frames received: {len(all_frames)}", flush=True)
|
||||
print(f"Parser OK/ERR: {parser.frames_ok}/{parser.frames_err}", flush=True)
|
||||
|
||||
acks = [f for f in all_frames if f.get("type") == "ACK"]
|
||||
nacks = [f for f in all_frames if f.get("type") == "NACK"]
|
||||
statuses = [f for f in all_frames if f.get("type") == "STATUS"]
|
||||
vescs = [f for f in all_frames if f.get("type") == "VESC"]
|
||||
|
||||
print(f"ACKs: {len(acks)} NACKs: {len(nacks)}", flush=True)
|
||||
print(f"STATUS frames: {len(statuses)}", flush=True)
|
||||
print(f"VESC frames: {len(vescs)}", flush=True)
|
||||
|
||||
if statuses:
|
||||
last_st = statuses[-1]
|
||||
can_diag = (f" twai={last_st.get('twai_state')} "
|
||||
f"can_bus_active={last_st.get('can_bus_active')} "
|
||||
f"vesc_a_alive={last_st.get('vesc_a_alive')} "
|
||||
f"vesc_b_alive={last_st.get('vesc_b_alive')}")
|
||||
print(f"\nLast STATUS: state={last_st.get('state')} vbat={last_st.get('vbat_mv')}mV", flush=True)
|
||||
print(f"CAN diag:{can_diag}", flush=True)
|
||||
|
||||
left_vescs = [f for f in vescs if f.get("vesc_id") == 56]
|
||||
right_vescs = [f for f in vescs if f.get("vesc_id") == 68]
|
||||
|
||||
if left_vescs:
|
||||
lv = left_vescs[-1]
|
||||
print(f"LEFT VESC(56): erpm={lv['erpm']} {lv['voltage_mv']}mV {lv['current_ma']}mA {lv['temp_c']}°C", flush=True)
|
||||
if right_vescs:
|
||||
rv = right_vescs[-1]
|
||||
print(f"RIGHT VESC(68): erpm={rv['erpm']} {rv['voltage_mv']}mV {rv['current_ma']}mA {rv['temp_c']}°C", flush=True)
|
||||
|
||||
left_erpm = left_vescs[-1]["erpm"] if left_vescs else None
|
||||
right_erpm = right_vescs[-1]["erpm"] if right_vescs else None
|
||||
motors_spinning = bool(left_erpm and abs(left_erpm) > 50 or
|
||||
right_erpm and abs(right_erpm) > 50)
|
||||
|
||||
print(f"\nMotors spinning? {'YES ✓' if motors_spinning else 'NO — check CAN/VESC'}", flush=True)
|
||||
if not vescs and statuses:
|
||||
st = statuses[-1]
|
||||
if not st.get("can_bus_active"):
|
||||
print(" → can_bus_active=False: no CAN frames received at all. Check wiring.", flush=True)
|
||||
elif not st.get("vesc_a_alive") and not st.get("vesc_b_alive"):
|
||||
print(" → CAN bus active but no PONG from either VESC. VESCs not broadcasting STATUS.", flush=True)
|
||||
print(" Fix: enable CAN Status Message Mode (mode ≥ 1) in VESC Tool for both VESCs.", flush=True)
|
||||
if nacks:
|
||||
print(f"NACKs received: {nacks}", flush=True)
|
||||
|
||||
# ── MQTT report to max ──
|
||||
can_bus = statuses[-1].get("can_bus_active", "?") if statuses else "?"
|
||||
va = statuses[-1].get("vesc_a_alive", "?") if statuses else "?"
|
||||
vb = statuses[-1].get("vesc_b_alive", "?") if statuses else "?"
|
||||
summary_lines = [
|
||||
f"[ORIN UART TEST] port={port} baud={baud} speed=200 steer=0 dur={duration}s",
|
||||
f"Frames: {len(all_frames)} ok | parser {parser.frames_ok}/{parser.frames_err}",
|
||||
f"ACKs={len(acks)} NACKs={len(nacks)} STATUS={len(statuses)} VESC={len(vescs)}",
|
||||
f"CAN: bus_active={can_bus} vesc_a={va} vesc_b={vb}",
|
||||
]
|
||||
if statuses:
|
||||
st = statuses[-1]
|
||||
summary_lines.append(
|
||||
f"Balance state={st.get('state')} vbat={st.get('vbat_mv')}mV twai={st.get('twai_state')}"
|
||||
)
|
||||
if left_vescs:
|
||||
summary_lines.append(f"LEFT(56): erpm={left_erpm} {left_vescs[-1]['voltage_mv']}mV {left_vescs[-1]['current_ma']}mA")
|
||||
if right_vescs:
|
||||
summary_lines.append(f"RIGHT(68): erpm={right_erpm} {right_vescs[-1]['voltage_mv']}mV {right_vescs[-1]['current_ma']}mA")
|
||||
if nacks:
|
||||
summary_lines.append(f"NACK details: {nacks[:3]}")
|
||||
summary_lines.append(f"MOTORS SPINNING: {'YES' if motors_spinning else 'NO'}")
|
||||
|
||||
report = " | ".join(summary_lines)
|
||||
mqtt_send(mqtt_host, mqtt_port, mqtt_user, mqtt_pass,
|
||||
"sl-firmware", "max", report)
|
||||
|
||||
return motors_spinning
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--port", default="/dev/ttyACM0")
|
||||
ap.add_argument("--baud", type=int, default=460800)
|
||||
ap.add_argument("--duration", type=int, default=10, help="seconds to run drive loop")
|
||||
ap.add_argument("--mqtt-host", default="192.168.87.29")
|
||||
ap.add_argument("--mqtt-port", type=int, default=1883)
|
||||
ap.add_argument("--mqtt-user", default="mqtt_seb")
|
||||
ap.add_argument("--mqtt-pass", default="mqtt_pass")
|
||||
args = ap.parse_args()
|
||||
|
||||
run_test(
|
||||
port=args.port,
|
||||
baud=args.baud,
|
||||
duration=args.duration,
|
||||
mqtt_host=args.mqtt_host,
|
||||
mqtt_port=args.mqtt_port,
|
||||
mqtt_user=args.mqtt_user,
|
||||
mqtt_pass=args.mqtt_pass,
|
||||
)
|
||||
Loading…
x
Reference in New Issue
Block a user