[SOFT 421] Transmit motor temp (#408)

This adds transmitting motor temperature over CAN from MCI.

codegen/can_messages.asciipb

@@ -335,7 +335,7 @@ msg {
   }
 }
 
-msg { 
+msg {
   id: 37
   source: MOTOR_CONTROLLER
   target: "TELEMETRY"
@@ -352,16 +352,31 @@ msg {
   id: 38
   source: MOTOR_CONTROLLER
   target: "TELEMETRY"
-  msg_name: "motor temps"
+  msg_name: "motor sink temps"
   can_data {
-    u32 {
+    u16 {
       field_name_1: "motor_temp_l"
-      field_name_2: "motor_temp_r"
+      field_name_2: "sink_temp_l"
+      field_name_3: "motor_temp_r"
+      field_name_4: "sink_temp_r"
+    }
+  }
+}
+
+msg {
+  id: 39
+  source: MOTOR_CONTROLLER
+  target: "TELEMETRY"
+  msg_name: "dsp board temps"
+  can_data {
+    u32 {
+      field_name_1: "dsp_board_temp_l"
+      field_name_2: "dsp_board_temp_r"
     }
   }
 }
 
-# IDs: 39-40 Reserved
+# ID 40: Reserved
 
 msg {
   id: 41

libraries/codegen-tooling/inc/can_msg_defs.h

@@ -51,7 +51,8 @@ typedef enum {
   SYSTEM_CAN_MESSAGE_MOTOR_CONTROLLER_VC = 35,
   SYSTEM_CAN_MESSAGE_MOTOR_VELOCITY = 36,
   SYSTEM_CAN_MESSAGE_MOTOR_STATUS = 37,
-  SYSTEM_CAN_MESSAGE_MOTOR_TEMPS = 38,
+  SYSTEM_CAN_MESSAGE_MOTOR_SINK_TEMPS = 38,
+  SYSTEM_CAN_MESSAGE_DSP_BOARD_TEMPS = 39,
   SYSTEM_CAN_MESSAGE_CRUISE_CONTROL_COMMAND = 41,
   SYSTEM_CAN_MESSAGE_AUX_MEAS_MAIN_VOLTAGE = 42,
   SYSTEM_CAN_MESSAGE_DCDC_MEAS_MAIN_CURRENT = 43,
@@ -73,5 +74,5 @@ typedef enum {
   SYSTEM_CAN_MESSAGE_REAR_PD_FAULT = 61,
   SYSTEM_CAN_MESSAGE_FRONT_PD_FAULT = 62,
   SYSTEM_CAN_MESSAGE_BABYDRIVER = 63,
-  NUM_SYSTEM_CAN_MESSAGES = 48
+  NUM_SYSTEM_CAN_MESSAGES = 49
 } SystemCanMessage;

libraries/codegen-tooling/inc/can_pack.h

@@ -132,9 +132,16 @@
                     SYSTEM_CAN_MESSAGE_MOTOR_STATUS, 8, (motor_status_l_u32),  \
                     (motor_status_r_u32))
 
-#define CAN_PACK_MOTOR_TEMPS(msg_ptr, motor_temp_l_u32, motor_temp_r_u32)                          \
-  can_pack_impl_u32((msg_ptr), SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER, SYSTEM_CAN_MESSAGE_MOTOR_TEMPS, \
-                    8, (motor_temp_l_u32), (motor_temp_r_u32))
+#define CAN_PACK_MOTOR_SINK_TEMPS(msg_ptr, motor_temp_l_u16, sink_temp_l_u16, motor_temp_r_u16,    \
+                                  sink_temp_r_u16)                                                 \
+  can_pack_impl_u16((msg_ptr), SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER,                                 \
+                    SYSTEM_CAN_MESSAGE_MOTOR_SINK_TEMPS, 8, (motor_temp_l_u16), (sink_temp_l_u16), \
+                    (motor_temp_r_u16), (sink_temp_r_u16))
+
+#define CAN_PACK_DSP_BOARD_TEMPS(msg_ptr, dsp_board_temp_l_u32, dsp_board_temp_r_u32) \
+  can_pack_impl_u32((msg_ptr), SYSTEM_CAN_DEVICE_MOTOR_CONTROLLER,                    \
+                    SYSTEM_CAN_MESSAGE_DSP_BOARD_TEMPS, 8, (dsp_board_temp_l_u32),    \
+                    (dsp_board_temp_r_u32))
 
 #define CAN_PACK_CRUISE_CONTROL_COMMAND(msg_ptr, command_u8)                               \
   can_pack_impl_u8(                                                                        \

libraries/codegen-tooling/inc/can_transmit.h

@@ -217,12 +217,22 @@
     status;                                                                  \
   })
 
-#define CAN_TRANSMIT_MOTOR_TEMPS(motor_temp_l_u32, motor_temp_r_u32)    \
-  ({                                                                    \
-    CanMessage msg = { 0 };                                             \
-    CAN_PACK_MOTOR_TEMPS(&msg, (motor_temp_l_u32), (motor_temp_r_u32)); \
-    StatusCode status = can_transmit(&msg, NULL);                       \
-    status;                                                             \
+#define CAN_TRANSMIT_MOTOR_SINK_TEMPS(motor_temp_l_u16, sink_temp_l_u16, motor_temp_r_u16,     \
+                                      sink_temp_r_u16)                                         \
+  ({                                                                                           \
+    CanMessage msg = { 0 };                                                                    \
+    CAN_PACK_MOTOR_SINK_TEMPS(&msg, (motor_temp_l_u16), (sink_temp_l_u16), (motor_temp_r_u16), \
+                              (sink_temp_r_u16));                                              \
+    StatusCode status = can_transmit(&msg, NULL);                                              \
+    status;                                                                                    \
+  })
+
+#define CAN_TRANSMIT_DSP_BOARD_TEMPS(dsp_board_temp_l_u32, dsp_board_temp_r_u32)    \
+  ({                                                                                \
+    CanMessage msg = { 0 };                                                         \
+    CAN_PACK_DSP_BOARD_TEMPS(&msg, (dsp_board_temp_l_u32), (dsp_board_temp_r_u32)); \
+    StatusCode status = can_transmit(&msg, NULL);                                   \
+    status;                                                                         \
   })
 
 #define CAN_TRANSMIT_CRUISE_CONTROL_COMMAND(command_u8)  \

libraries/codegen-tooling/inc/can_unpack.h

@@ -102,8 +102,13 @@
 #define CAN_UNPACK_MOTOR_STATUS(msg_ptr, motor_status_l_u32_ptr, motor_status_r_u32_ptr) \
   can_unpack_impl_u32((msg_ptr), 8, (motor_status_l_u32_ptr), (motor_status_r_u32_ptr))
 
-#define CAN_UNPACK_MOTOR_TEMPS(msg_ptr, motor_temp_l_u32_ptr, motor_temp_r_u32_ptr) \
-  can_unpack_impl_u32((msg_ptr), 8, (motor_temp_l_u32_ptr), (motor_temp_r_u32_ptr))
+#define CAN_UNPACK_MOTOR_SINK_TEMPS(msg_ptr, motor_temp_l_u16_ptr, sink_temp_l_u16_ptr, \
+                                    motor_temp_r_u16_ptr, sink_temp_r_u16_ptr)          \
+  can_unpack_impl_u16((msg_ptr), 8, (motor_temp_l_u16_ptr), (sink_temp_l_u16_ptr),      \
+                      (motor_temp_r_u16_ptr), (sink_temp_r_u16_ptr))
+
+#define CAN_UNPACK_DSP_BOARD_TEMPS(msg_ptr, dsp_board_temp_l_u32_ptr, dsp_board_temp_r_u32_ptr) \
+  can_unpack_impl_u32((msg_ptr), 8, (dsp_board_temp_l_u32_ptr), (dsp_board_temp_r_u32_ptr))
 
 #define CAN_UNPACK_CRUISE_CONTROL_COMMAND(msg_ptr, command_u8_ptr)                                 \
   can_unpack_impl_u8((msg_ptr), 1, (command_u8_ptr), CAN_UNPACK_IMPL_EMPTY, CAN_UNPACK_IMPL_EMPTY, \

projects/mci/inc/mci_broadcast.h

@@ -16,7 +16,7 @@ typedef enum {
   MOTOR_CONTROLLER_BROADCAST_STATUS = 0,
   MOTOR_CONTROLLER_BROADCAST_BUS,
   MOTOR_CONTROLLER_BROADCAST_VELOCITY,
-  MOTOR_CONTROLLER_BROADCAST_MOTOR_TEMP,
+  MOTOR_CONTROLLER_BROADCAST_SINK_MOTOR_TEMP,
   MOTOR_CONTROLLER_BROADCAST_DSP_TEMP,
   NUM_MOTOR_CONTROLLER_BROADCAST_MEASUREMENTS,
 } MotorControllerBroadcastMeasurement;
@@ -36,6 +36,8 @@ typedef struct MotorControllerMeasurements {
   WaveSculptorBusMeasurement bus_measurements[NUM_MOTOR_CONTROLLERS];
   float vehicle_velocity[NUM_MOTOR_CONTROLLERS];
   uint32_t status[NUM_MOTOR_CONTROLLERS];
+  WaveSculptorSinkMotorTempMeasurement sink_motor_measurements[NUM_MOTOR_CONTROLLERS];
+  WaveSculptorDspTempMeasurement dsp_measurements[NUM_MOTOR_CONTROLLERS];
 } MotorControllerMeasurements;
 
 typedef struct MotorControllerBroadcastSettings {
@@ -48,6 +50,8 @@ typedef struct MotorControllerBroadcastStorage {
   uint8_t bus_rx_bitset;
   uint8_t velocity_rx_bitset;
   uint8_t status_rx_bitset;
+  uint8_t motor_sink_rx_bitset;
+  uint8_t dsp_rx_bitset;
   MotorControllerMeasurements measurements;
   MotorCanDeviceId ids[NUM_MOTOR_CONTROLLERS];
   // What we're currently filtering for

projects/mci/inc/motor_can.h

@@ -46,10 +46,16 @@ typedef uint32_t MotorCanDeviceId;
 typedef uint32_t MotorCanFrameId;
 #define MOTOR_CAN_LEFT_DRIVE_COMMAND_FRAME_ID (0x501u)
 #define MOTOR_CAN_RIGHT_DRIVE_COMMAND_FRAME_ID (0x41u)
+
 #define MOTOR_CAN_LEFT_BUS_MEASUREMENT_FRAME_ID (0x402u)
 #define MOTOR_CAN_LEFT_VELOCITY_MEASUREMENT_FRAME_ID (0x403u)
+#define MOTOR_CAN_LEFT_SINK_MOTOR_TEMPERATURE_FRAME_ID (0x40Bu)
+#define MOTOR_CAN_LEFT_DSP_TEMPERATURE_FRAME_ID (0x40Cu)
+
 #define MOTOR_CAN_RIGHT_BUS_MEASUREMENT_FRAME_ID (0x202u)
 #define MOTOR_CAN_RIGHT_VELOCITY_MEASUREMENT_FRAME_ID (0x203u)
+#define MOTOR_CAN_RIGHT_SINK_MOTOR_TEMPERATURE_FRAME_ID (0x20Bu)
+#define MOTOR_CAN_RIGHT_DSP_TEMPERATURE_FRAME_ID (0x20Cu)
 
 // Frame lengths in bytes.
 #define MOTOR_CAN_DRIVE_COMMAND_LENGTH (8u)

projects/mci/src/mci_broadcast.c

@@ -25,7 +25,7 @@ static const uint32_t s_offset_lookup[NUM_MOTOR_CONTROLLER_BROADCAST_MEASUREMENT
   [MOTOR_CONTROLLER_BROADCAST_STATUS] = WAVESCULPTOR_MEASUREMENT_ID_STATUS,
   [MOTOR_CONTROLLER_BROADCAST_BUS] = WAVESCULPTOR_MEASUREMENT_ID_BUS,
   [MOTOR_CONTROLLER_BROADCAST_VELOCITY] = WAVESCULPTOR_MEASUREMENT_ID_VELOCITY,
-  [MOTOR_CONTROLLER_BROADCAST_MOTOR_TEMP] = WAVESCULPTOR_MEASUREMENT_ID_SINK_MOTOR_TEMPERATURE,
+  [MOTOR_CONTROLLER_BROADCAST_SINK_MOTOR_TEMP] = WAVESCULPTOR_MEASUREMENT_ID_SINK_MOTOR_TEMPERATURE,
   [MOTOR_CONTROLLER_BROADCAST_DSP_TEMP] = WAVESCULPTOR_MEASUREMENT_ID_DSP_BOARD_TEMPERATURE,
 };
 
@@ -36,12 +36,14 @@ static const uint32_t s_offset_lookup[NUM_MOTOR_CONTROLLER_BROADCAST_MEASUREMENT
 #define NUM_MOTOR_CONTROLLERS 1
 #endif
 
+// Velocity
 static void prv_broadcast_speed(MotorControllerBroadcastStorage *storage) {
   float *measurements = storage->measurements.vehicle_velocity;
   CAN_TRANSMIT_MOTOR_VELOCITY((uint16_t)measurements[LEFT_MOTOR_CONTROLLER],
                               (uint16_t)measurements[RIGHT_MOTOR_CONTROLLER]);
 }
 
+// Bus current and voltage
 static void prv_broadcast_bus_measurement(MotorControllerBroadcastStorage *storage) {
   WaveSculptorBusMeasurement *measurements = storage->measurements.bus_measurements;
   CAN_TRANSMIT_MOTOR_CONTROLLER_VC((uint16_t)measurements[LEFT_MOTOR_CONTROLLER].bus_voltage_v,
@@ -50,12 +52,30 @@ static void prv_broadcast_bus_measurement(MotorControllerBroadcastStorage *stora
                                    (uint16_t)measurements[RIGHT_MOTOR_CONTROLLER].bus_current_a);
 }
 
+// Status
 static void prv_broadcast_status(MotorControllerBroadcastStorage *storage) {
   uint32_t *measurements = storage->measurements.status;
   CAN_TRANSMIT_MOTOR_STATUS(measurements[LEFT_MOTOR_CONTROLLER],
                             measurements[RIGHT_MOTOR_CONTROLLER]);
 }
 
+// Motor and heat sink temperatures
+static void prv_broadcast_motor_sink_temp(MotorControllerBroadcastStorage *storage) {
+  WaveSculptorSinkMotorTempMeasurement *measurements =
+      storage->measurements.sink_motor_measurements;
+  CAN_TRANSMIT_MOTOR_SINK_TEMPS((uint16_t)measurements[LEFT_MOTOR_CONTROLLER].motor_temp_c,
+                                (uint16_t)measurements[LEFT_MOTOR_CONTROLLER].heatsink_temp_c,
+                                (uint16_t)measurements[RIGHT_MOTOR_CONTROLLER].motor_temp_c,
+                                (uint16_t)measurements[RIGHT_MOTOR_CONTROLLER].heatsink_temp_c);
+}
+
+// CPU/DSP temperature
+static void prv_broadcast_dsp_temp(MotorControllerBroadcastStorage *storage) {
+  WaveSculptorDspTempMeasurement *measurements = storage->measurements.dsp_measurements;
+  CAN_TRANSMIT_DSP_BOARD_TEMPS((uint32_t)measurements[LEFT_MOTOR_CONTROLLER].dsp_temp_c,
+                               (uint32_t)measurements[RIGHT_MOTOR_CONTROLLER].dsp_temp_c);
+}
+
 static void prv_handle_status_rx(const GenericCanMsg *msg, void *context) {
   LOG_DEBUG("Received status message\n");
   MotorControllerBroadcastStorage *storage = context;
@@ -117,22 +137,47 @@ static void prv_handle_bus_measurement_rx(const GenericCanMsg *msg, void *contex
   }
 }
 
-static void prv_handle_motor_temp_rx(const GenericCanMsg *msg, void *context) {
-  // TODO(SOFT-421): send this over CAN and store it
-  LOG_DEBUG("Received motor temp message\n");
+static void prv_handle_motor_sink_temp_rx(const GenericCanMsg *msg, void *context) {
+  LOG_DEBUG("Received motor temperature message\n");
+  MotorControllerBroadcastStorage *storage = context;
+  WaveSculptorSinkMotorTempMeasurement *measurements =
+      storage->measurements.sink_motor_measurements;
+
+  WaveSculptorCanId can_id = { .raw = msg->id };
+  WaveSculptorCanData can_data = { .raw = msg->data };
+  for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) {
+    if (can_id.device_id == storage->ids[motor_id]) {
+      bool disabled = critical_section_start();
+      measurements[motor_id] = can_data.sink_motor_temp_measurement;
+      storage->motor_sink_rx_bitset |= 1 << motor_id;
+      critical_section_end(disabled);
+    }
+  }
 }
 
 static void prv_handle_dsp_temp_rx(const GenericCanMsg *msg, void *context) {
-  // TODO(SOFT-421): send this over CAN and store it
-  LOG_DEBUG("Received DSP temp message\n");
+  LOG_DEBUG("Received DSP temperature message\n");
+  MotorControllerBroadcastStorage *storage = context;
+  WaveSculptorDspTempMeasurement *measurements = storage->measurements.dsp_measurements;
+
+  WaveSculptorCanId can_id = { .raw = msg->id };
+  WaveSculptorCanData can_data = { .raw = msg->data };
+  for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) {
+    if (can_id.device_id == storage->ids[motor_id]) {
+      bool disabled = critical_section_start();
+      measurements[motor_id] = can_data.dsp_temp_measurement;
+      storage->dsp_rx_bitset |= 1 << motor_id;
+      critical_section_end(disabled);
+    }
+  }
 }
 
 static MotorControllerMeasurementCallback
     s_cb_storage[NUM_MOTOR_CONTROLLER_BROADCAST_MEASUREMENTS] = {
       [MOTOR_CONTROLLER_BROADCAST_STATUS] = prv_handle_status_rx,
       [MOTOR_CONTROLLER_BROADCAST_BUS] = prv_handle_bus_measurement_rx,
       [MOTOR_CONTROLLER_BROADCAST_VELOCITY] = prv_handle_speed_rx,
-      [MOTOR_CONTROLLER_BROADCAST_MOTOR_TEMP] = prv_handle_motor_temp_rx,
+      [MOTOR_CONTROLLER_BROADCAST_SINK_MOTOR_TEMP] = prv_handle_motor_sink_temp_rx,
       [MOTOR_CONTROLLER_BROADCAST_DSP_TEMP] = prv_handle_dsp_temp_rx,
     };
 
@@ -262,7 +307,20 @@ static void prv_periodic_broadcast_tx(SoftTimerId timer_id, void *context) {
     LOG_DEBUG("Sending status periodic broadcast\n");
     prv_broadcast_status(storage);
   }
-  // TODO(SOFT-421): broadcast temp measurements
+  if (storage->motor_sink_rx_bitset == (1 << NUM_MOTOR_CONTROLLERS) - 1) {
+    // Received heat sink temperature measurements from all motor controllers - clear bitset and
+    // broadcast
+    storage->motor_sink_rx_bitset = 0;
+    LOG_DEBUG("Sending motor and heat sink temperature periodic broadcast\n");
+    prv_broadcast_motor_sink_temp(storage);
+  }
+  if (storage->dsp_rx_bitset == (1 << NUM_MOTOR_CONTROLLERS) - 1) {
+    // Received DSP/CPU temperature from all motor controllers - clear bitset and broadcast
+    storage->dsp_rx_bitset = 0;
+    LOG_DEBUG("Sending DSP temperature periodic broadcast\n");
+    prv_broadcast_dsp_temp(storage);
+  }
+
   soft_timer_start_millis(MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS, prv_periodic_broadcast_tx,
                           storage, NULL);
 }
@@ -274,6 +332,8 @@ StatusCode mci_broadcast_init(MotorControllerBroadcastStorage *storage,
   }
   storage->velocity_rx_bitset = 0;
   storage->bus_rx_bitset = 0;
+  storage->motor_sink_rx_bitset = 0;
+  storage->dsp_rx_bitset = 0;
   storage->motor_can = settings->motor_can;
   prv_setup_motor_can(storage);
   return soft_timer_start_millis(MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS, prv_periodic_broadcast_tx,