embedded-system-v1 / ACE2026 embedded code

apis/can/CAN_API.md

@@ -644,7 +644,7 @@ candump can0 -c -t a
 ### "No messages received"
 
 - **Is the bus up?** Run `ip link show can0` — state should be "UP".
-- **Bitrate match?** All nodes must use 1 Mbit/s. STM32 config: prescaler 6, BS1 11TQ, BS2 2TQ.
+- **Bitrate match?** All nodes must use 1 Mbit/s. STM32 bit-timing depends on PCLK1: with PCLK1 = 84 MHz use prescaler 6, BS1 11TQ, BS2 2TQ (torque-controller, F429); with PCLK1 = 42 MHz use prescaler 3, BS1 10TQ, BS2 3TQ (joint-controller, F446). Both yield a 14-tq, 1 us bit time.
 - **Wiring:** Check CAN_H to CAN_H, CAN_L to CAN_L. No crossover.
 - **Termination:** At least one 120-ohm resistor between CAN_H and CAN_L.
 - **Power:** All nodes must be powered and running.

apis/can/Inc/can_imu.h

@@ -1,8 +1,16 @@
 /*
  * can_imu.h
  *
- * IMU CAN messages: accelerometer and gyroscope data.
- * All values are little-endian int16 scaled by 100.
+ * IMU CAN messages: accelerometer and gyroscope+position data.
+ *
+ * Wire format:
+ *   IMU_ACCEL (DLC 6): ax, ay, az  -> int16 * 100  (m/s^2)
+ *   IMU_GYRO  (DLC 8): gx, gy, gz  -> int16 * 100  (deg/s)
+ *                      motor_pos   -> int16 *  10  (degrees, +/-3276.7)
+ *
+ * The motor position is co-located in the gyro frame (bytes 6-7) so the
+ * IMU sample and the joint angle share the same timestamp on the wire
+ * without adding a third frame per IMU tick.
  */
 
 #ifndef CAN_IMU_H
@@ -18,11 +26,13 @@
 int can_send_imu_accel(uint8_t src_node, float ax, float ay, float az);
 
 /*
- * Send IMU gyroscope data (DLC 6).
- * gx, gy, gz in deg/s. Encoded as int16 * 100.
+ * Send IMU gyroscope data + motor position (DLC 8).
+ * gx, gy, gz in deg/s, encoded as int16 * 100.
+ * motor_position in degrees, encoded as int16 * 10 (use 0 if no motor).
  * Returns 1 on success, 0 on failure.
  */
-int can_send_imu_gyro(uint8_t src_node, float gx, float gy, float gz);
+int can_send_imu_gyro(uint8_t src_node, float gx, float gy, float gz,
+                      float motor_position);
 
 /*
  * Parse an IMU accelerometer frame.
@@ -31,9 +41,11 @@ int can_send_imu_gyro(uint8_t src_node, float gx, float gy, float gz);
 int can_parse_imu_accel(const CanFrame *frame, float *ax, float *ay, float *az);
 
 /*
- * Parse an IMU gyroscope frame.
+ * Parse an IMU gyroscope frame, optionally extracting motor position.
+ * If motor_position is non-NULL: set to bytes 6-7 / 10 if DLC >= 8, else 0.
  * Returns 1 on success, 0 if the frame is not an IMU_GYRO message.
  */
-int can_parse_imu_gyro(const CanFrame *frame, float *gx, float *gy, float *gz);
+int can_parse_imu_gyro(const CanFrame *frame, float *gx, float *gy, float *gz,
+                       float *motor_position);
 
 #endif /* CAN_IMU_H */

apis/can/Src/can_common.c

@@ -47,30 +47,36 @@ int can_common_init(CAN_HandleTypeDef *hcan, uint8_t my_node_id) {
 
     CAN_FilterTypeDef f;
 
-    /* Filter 0: Accept all ESTOP messages (FIFO0) */
+    /* Filter 0: Accept all ESTOP messages (FIFO0).
+     * Mask must include IDE=0 — without it, every extended frame whose
+     * ExtID[28:25] = 0 (which is essentially every VESC packet, since their
+     * packet IDs stay below ~30) accidentally matches the type=0 condition
+     * here and leaks into FIFO0. */
     memset(&f, 0, sizeof(f));
     f.FilterBank           = 0;
     f.FilterMode           = CAN_FILTERMODE_IDMASK;
     f.FilterScale          = CAN_FILTERSCALE_32BIT;
     f.FilterIdHigh         = (CAN_BUILD_ID(CAN_MSG_ESTOP, 0, 0) << 5);
-    f.FilterIdLow          = 0x0000;
-    f.FilterMaskIdHigh     = (0x0780 << 5);  /* match type bits [10:7] only */
-    f.FilterMaskIdLow      = 0x0000;
+    f.FilterIdLow          = 0x0000;        /* IDE bit (bit 2) = 0 -> standard frame */
+    f.FilterMaskIdHigh     = (0x0780 << 5);  /* match type bits [10:7] */
+    f.FilterMaskIdLow      = 0x0004;        /* require IDE = 0 */
     f.FilterFIFOAssignment = CAN_FILTER_FIFO0;
     f.FilterActivation     = ENABLE;
     f.SlaveStartFilterBank = 14;
     if (HAL_CAN_ConfigFilter(hcan, &f) != HAL_OK) return 0;
 
-    /* Filter 1: Accept TORQUE_CMD for my node (FIFO0) */
+    /* Filter 1: Accept TORQUE_CMD for my node (FIFO0).
+     * Same IDE=0 requirement as Filter 0 — without it the filter rejects
+     * VESC frames only by accidental bit alignment, which is fragile. */
     uint16_t torque_id = CAN_BUILD_ID(CAN_MSG_TORQUE_CMD, 0, my_node_id);
     memset(&f, 0, sizeof(f));
     f.FilterBank           = 1;
     f.FilterMode           = CAN_FILTERMODE_IDMASK;
     f.FilterScale          = CAN_FILTERSCALE_32BIT;
     f.FilterIdHigh         = (torque_id << 5);
-    f.FilterIdLow          = 0x0000;
+    f.FilterIdLow          = 0x0000;        /* IDE = 0 -> standard frame */
     f.FilterMaskIdHigh     = (0x078F << 5);  /* match type + dest bits */
-    f.FilterMaskIdLow      = 0x0000;
+    f.FilterMaskIdLow      = 0x0004;        /* require IDE = 0 */
     f.FilterFIFOAssignment = CAN_FILTER_FIFO0;
     f.FilterActivation     = ENABLE;
     f.SlaveStartFilterBank = 14;
@@ -120,6 +126,7 @@ int can_send_std(uint16_t std_id, const uint8_t *data, uint8_t dlc) {
     if (!g_hcan) return 0;
     if (std_id > 0x7FF) return 0;
     if (dlc > 8) return 0;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(g_hcan) == 0) return 0;
 
     CAN_TxHeaderTypeDef hdr;
     memset(&hdr, 0, sizeof(hdr));
@@ -137,6 +144,7 @@ int can_send_ext(uint32_t ext_id, const uint8_t *data, uint8_t dlc) {
     if (!g_hcan) return 0;
     if (ext_id > 0x1FFFFFFFU) return 0;
     if (dlc > 8) return 0;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(g_hcan) == 0) return 0;
 
     CAN_TxHeaderTypeDef hdr;
     memset(&hdr, 0, sizeof(hdr));

apis/can/Src/can_imu.c

@@ -22,18 +22,21 @@ int can_send_imu_accel(uint8_t src_node, float ax, float ay, float az) {
     return can_send_std(id, data, 6);
 }
 
-int can_send_imu_gyro(uint8_t src_node, float gx, float gy, float gz) {
+int can_send_imu_gyro(uint8_t src_node, float gx, float gy, float gz,
+                      float motor_position) {
     uint16_t id = CAN_BUILD_ID(CAN_MSG_IMU_GYRO, src_node, CAN_NODE_PI);
-    int16_t raw_gx = (int16_t)(gx * 100.0f);
-    int16_t raw_gy = (int16_t)(gy * 100.0f);
-    int16_t raw_gz = (int16_t)(gz * 100.0f);
+    int16_t raw_gx  = (int16_t)(gx * 100.0f);
+    int16_t raw_gy  = (int16_t)(gy * 100.0f);
+    int16_t raw_gz  = (int16_t)(gz * 100.0f);
+    int16_t raw_pos = (int16_t)(motor_position * 10.0f);
 
-    uint8_t data[6];
-    memcpy(&data[0], &raw_gx, 2);
-    memcpy(&data[2], &raw_gy, 2);
-    memcpy(&data[4], &raw_gz, 2);
+    uint8_t data[8];
+    memcpy(&data[0], &raw_gx,  2);
+    memcpy(&data[2], &raw_gy,  2);
+    memcpy(&data[4], &raw_gz,  2);
+    memcpy(&data[6], &raw_pos, 2);
 
-    return can_send_std(id, data, 6);
+    return can_send_std(id, data, 8);
 }
 
 int can_parse_imu_accel(const CanFrame *frame, float *ax, float *ay, float *az) {
@@ -49,7 +52,8 @@ int can_parse_imu_accel(const CanFrame *frame, float *ax, float *ay, float *az)
     return 1;
 }
 
-int can_parse_imu_gyro(const CanFrame *frame, float *gx, float *gy, float *gz) {
+int can_parse_imu_gyro(const CanFrame *frame, float *gx, float *gy, float *gz,
+                       float *motor_position) {
     if (frame->is_extended) return 0;
     if (can_get_msg_type((uint16_t)frame->id) != CAN_MSG_IMU_GYRO) return 0;
     if (frame->dlc < 6) return 0;
@@ -59,5 +63,13 @@ int can_parse_imu_gyro(const CanFrame *frame, float *gx, float *gy, float *gz) {
     memcpy(&raw, &frame->data[2], 2); *gy = raw / 100.0f;
     memcpy(&raw, &frame->data[4], 2); *gz = raw / 100.0f;
 
+    if (motor_position != NULL) {
+        if (frame->dlc >= 8) {
+            memcpy(&raw, &frame->data[6], 2); *motor_position = raw / 10.0f;
+        } else {
+            *motor_position = 0.0f;
+        }
+    }
+
     return 1;
 }

apis/can/python/can_imu.py

@@ -1,14 +1,25 @@
 """
 can_imu.py
 
-IMU CAN messages: accelerometer and gyroscope data.
+IMU CAN messages: accelerometer and gyroscope+position data.
 Mirrors the C API in can_imu.h.
-All values are little-endian int16 scaled by 100.
+
+Wire format:
+  IMU_ACCEL (DLC 6): ax, ay, az -> int16 * 100  (m/s^2)
+  IMU_GYRO  (DLC 8): gx, gy, gz -> int16 * 100  (deg/s)
+                     motor_pos  -> int16 *  10  (degrees)
+
+The motor position is co-located in the gyro frame so the IMU sample and
+the joint angle share the same on-wire timestamp.
 """
 
 import struct
 import can
-from . import can_common
+
+try:
+    from . import can_common
+except ImportError:
+    import can_common
 
 
 def send_imu_accel(bus: can.Bus, src_node: int, ax: float, ay: float, az: float):
@@ -19,10 +30,13 @@ def send_imu_accel(bus: can.Bus, src_node: int, ax: float, ay: float, az: float)
     bus.send(msg)
 
 
-def send_imu_gyro(bus: can.Bus, src_node: int, gx: float, gy: float, gz: float):
-    """Send IMU gyroscope data. gx, gy, gz in deg/s."""
+def send_imu_gyro(bus: can.Bus, src_node: int, gx: float, gy: float, gz: float,
+                  motor_position: float = 0.0):
+    """Send IMU gyroscope data + motor position. gx,gy,gz in deg/s, position in deg."""
     can_id = can_common.build_can_id(can_common.MSG_IMU_GYRO, src_node, can_common.NODE_PI)
-    data = struct.pack('<hhh', int(gx * 100), int(gy * 100), int(gz * 100))
+    data = struct.pack('<hhhh',
+                       int(gx * 100), int(gy * 100), int(gz * 100),
+                       int(motor_position * 10))
     msg = can.Message(arbitration_id=can_id, data=data, is_extended_id=False)
     bus.send(msg)
 
@@ -34,6 +48,17 @@ def parse_imu_accel(msg: can.Message) -> tuple[float, float, float]:
 
 
 def parse_imu_gyro(msg: can.Message) -> tuple[float, float, float]:
-    """Parse IMU gyroscope data. Returns (gx, gy, gz) in deg/s."""
+    """Parse IMU gyroscope data only. Returns (gx, gy, gz) in deg/s.
+    Backward-compatible: ignores the motor_position byte if present."""
     gx, gy, gz = struct.unpack('<hhh', msg.data[0:6])
     return gx / 100.0, gy / 100.0, gz / 100.0
+
+
+def parse_imu_gyro_pos(msg: can.Message) -> tuple[float, float, float, float | None]:
+    """Parse IMU gyroscope + motor position. Returns (gx, gy, gz, position).
+    position is None if the frame is the legacy DLC 6 form."""
+    if len(msg.data) >= 8:
+        gx, gy, gz, raw_pos = struct.unpack('<hhhh', msg.data[0:8])
+        return gx / 100.0, gy / 100.0, gz / 100.0, raw_pos / 10.0
+    gx, gy, gz = struct.unpack('<hhh', msg.data[0:6])
+    return gx / 100.0, gy / 100.0, gz / 100.0, None