Vcu mode set over can

BEVO/cand/main.rs

@@ -1,7 +1,7 @@
 use anyhow::Result;
 use prost::Message;
 use std::collections::HashMap;
-use std::io::{BufRead, BufReader, Write};
+use std::io::{BufRead, BufReader, Read, Write};
 use std::net::{TcpListener, UdpSocket};
 use std::os::unix::net::{UnixListener, UnixStream};
 use std::path::Path;
@@ -16,14 +16,18 @@ use sensor_proto::config::PacketConfig;
 use sensor_proto::generated_mapping;
 
 #[cfg(target_os = "linux")]
-use socketcan::{CanSocket, EmbeddedFrame, Id, Socket};
+use socketcan::{CanFrame, CanSocket, EmbeddedFrame, Id, Socket, StandardId};
 
 //use std::process::Command;
 
 const MOCK_ADDR_0: &str = "127.0.0.1:5005";
 const MOCK_ADDR_1: &str = "127.0.0.1:5006";
 const SOCKET_PATH: &str = "/tmp/BEVO_cand.sock";
 const PUBLISHD_SOCKET_PATH: &str = "/tmp/BEVO_cand_publishd.sock";
+/// CAN transmit request socket. dashd connects here and sends frame requests;
+/// cand owns the only write socket on the critical bus, so it stays the sole
+/// CAN writer. Wire format below in `can_tx_listener_loop`.
+const CAND_TX_SOCKET_PATH: &str = "/tmp/BEVO_cand_tx.sock";
 const STARTUP_SEMAPHORE_PATH: &str = "/tmp/BEVO_publishd_ready";
 //const OTA_SEMAPHORE_PATH: &str = "/tmp/BEVO_ota_request"; 
 const CAN_INTERFACE_0: &str = "can0";
@@ -115,6 +119,16 @@ fn main() -> Result<()> {
                 eprintln!("[CAND-CAN-1] Error: {:?}", e);
             }
         });
+
+        // CAN transmit path on the critical bus (can0) — where the VCU listens.
+        // Single writer thread owns the only TX socket; dashd feeds it frame
+        // requests over CAND_TX_SOCKET_PATH (e.g. the 0x029 VCU Mode Command).
+        let can_interface_tx = can_interface_0.clone();
+        thread::spawn(move || {
+            if let Err(e) = can_tx_listener_loop(can_interface_tx) {
+                eprintln!("[CAND-TX] Error: {:?}", e);
+            }
+        });
     }
 
     let processing_sensor_data = Arc::clone(&sensor_data_cache);
@@ -330,6 +344,60 @@ fn can_socket_reader_loop(raw_tx: Sender<RawCanMessage>, can_interface: String)
     }
 }
 
+// Single-writer CAN transmit path. dashd connects to CAND_TX_SOCKET_PATH and
+// sends frame requests; we own the only write socket on `can_interface` so cand
+// stays the sole CAN writer. One write socket is opened up front and reused.
+//
+// Wire format per request (13 bytes, fixed):
+//   [0..4]  CAN id   (u32, little-endian; standard 11-bit)
+//   [4]     dlc      (u8, clamped 0..=8)
+//   [5..13] data     (8 bytes; only the first `dlc` are transmitted)
+// A client may send multiple back-to-back requests on one connection.
+#[cfg(target_os = "linux")]
+fn can_tx_listener_loop(can_interface: String) -> Result<()> {
+    let _ = std::fs::remove_file(CAND_TX_SOCKET_PATH);
+    let listener = UnixListener::bind(CAND_TX_SOCKET_PATH)?;
+    let socket = CanSocket::open(&can_interface)?;
+    println!(
+        "[CAND-TX] CAN transmit listener on {} -> {}",
+        CAND_TX_SOCKET_PATH, can_interface
+    );
+    for stream in listener.incoming() {
+        let mut stream = match stream {
+            Ok(s) => s,
+            Err(_) => continue,
+        };
+        let mut buf = [0u8; 13];
+        // read_exact returns Err at EOF / short read -> connection done.
+        while stream.read_exact(&mut buf).is_ok() {
+            let id = u32::from_le_bytes([buf[0], buf[1], buf[2], buf[3]]);
+            let dlc = (buf[4] as usize).min(8);
+            let data = &buf[5..5 + dlc];
+            // Standard CAN ids are 11-bit (<= 0x7FF). Range-check BEFORE casting:
+            // `id as u16` would silently truncate a >16-bit / extended id
+            // (e.g. 0x10005 -> 0x5) and transmit to the wrong node.
+            let sid = match (id <= 0x7FF).then(|| StandardId::new(id as u16)).flatten() {
+                Some(s) => s,
+                None => {
+                    eprintln!("[CAND-TX] rejecting non-standard CAN id {:#x}", id);
+                    continue;
+                }
+            };
+            match CanFrame::new(sid, data) {
+                Some(frame) => match socket.write_frame(&frame) {
+                    Ok(()) => println!(
+                        "[CAND-TX] sent CAN {:#05x} dlc={} data={:02x?}",
+                        id, dlc, data
+                    ),
+                    Err(e) => eprintln!("[CAND-TX] write_frame failed: {:?}", e),
+                },
+                None => eprintln!("[CAND-TX] could not build frame (dlc {})", dlc),
+            }
+        }
+    }
+    Ok(())
+}
+
 // reads CAN frames from the mock UDP socket used for local testing.
 
 #[cfg(target_os = "linux")]

BEVO/dashd/main.rs

@@ -2,7 +2,7 @@ use anyhow::Result;
 use prost::Message;
 use rumqttc::{Client, Event, Incoming, MqttOptions, QoS};
 use serde::Serialize;
-use std::io::Read;
+use std::io::{Read, Write};
 use std::net::TcpListener;
 use std::os::unix::net::UnixStream;
 use std::sync::{Arc, Mutex};
@@ -16,6 +16,11 @@ use sensor_proto::proto::orion::OrionSensorData;
 // ---------------------------------------------------------------------------
 
 const SOCKET_PATH: &str = "/tmp/BEVO_cand.sock";
+/// cand's CAN transmit request socket — dashd relays trackside commands (the
+/// 0x029 VCU Mode Command) here; cand owns the actual CAN write.
+const CAND_TX_SOCKET_PATH: &str = "/tmp/BEVO_cand_tx.sock";
+/// CAN id of the Pi->VCU "VCU Mode Command" packet (see can_packets.csv 0x029).
+const VCU_MODE_COMMAND_ID: u32 = 0x029;
 const WS_PORT: u16 = 8001;
 const WS_SEND_HZ: u64 = 30;
 
@@ -964,6 +969,21 @@ fn ws_server_loop(state: Arc<Mutex<DashState>>) {
 }
 
 // ---------------------------------------------------------------------------
+// Relay a VCU mode command to cand's CAN TX socket. Builds the 13-byte request
+// cand::can_tx_listener_loop expects: u32 LE CAN id, u8 dlc, 8 data bytes. The
+// 0x029 packet is 8 bytes; byte0 = event_mode, bytes 1-7 reserved (left zero).
+// dashd doesn't interpret the mode — it just bridges trackside MQTT -> CAN.
+fn send_can_mode_command(event_mode: u8) -> std::io::Result<()> {
+    let mut req = [0u8; 13];
+    req[0..4].copy_from_slice(&VCU_MODE_COMMAND_ID.to_le_bytes());
+    req[4] = 8; // DLC
+    req[5] = event_mode; // data[0]; data[1..8] reserved
+    let mut stream = UnixStream::connect(CAND_TX_SOCKET_PATH)?;
+    stream.write_all(&req)?;
+    stream.flush()?;
+    Ok(())
+}
+
 // Thread 3: MQTT subscriber — receives off-car computed values
 // ---------------------------------------------------------------------------
 
@@ -1127,6 +1147,35 @@ fn mqtt_subscriber_loop(state: Arc<Mutex<DashState>>) {
                         continue;
                     }
 
+                    // VCU mode command from trackside (lhre/dash/eventMode).
+                    // Relay it to cand as the 0x029 CAN frame; dashd doesn't act
+                    // on it. The VCU re-broadcasts its active event_mode in 0x1C7,
+                    // which trackside already reads as confirmation (closed loop).
+                    if field == "eventMode" {
+                        match payload_str.trim().parse::<u8>() {
+                            Ok(mode) => match send_can_mode_command(mode) {
+                                Ok(()) => {
+                                    println!("[DASHD] relayed VCU mode command: event_mode={}", mode);
+                                    let _ = client.publish(
+                                        format!("{}ack/eventMode", MQTT_TOPIC_PREFIX),
+                                        QoS::AtMostOnce,
+                                        false,
+                                        payload_str.as_bytes().to_vec(),
+                                    );
+                                }
+                                Err(e) => eprintln!(
+                                    "[DASHD] eventMode relay to cand TX failed (is cand up?): {}",
+                                    e
+                                ),
+                            },
+                            Err(_) => eprintln!(
+                                "[DASHD] MQTT bad eventMode payload: {:?}",
+                                payload_str
+                            ),
+                        }
+                        continue;
+                    }
+
                     // We subscribe to lhre/dash/# and therefore hear our own
                     // publishes echoed back (state @2Hz + ack/* retained). Those
                     // aren't numeric inputs — skip them silently so they don't

BEVO/nonhermetic/assets/can.json

@@ -4793,6 +4793,31 @@
         "bus": "Critical",
         "bytes": []
     },
+    {
+        "packet_id": 41,
+        "packet_name": "VCU Mode Command",
+        "from": [
+            "Pi"
+        ],
+        "to": [
+            "VCU"
+        ],
+        "data_length": 8,
+        "frequency_ms": null,
+        "frequency": null,
+        "quantity": 1,
+        "bus": "Critical",
+        "bytes": [
+            {
+                "index": 0,
+                "start_byte": 0,
+                "name": "Event Mode",
+                "length": 1,
+                "conv_type": "uint8",
+                "precision": 1.0
+            }
+        ]
+    },
     {
         "packet_id": 80,
         "packet_name": "HVC Charger Command",

VCU/firmware/Core/Src/app_freertos.c

@@ -120,6 +120,7 @@ void StartControlTask(void *argument);
 static float steering_adc_to_sensor_voltage(float adc_voltage_v);
 static float steering_sensor_voltage_to_percent(float sensor_voltage_v);
 static float steering_sensor_voltage_to_angle_deg(float sensor_voltage_v);
+static const vcu_parameters_t *vcu_params_for_event_mode(uint8_t event_mode);
 
 /* USER CODE END FunctionPrototypes */
 
@@ -241,6 +242,18 @@ static float steering_sensor_voltage_to_percent(float sensor_voltage_v) {
   return pct;
 }
 
+// Maps a trackside-requested event mode (1=accel, 2=skid, 3=autox, 4=endur)
+// to its params table. 0/unrecognized falls back to the firmware default.
+static const vcu_parameters_t *vcu_params_for_event_mode(uint8_t event_mode) {
+  switch (event_mode) {
+    case 1: return &acceleration_params;
+    case 2: return &skidpad_params;
+    case 3: return &autocross_params;
+    case 4: return &endurance_params;
+    default: return &SELECTED_PARAMS;
+  }
+}
+
 // SystemTask: one-time initialization (USB, logging, DFU, ADC DMA, CAN) -----
 void StartSystemTask(void *argument) {
   // USB + logging
@@ -376,6 +389,18 @@ void StartControlTask(void *argument) {
     // Run control model
     vcu_model_step(&ctx, &in, &out, dt_ms);
 
+    // While parked, allow trackside (0x029 VCU Mode Command) to swap the
+    // active params table on the fly. Only re-init when the requested mode
+    // actually changes the active params, so this is safe to check every loop.
+    if (out.prndl_state == PRNDL_PARK) {
+      const vcu_parameters_t *requested_params =
+          vcu_params_for_event_mode(vcu_can_get_requested_event_mode());
+      if (requested_params->event_mode != s_params.event_mode) {
+        s_params = *requested_params;
+        vcu_model_init(&ctx, &s_params);
+      }
+    }
+
     vcu_can_set_model_inputs(&in);
     vcu_can_set_model_outputs(&out);
     vcu_can_set_steering_angle_deg(steering_angle_deg);

VCU/firmware/vcu/vcu_can.c

@@ -54,6 +54,9 @@ static can_receive_message_t *inverter_voltage_mailbox_handle = NULL;
 
 static msg_inverter_faults_t inverter_faults_mailbox = {0};
 static can_receive_message_t *inverter_faults_mailbox_handle = NULL;
+
+static msg_vcu_mode_command_t vcu_mode_command_mailbox = {0};
+static can_receive_message_t *vcu_mode_command_mailbox_handle = NULL;
 // #define DUI_R2D_STATUS_TIMEOUT_MS 1000u
 
 // static bool dui_r2d_timed_out_logged = false;
@@ -328,6 +331,10 @@ void vcu_can_set_event_mode(uint8_t event_mode) {
   vcu_state_mailbox.event_mode = event_mode;
 }
 
+uint8_t vcu_can_get_requested_event_mode(void) {
+  return vcu_mode_command_mailbox.event_mode;
+}
+
 static float compute_brake_bias_pct(const vcu_outputs_t *out) {
   static float remembered_brake_bias = 0.0f;
   float total = out->bse1_psi + out->bse2_psi;
@@ -551,4 +558,12 @@ void vcu_can_add_receive_handlers(void) {
                                    inverter_faults_mailbox_handle);
   log_printf(LOG_INFO,
              "[VCU] CAN receive handler for inverter faults registered\n");
+
+  vcu_mode_command_mailbox_handle = can_get_receive_message_handle(
+      &vcu_mode_command_mailbox, VCU_MODE_COMMAND_ID,
+      (CAN_unpack_message_fn)unpack_vcu_mode_command);
+  can_rtos_register_receive_packet(&critical_bus,
+                                   vcu_mode_command_mailbox_handle);
+  log_printf(LOG_INFO,
+             "[VCU] CAN receive handler for VCU mode command registered\n");
 }

VCU/firmware/vcu/vcu_can.h

@@ -30,6 +30,10 @@ void vcu_can_set_model_outputs(const vcu_outputs_t *out);
 void vcu_can_set_steering_angle_deg(float steering_angle_deg);
 void vcu_can_set_event_mode(uint8_t event_mode);
 
+// Latest event mode requested by trackside via the VCU Mode Command (0x029).
+// Returns 0 if no command has been received (i.e. use the firmware default).
+uint8_t vcu_can_get_requested_event_mode(void);
+
 bool is_drive_switch_pressed(void);
 
 bool hvc_tractive_ready(void);