/* * scout_can_protocol_test.cpp * * Created on: Jun 11, 2019 23:26 * Description: * * Reference: * [1] https://cryptii.com/pipes/integer-encoder * * Copyright (c) 2019 Ruixiang Du (rdu) */ #include #include #include "gtest/gtest.h" #include "scout_base/scout_base.hpp" using namespace wescore; struct ScoutCANProtocolTest : testing::Test { ScoutCANProtocolTest() { // control frames motion_ctrl_frame.can_id = CAN_MSG_MOTION_CONTROL_CMD_ID; motion_ctrl_frame.can_dlc = 8; motion_ctrl_frame.data[0] = 0x01; motion_ctrl_frame.data[1] = 0x00; motion_ctrl_frame.data[2] = 0x0a; motion_ctrl_frame.data[3] = 0x00; motion_ctrl_frame.data[4] = 0x00; motion_ctrl_frame.data[5] = 0x00; motion_ctrl_frame.data[6] = 0x00; motion_ctrl_frame.data[7] = CalcScoutCANChecksum(motion_ctrl_frame.can_id, motion_ctrl_frame.data, motion_ctrl_frame.can_dlc); light_ctrl_frame.can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID; light_ctrl_frame.can_dlc = 8; light_ctrl_frame.data[0] = 0x01; light_ctrl_frame.data[1] = 0x00; light_ctrl_frame.data[2] = 0x0a; light_ctrl_frame.data[3] = 0x00; light_ctrl_frame.data[4] = 0x00; light_ctrl_frame.data[5] = 0x00; light_ctrl_frame.data[6] = 0x00; light_ctrl_frame.data[7] = CalcScoutCANChecksum(light_ctrl_frame.can_id, light_ctrl_frame.data, light_ctrl_frame.can_dlc); // feedback frames motion_status_frame.can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; motion_status_frame.can_dlc = 8; motion_status_frame.data[0] = 0x04; motion_status_frame.data[1] = 0xe8; // 1.256 motion_status_frame.data[2] = 0x00; motion_status_frame.data[3] = 0x7b; // 0.123 motion_status_frame.data[4] = 0x00; motion_status_frame.data[5] = 0x00; motion_status_frame.data[6] = 0x00; motion_status_frame.data[7] = CalcScoutCANChecksum(motion_status_frame.can_id, motion_status_frame.data, motion_status_frame.can_dlc); motion_status_frame2.can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; motion_status_frame2.can_dlc = 8; motion_status_frame2.data[0] = 0xfb; motion_status_frame2.data[1] = 0x18; // -1.256 motion_status_frame2.data[2] = 0xff; motion_status_frame2.data[3] = 0x85; // -0.123 motion_status_frame2.data[4] = 0x00; motion_status_frame2.data[5] = 0x00; motion_status_frame2.data[6] = 0x00; motion_status_frame2.data[7] = CalcScoutCANChecksum(motion_status_frame2.can_id, motion_status_frame2.data, motion_status_frame2.can_dlc); light_status_frame.can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; light_status_frame.can_dlc = 8; light_status_frame.data[0] = 0x01; light_status_frame.data[1] = 0x03; light_status_frame.data[2] = 0x55; // 85 light_status_frame.data[3] = 0x02; light_status_frame.data[4] = 0x32; // 50 light_status_frame.data[5] = 0x00; light_status_frame.data[6] = 0x00; light_status_frame.data[7] = CalcScoutCANChecksum(light_status_frame.can_id, light_status_frame.data, light_status_frame.can_dlc); system_status_frame.can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; system_status_frame.can_dlc = 8; system_status_frame.data[0] = 0x01; system_status_frame.data[1] = 0x01; system_status_frame.data[2] = 0x01; system_status_frame.data[3] = 0x09; // 26.5 system_status_frame.data[4] = 0xf0; system_status_frame.data[5] = 0xff; system_status_frame.data[6] = 0x00; system_status_frame.data[7] = CalcScoutCANChecksum(system_status_frame.can_id, system_status_frame.data, system_status_frame.can_dlc); motor1_driver_status_frame.can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; motor1_driver_status_frame.can_dlc = 8; motor1_driver_status_frame.data[0] = 0x00; motor1_driver_status_frame.data[1] = 0x7d; // 12.5 motor1_driver_status_frame.data[2] = 0x04; motor1_driver_status_frame.data[3] = 0xd2; // 1234 motor1_driver_status_frame.data[4] = 0x38; // 56 motor1_driver_status_frame.data[5] = 0x00; motor1_driver_status_frame.data[6] = 0x00; motor1_driver_status_frame.data[7] = CalcScoutCANChecksum(motor1_driver_status_frame.can_id, motor1_driver_status_frame.data, motor1_driver_status_frame.can_dlc); motor2_driver_status_frame = motor1_driver_status_frame; motor2_driver_status_frame.can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; motor2_driver_status_frame.data[2] = 0xfb; motor2_driver_status_frame.data[3] = 0x2e; // -1234 motor2_driver_status_frame.data[4] = 0xc8; // -56 motor2_driver_status_frame.data[7] = CalcScoutCANChecksum(motor2_driver_status_frame.can_id, motor2_driver_status_frame.data, motor2_driver_status_frame.can_dlc); motor3_driver_status_frame = motor1_driver_status_frame; motor3_driver_status_frame.can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; motor3_driver_status_frame.data[7] = CalcScoutCANChecksum(motor3_driver_status_frame.can_id, motor3_driver_status_frame.data, motor3_driver_status_frame.can_dlc); motor4_driver_status_frame = motor2_driver_status_frame; motor4_driver_status_frame.can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; motor4_driver_status_frame.data[7] = CalcScoutCANChecksum(motor4_driver_status_frame.can_id, motor4_driver_status_frame.data, motor4_driver_status_frame.can_dlc); } ScoutBase scout_base; can_frame motion_ctrl_frame; can_frame light_ctrl_frame; can_frame motion_status_frame; can_frame motion_status_frame2; can_frame light_status_frame; can_frame system_status_frame; can_frame motor1_driver_status_frame; can_frame motor2_driver_status_frame; can_frame motor3_driver_status_frame; can_frame motor4_driver_status_frame; }; TEST_F(ScoutCANProtocolTest, MotionStatusMsg) { ScoutState state; ScoutMessage msg; DecodeScoutMsgFromCAN(&motion_status_frame, &msg); ScoutBase::UpdateScoutState(msg, state); ASSERT_FLOAT_EQ(state.linear_velocity, 1.256); ASSERT_FLOAT_EQ(state.angular_velocity, 0.123); ScoutState state2; ScoutMessage msg2; DecodeScoutMsgFromCAN(&motion_status_frame2, &msg2); ScoutBase::UpdateScoutState(msg2, state2); ASSERT_FLOAT_EQ(state2.linear_velocity, -1.256); ASSERT_FLOAT_EQ(state2.angular_velocity, -0.123); } TEST_F(ScoutCANProtocolTest, LightStatusMsg) { ScoutState state; ScoutMessage msg; DecodeScoutMsgFromCAN(&light_status_frame, &msg); ScoutBase::UpdateScoutState(msg, state); ASSERT_EQ(state.light_control_enabled, true); ASSERT_EQ(state.front_light_state.mode, 0x03); ASSERT_EQ(state.front_light_state.custom_value, 85); ASSERT_EQ(state.rear_light_state.mode, 0x02); ASSERT_EQ(state.rear_light_state.custom_value, 50); } TEST_F(ScoutCANProtocolTest, SystemStatusMsg) { ScoutState state; ScoutMessage msg; DecodeScoutMsgFromCAN(&system_status_frame, &msg); ScoutBase::UpdateScoutState(msg, state); ASSERT_EQ(state.base_state, true); ASSERT_EQ(state.control_mode, 0x01); ASSERT_FLOAT_EQ(state.battery_voltage, 26.5); ASSERT_EQ(state.fault_code, 0xf0ff); } TEST_F(ScoutCANProtocolTest, MotorDriverStatusMsg) { ScoutState state; ScoutMessage msg1, msg2, msg3, msg4; DecodeScoutMsgFromCAN(&motor1_driver_status_frame, &msg1); DecodeScoutMsgFromCAN(&motor2_driver_status_frame, &msg2); DecodeScoutMsgFromCAN(&motor3_driver_status_frame, &msg3); DecodeScoutMsgFromCAN(&motor4_driver_status_frame, &msg4); ScoutBase::UpdateScoutState(msg1, state); ScoutBase::UpdateScoutState(msg2, state); ScoutBase::UpdateScoutState(msg3, state); ScoutBase::UpdateScoutState(msg4, state); ASSERT_FLOAT_EQ(state.motor_states[0].current, 12.5); ASSERT_FLOAT_EQ(state.motor_states[0].rpm, 1234); ASSERT_FLOAT_EQ(state.motor_states[0].temperature, 56); ASSERT_FLOAT_EQ(state.motor_states[1].current, 12.5); ASSERT_FLOAT_EQ(state.motor_states[1].rpm, -1234); ASSERT_FLOAT_EQ(state.motor_states[1].temperature, -56); ASSERT_FLOAT_EQ(state.motor_states[2].current, 12.5); ASSERT_FLOAT_EQ(state.motor_states[2].rpm, 1234); ASSERT_FLOAT_EQ(state.motor_states[2].temperature, 56); ASSERT_FLOAT_EQ(state.motor_states[3].current, 12.5); ASSERT_FLOAT_EQ(state.motor_states[3].rpm, -1234); ASSERT_FLOAT_EQ(state.motor_states[3].temperature, -56); }