From 1f88835facfc58528993dc06e6bca4078678e38a Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Tue, 21 Apr 2026 13:21:57 -0400 Subject: [PATCH] diag: VESC PING/PONG + CAN bus activity flags in STATUS telemetry MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- esp32s3/balance/main/main.c | 30 ++- esp32s3/balance/main/vesc_can.c | 47 +++- esp32s3/balance/main/vesc_can.h | 11 +- test/orin_can_drive_test.py | 379 ++++++++++++++++++++++++++++++++ 4 files changed, 456 insertions(+), 11 deletions(-) create mode 100644 test/orin_can_drive_test.py diff --git a/esp32s3/balance/main/main.c b/esp32s3/balance/main/main.c index 1e0581f..06e7c2f 100644 --- a/esp32s3/balance/main/main.c +++ b/esp32s3/balance/main/main.c @@ -13,6 +13,7 @@ #include "freertos/queue.h" #include "esp_log.h" #include "esp_timer.h" +#include "driver/twai.h" #include 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(); diff --git a/esp32s3/balance/main/vesc_can.c b/esp32s3/balance/main/vesc_can.c index 866016a..3ce72bb 100644 --- a/esp32s3/balance/main/vesc_can.c +++ b/esp32s3/balance/main/vesc_can.c @@ -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); diff --git a/esp32s3/balance/main/vesc_can.h b/esp32s3/balance/main/vesc_can.h index b5d82c7..8545a0a 100644 --- a/esp32s3/balance/main/vesc_can.h +++ b/esp32s3/balance/main/vesc_can.h @@ -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 */ diff --git a/test/orin_can_drive_test.py b/test/orin_can_drive_test.py new file mode 100644 index 0000000..011136b --- /dev/null +++ b/test/orin_can_drive_test.py @@ -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, + )