diff --git a/codegen/can_messages.asciipb b/codegen/can_messages.asciipb index cb4627091..65a4d988d 100644 --- a/codegen/can_messages.asciipb +++ b/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 diff --git a/libraries/codegen-tooling/inc/can_msg_defs.h b/libraries/codegen-tooling/inc/can_msg_defs.h index 0b4b0f470..701210ea5 100644 --- a/libraries/codegen-tooling/inc/can_msg_defs.h +++ b/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; diff --git a/libraries/codegen-tooling/inc/can_pack.h b/libraries/codegen-tooling/inc/can_pack.h index 8ec035bc9..ab1918085 100644 --- a/libraries/codegen-tooling/inc/can_pack.h +++ b/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( \ diff --git a/libraries/codegen-tooling/inc/can_transmit.h b/libraries/codegen-tooling/inc/can_transmit.h index 39f1ef8fa..c8770d9f3 100644 --- a/libraries/codegen-tooling/inc/can_transmit.h +++ b/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) \ diff --git a/libraries/codegen-tooling/inc/can_unpack.h b/libraries/codegen-tooling/inc/can_unpack.h index 3297dcb41..093c4c0a4 100644 --- a/libraries/codegen-tooling/inc/can_unpack.h +++ b/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, \ diff --git a/projects/mci/inc/mci_broadcast.h b/projects/mci/inc/mci_broadcast.h index df0d8458c..51957f8c9 100644 --- a/projects/mci/inc/mci_broadcast.h +++ b/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 diff --git a/projects/mci/inc/motor_can.h b/projects/mci/inc/motor_can.h index b00ec20b7..c6cebde3f 100644 --- a/projects/mci/inc/motor_can.h +++ b/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) diff --git a/projects/mci/src/mci_broadcast.c b/projects/mci/src/mci_broadcast.c index a8a233c8a..924cdb4a3 100644 --- a/projects/mci/src/mci_broadcast.c +++ b/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,14 +137,39 @@ 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 @@ -132,7 +177,7 @@ static MotorControllerMeasurementCallback [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, diff --git a/projects/mci/test/test_mci_broadcast.c b/projects/mci/test/test_mci_broadcast.c index 431399deb..407629e21 100644 --- a/projects/mci/test/test_mci_broadcast.c +++ b/projects/mci/test/test_mci_broadcast.c @@ -35,7 +35,7 @@ typedef enum { TEST_MCI_VELOCITY_MESSAGE = 0, TEST_MCI_BUS_MEASUREMENT_MESSAGE, TEST_MCI_STATUS_MESSAGE, - TEST_MCI_MOTOR_TEMP_MESSAGE, + TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, TEST_MCI_DSP_TEMP_MESSAGE, NUM_TEST_MCI_MESSAGES } TestMciMessage; @@ -47,6 +47,8 @@ static MotorControllerBroadcastStorage s_broadcast_storage; static bool s_received_velocity = false; static bool s_received_bus_measurement = false; static bool s_received_status = false; +static bool s_received_sink_motor_temp = false; +static bool s_received_dsp_temp = false; static MotorControllerBroadcastSettings s_broadcast_settings = { .motor_can = &s_motor_can_storage, @@ -60,10 +62,17 @@ typedef struct TestWaveSculptorBusMeasurement { uint16_t bus_current_a; } TestWaveSculptorBusMeasurement; +typedef struct TestWaveSculptorSinkMotorTempMeasurement { + uint32_t heatsink_temp_c; + uint32_t motor_temp_c; +} TestWaveSculptorSinkMotorTempMeasurement; + typedef struct TestMotorControllerMeasurements { TestWaveSculptorBusMeasurement bus_measurements[NUM_MOTOR_CONTROLLERS]; uint16_t vehicle_velocity[NUM_MOTOR_CONTROLLERS]; uint32_t status[NUM_MOTOR_CONTROLLERS]; + TestWaveSculptorSinkMotorTempMeasurement sink_motor_measurements[NUM_MOTOR_CONTROLLERS]; + uint32_t dsp_measurements[NUM_MOTOR_CONTROLLERS]; } TestMotorControllerMeasurements; static TestMotorControllerMeasurements s_test_measurements = { 0 }; @@ -85,9 +94,9 @@ static MotorCanFrameId s_frame_id_map[] = { [L_MTR_MSG_IDX(TEST_MCI_VELOCITY_MESSAGE)] = L_MTR_MSG_ID(WAVESCULPTOR_MEASUREMENT_ID_VELOCITY), [R_MTR_MSG_IDX(TEST_MCI_VELOCITY_MESSAGE)] = R_MTR_MSG_ID(WAVESCULPTOR_MEASUREMENT_ID_VELOCITY), - [L_MTR_MSG_IDX(TEST_MCI_MOTOR_TEMP_MESSAGE)] = + [L_MTR_MSG_IDX(TEST_MCI_SINK_MOTOR_TEMP_MESSAGE)] = L_MTR_MSG_ID(WAVESCULPTOR_MEASUREMENT_ID_SINK_MOTOR_TEMPERATURE), - [R_MTR_MSG_IDX(TEST_MCI_MOTOR_TEMP_MESSAGE)] = + [R_MTR_MSG_IDX(TEST_MCI_SINK_MOTOR_TEMP_MESSAGE)] = R_MTR_MSG_ID(WAVESCULPTOR_MEASUREMENT_ID_SINK_MOTOR_TEMPERATURE), [L_MTR_MSG_IDX(TEST_MCI_DSP_TEMP_MESSAGE)] = @@ -127,6 +136,32 @@ static StatusCode prv_handle_status(const CanMessage *msg, void *context, CanAck return STATUS_CODE_OK; } +static StatusCode prv_handle_sink_motor_measurement(const CanMessage *msg, void *context, + CanAckStatus *ack_reply) { + uint16_t left_motor_temp, left_sink_temp, right_motor_temp, right_sink_temp; + CAN_UNPACK_MOTOR_SINK_TEMPS(msg, &left_motor_temp, &left_sink_temp, &right_motor_temp, + &right_sink_temp); + s_test_measurements.sink_motor_measurements[LEFT_MOTOR_CONTROLLER].motor_temp_c = left_motor_temp; + s_test_measurements.sink_motor_measurements[LEFT_MOTOR_CONTROLLER].heatsink_temp_c = + left_sink_temp; + s_test_measurements.sink_motor_measurements[RIGHT_MOTOR_CONTROLLER].motor_temp_c = + right_motor_temp; + s_test_measurements.sink_motor_measurements[RIGHT_MOTOR_CONTROLLER].heatsink_temp_c = + right_sink_temp; + s_received_sink_motor_temp = true; + return STATUS_CODE_OK; +} + +static StatusCode prv_handle_dsp_measurement(const CanMessage *msg, void *context, + CanAckStatus *ack_reply) { + uint32_t left_dsp_temp, right_dsp_temp; + CAN_UNPACK_DSP_BOARD_TEMPS(msg, &left_dsp_temp, &right_dsp_temp); + s_test_measurements.dsp_measurements[LEFT_MOTOR_CONTROLLER] = left_dsp_temp; + s_test_measurements.dsp_measurements[RIGHT_MOTOR_CONTROLLER] = right_dsp_temp; + s_received_dsp_temp = true; + return STATUS_CODE_OK; +} + static void prv_send_measurements(MotorController controller, TestMciMessage message_type, MotorControllerMeasurements *measurements) { WaveSculptorCanData can_data = { 0 }; @@ -140,6 +175,14 @@ static void prv_send_measurements(MotorController controller, TestMciMessage mes measurements->bus_measurements[controller].bus_current_a; } else if (message_type == TEST_MCI_STATUS_MESSAGE) { can_data.raw = measurements->status[controller]; + } else if (message_type == TEST_MCI_SINK_MOTOR_TEMP_MESSAGE) { + can_data.sink_motor_temp_measurement.heatsink_temp_c = + measurements->sink_motor_measurements[controller].heatsink_temp_c; + can_data.sink_motor_temp_measurement.motor_temp_c = + measurements->sink_motor_measurements[controller].motor_temp_c; + } else if (message_type == TEST_MCI_DSP_TEMP_MESSAGE) { + can_data.dsp_temp_measurement.dsp_temp_c = + measurements->dsp_measurements[controller].dsp_temp_c; } GenericCanMsg msg = { @@ -211,6 +254,31 @@ static void prv_assert_eq_expected_storage_status(MotorControllerMeasurements ex (uint16_t)s_broadcast_storage.measurements.status[controller]); } +// Sink temperature +static void prv_assert_eq_expected_storage_ht(MotorControllerMeasurements expected_measurements, + MotorController controller) { + TEST_ASSERT_EQUAL( + (uint16_t)expected_measurements.sink_motor_measurements[controller].heatsink_temp_c, + (uint16_t)s_broadcast_storage.measurements.sink_motor_measurements[controller] + .heatsink_temp_c); +} + +// Motor temperature +static void prv_assert_eq_expected_storage_mt(MotorControllerMeasurements expected_measurements, + MotorController controller) { + TEST_ASSERT_EQUAL( + (uint16_t)expected_measurements.sink_motor_measurements[controller].motor_temp_c, + (uint16_t)s_broadcast_storage.measurements.sink_motor_measurements[controller].motor_temp_c); +} + +// DSP temperature +static void prv_assert_eq_expected_storage_dt(MotorControllerMeasurements expected_measurements, + MotorController controller) { + TEST_ASSERT_EQUAL( + (uint32_t)expected_measurements.dsp_measurements[controller].dsp_temp_c, + (uint32_t)s_broadcast_storage.measurements.dsp_measurements[controller].dsp_temp_c); +} + void setup_test(void) { event_queue_init(); interrupt_init(); @@ -224,12 +292,20 @@ void setup_test(void) { can_register_rx_handler(SYSTEM_CAN_MESSAGE_MOTOR_VELOCITY, prv_handle_velocity, NULL)); TEST_ASSERT_OK(can_register_rx_handler(SYSTEM_CAN_MESSAGE_MOTOR_STATUS, prv_handle_status, NULL)); + + TEST_ASSERT_OK(can_register_rx_handler(SYSTEM_CAN_MESSAGE_MOTOR_SINK_TEMPS, + prv_handle_sink_motor_measurement, NULL)); + + TEST_ASSERT_OK(can_register_rx_handler(SYSTEM_CAN_MESSAGE_DSP_BOARD_TEMPS, + prv_handle_dsp_measurement, NULL)); } void teardown_test(void) { s_received_velocity = false; s_received_bus_measurement = false; s_received_status = false; + s_received_sink_motor_temp = false; + s_received_dsp_temp = false; memset(&s_test_measurements, 0, sizeof(s_test_measurements)); memset(&s_broadcast_storage, 0, sizeof(s_broadcast_storage)); } @@ -268,6 +344,33 @@ void test_left_all_right_all(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; // need to send in this order because of how the filter works @@ -275,23 +378,26 @@ void test_left_all_right_all(void) { prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, + &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); delay_ms(2 * MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS + 50); - // Velocity + bus measurement + status - prv_assert_num_broadcasts(3); + // Velocity + bus measurement + status + sink/motor temperature + DSP temperature + prv_assert_num_broadcasts(5); TEST_ASSERT_TRUE(s_received_velocity); TEST_ASSERT_TRUE(s_received_bus_measurement); TEST_ASSERT_TRUE(s_received_status); + TEST_ASSERT_TRUE(s_received_sink_motor_temp); + TEST_ASSERT_TRUE(s_received_dsp_temp); for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) { TEST_ASSERT_EQUAL((uint16_t)expected_measurements.bus_measurements[motor_id].bus_voltage_v, s_test_measurements.bus_measurements[motor_id].bus_voltage_v); @@ -299,7 +405,12 @@ void test_left_all_right_all(void) { s_test_measurements.bus_measurements[motor_id].bus_current_a); TEST_ASSERT_EQUAL((uint16_t)(expected_measurements.vehicle_velocity[motor_id] * 100), s_test_measurements.vehicle_velocity[motor_id]); - TEST_ASSERT_EQUAL(expected_measurements.status[motor_id], s_test_measurements.status[motor_id]); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].heatsink_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].heatsink_temp_c); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].motor_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].motor_temp_c); + TEST_ASSERT_EQUAL((uint32_t)expected_measurements.dsp_measurements[motor_id].dsp_temp_c, + s_test_measurements.dsp_measurements[motor_id]); } } @@ -334,13 +445,41 @@ void test_left_all_right_none(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, + &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); delay_ms(MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS + 50); @@ -349,12 +488,17 @@ void test_left_all_right_none(void) { TEST_ASSERT_FALSE(s_received_velocity); TEST_ASSERT_FALSE(s_received_bus_measurement); TEST_ASSERT_FALSE(s_received_status); + TEST_ASSERT_FALSE(s_received_sink_motor_temp); + TEST_ASSERT_FALSE(s_received_dsp_temp); // Make sure left measurements stored correctly still prv_assert_eq_expected_storage_bv(expected_measurements, LEFT_MOTOR_CONTROLLER); prv_assert_eq_expected_storage_bi(expected_measurements, LEFT_MOTOR_CONTROLLER); prv_assert_eq_expected_storage_vel(expected_measurements, LEFT_MOTOR_CONTROLLER); prv_assert_eq_expected_storage_status(expected_measurements, LEFT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_ht(expected_measurements, LEFT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_mt(expected_measurements, LEFT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_dt(expected_measurements, LEFT_MOTOR_CONTROLLER); } // Test 3: Send left all + right status and check that only status outputs @@ -389,13 +533,41 @@ void test_left_all_right_status(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, + &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); @@ -404,6 +576,8 @@ void test_left_all_right_status(void) { TEST_ASSERT_FALSE(s_received_velocity); TEST_ASSERT_FALSE(s_received_bus_measurement); + TEST_ASSERT_FALSE(s_received_sink_motor_temp); + TEST_ASSERT_FALSE(s_received_dsp_temp); TEST_ASSERT_TRUE(s_received_status); for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) { TEST_ASSERT_EQUAL(expected_measurements.status[motor_id], s_test_measurements.status[motor_id]); @@ -443,13 +617,41 @@ void test_left_all_right_status_bus(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, + &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, @@ -459,6 +661,8 @@ void test_left_all_right_status_bus(void) { prv_assert_num_broadcasts(2); TEST_ASSERT_FALSE(s_received_velocity); + TEST_ASSERT_FALSE(s_received_sink_motor_temp); + TEST_ASSERT_FALSE(s_received_dsp_temp); TEST_ASSERT_TRUE(s_received_bus_measurement); TEST_ASSERT_TRUE(s_received_status); for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) { @@ -499,6 +703,33 @@ void test_3x_left_all_right_all(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; // Repeat full sequence 3 times @@ -508,7 +739,7 @@ void test_3x_left_all_right_all(void) { prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_BUS_MEASUREMENT_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); @@ -516,15 +747,15 @@ void test_3x_left_all_right_all(void) { &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); - prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); delay_ms(2 * MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS + 50); - // Velocity + bus measurement + status - prv_assert_num_broadcasts(3); + // Velocity + bus measurement + status + sink/motor temperature + DSP temperature + prv_assert_num_broadcasts(5); TEST_ASSERT_TRUE(s_received_velocity); TEST_ASSERT_TRUE(s_received_bus_measurement); TEST_ASSERT_TRUE(s_received_status); @@ -537,12 +768,20 @@ void test_3x_left_all_right_all(void) { s_test_measurements.vehicle_velocity[motor_id]); TEST_ASSERT_EQUAL(expected_measurements.status[motor_id], s_test_measurements.status[motor_id]); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].heatsink_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].heatsink_temp_c); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].motor_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].motor_temp_c); + TEST_ASSERT_EQUAL((uint32_t)expected_measurements.dsp_measurements[motor_id].dsp_temp_c, + s_test_measurements.dsp_measurements[motor_id]); } // Reset before next iteration s_received_velocity = false; s_received_bus_measurement = false; s_received_status = false; + s_received_sink_motor_temp = false; + s_received_dsp_temp = false; } } @@ -574,6 +813,33 @@ void test_message_id_filter(void) { [LEFT_MOTOR_CONTROLLER] = 0xDEADBEEF, [RIGHT_MOTOR_CONTROLLER] = 0xDEADBEEF, }, + .sink_motor_measurements = + { + [LEFT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 50.0221, + .heatsink_temp_c = 25.1245, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .motor_temp_c = 57.1967, + .heatsink_temp_c = 28.4287, + }, + }, + .dsp_measurements = + { + // Set reserved field to 0.0 + [LEFT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 45.8910, + .reserved = 0.0, + }, + [RIGHT_MOTOR_CONTROLLER] = + { + .dsp_temp_c = 56.1458, + .reserved = 0.0, + }, + }, }; // Should skip @@ -590,7 +856,7 @@ void test_message_id_filter(void) { prv_assert_eq_expected_storage_bi(expected_measurements, LEFT_MOTOR_CONTROLLER); // Should skip - prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); // Should process and store @@ -602,11 +868,16 @@ void test_message_id_filter(void) { // Should still skip prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_VELOCITY_MESSAGE, &expected_measurements); // Should process - prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, + &expected_measurements); + prv_assert_eq_expected_storage_ht(expected_measurements, LEFT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_mt(expected_measurements, LEFT_MOTOR_CONTROLLER); // Should skip prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); // Should process prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); + prv_assert_eq_expected_storage_dt(expected_measurements, LEFT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_dt(expected_measurements, LEFT_MOTOR_CONTROLLER); // Should skip prv_send_measurements(LEFT_MOTOR_CONTROLLER, TEST_MCI_STATUS_MESSAGE, &expected_measurements); @@ -615,7 +886,7 @@ void test_message_id_filter(void) { prv_assert_eq_expected_storage_status(expected_measurements, RIGHT_MOTOR_CONTROLLER); // Should skip - prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); // Should process and store @@ -629,18 +900,23 @@ void test_message_id_filter(void) { prv_assert_eq_expected_storage_vel(expected_measurements, RIGHT_MOTOR_CONTROLLER); // Should process - prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_MOTOR_TEMP_MESSAGE, + prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_SINK_MOTOR_TEMP_MESSAGE, &expected_measurements); + prv_assert_eq_expected_storage_ht(expected_measurements, RIGHT_MOTOR_CONTROLLER); + prv_assert_eq_expected_storage_mt(expected_measurements, RIGHT_MOTOR_CONTROLLER); // Should process prv_send_measurements(RIGHT_MOTOR_CONTROLLER, TEST_MCI_DSP_TEMP_MESSAGE, &expected_measurements); + prv_assert_eq_expected_storage_dt(expected_measurements, RIGHT_MOTOR_CONTROLLER); delay_ms(2 * MOTOR_CONTROLLER_BROADCAST_TX_PERIOD_MS + 50); - // Velocity + bus measurement + status - prv_assert_num_broadcasts(3); + // Velocity + bus measurement + status + sink/motor temperature + DSP temperature + prv_assert_num_broadcasts(5); TEST_ASSERT_TRUE(s_received_velocity); TEST_ASSERT_TRUE(s_received_bus_measurement); TEST_ASSERT_TRUE(s_received_status); + TEST_ASSERT_TRUE(s_received_sink_motor_temp); + TEST_ASSERT_TRUE(s_received_dsp_temp); for (size_t motor_id = 0; motor_id < NUM_MOTOR_CONTROLLERS; motor_id++) { TEST_ASSERT_EQUAL((uint16_t)expected_measurements.bus_measurements[motor_id].bus_voltage_v, s_test_measurements.bus_measurements[motor_id].bus_voltage_v); @@ -649,6 +925,12 @@ void test_message_id_filter(void) { TEST_ASSERT_EQUAL((uint16_t)(expected_measurements.vehicle_velocity[motor_id] * 100), s_test_measurements.vehicle_velocity[motor_id]); TEST_ASSERT_EQUAL(expected_measurements.status[motor_id], s_test_measurements.status[motor_id]); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].heatsink_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].heatsink_temp_c); + TEST_ASSERT_EQUAL(expected_measurements.sink_motor_measurements[motor_id].motor_temp_c, + s_test_measurements.sink_motor_measurements[motor_id].motor_temp_c); + TEST_ASSERT_EQUAL((uint32_t)expected_measurements.dsp_measurements[motor_id].dsp_temp_c, + s_test_measurements.dsp_measurements[motor_id]); } // Stored measurements should be unchanged @@ -657,5 +939,8 @@ void test_message_id_filter(void) { prv_assert_eq_expected_storage_bi(expected_measurements, i); prv_assert_eq_expected_storage_vel(expected_measurements, i); prv_assert_eq_expected_storage_status(expected_measurements, i); + prv_assert_eq_expected_storage_ht(expected_measurements, i); + prv_assert_eq_expected_storage_mt(expected_measurements, i); + prv_assert_eq_expected_storage_dt(expected_measurements, i); } }