diff --git a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h index b2ddd3cc2..4d7c9733d 100644 --- a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h +++ b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h @@ -52,7 +52,7 @@ static const std::map CAN_DESC{ {CanId::PWR_MODE, "PWR_MODE (Power Mode)"}, {CanId::MAIN_HEADING, "MAIN_HEADING (Main heading for rudder)"}, {CanId::MAIN_TR_TAB, "MAIN_TR_TAB (Trim tab for sail)"}, - {CanId::BMS_DATA_FRAME, "BMS_P_DATA_FRAME_1 (Battery 1 data)"}, + {CanId::BMS_DATA_FRAME, "BMS_P_DATA_FRAME (Battery data)"}, {CanId::RESERVED, "Reserved for mainframe (0x0 - 0x29)"}, {CanId::SAIL_AIS, "SAIL_AIS (AIS ship data)"}, {CanId::MAIN_HEADING, "MAIN_HEADING (Main rudder command)"}, @@ -525,6 +525,73 @@ class AISShips final : public BaseFrame uint8_t idx_; }; +/** + * @brief Power mode class derived from BaseFrame. Represents power mode data. + * + */ +class PwrMode final : public BaseFrame +{ +public: + static constexpr std::array PWR_MODE_IDS = {CanId::PWR_MODE}; + static constexpr uint8_t CAN_BYTE_DLEN_ = 1; + static constexpr uint8_t BYTE_OFF_MODE = 0; + static constexpr uint8_t POWER_MODE_LOW = 0; + static constexpr uint8_t POWER_MODE_NORMAL = 1; + static constexpr std::array PWR_MODES = {POWER_MODE_LOW, POWER_MODE_NORMAL}; + + /** + * @brief Explicitly deleted no-argument constructor + * + */ + PwrMode() = delete; + + /** + * @brief Construct an PwrMode object from a Linux CanFrame representation + * + * @param cf Linux CanFrame + */ + explicit PwrMode(const CanFrame & cf); + + /** + * @brief Construct a PwrMode object given a mode and CAN ID + * + * @param mode Power mode select + * @param id CanId of the PwrMode + */ + explicit PwrMode(uint8_t mode, CanId id); + + /** + * @return the custom_interfaces ROS representation of the PwrMode object + */ + //msg::HelperPwrMode toRosMsg() const; + + /** + * @return the Linux CanFrame representation of the PwrMode object + */ + CanFrame toLinuxCan() const override; + + /** + * @return A string that can be printed or logged to debug a PwrMode object + */ + std::string debugStr() const override; + +private: + /** + * @brief Private helper constructor for PwrMode objects + * + * @param id CanId of the PwrMode + */ + explicit PwrMode(CanId id); + + /** + * @brief Check if the assigned fields after constructing a PwrMode object are within bounds. + * @throws std::out_of_range if any assigned fields are outside of expected bounds + */ + void checkBounds() const; + + uint8_t mode_; +}; + /** * @brief A DesiredHeading class derived from the BaseFrame. Represents a desired heading for the rudder. * diff --git a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp index 69bfe1123..10ee448cb 100644 --- a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp @@ -526,7 +526,7 @@ CanFrame AISShips::toLinuxCan() const std::memcpy(cf.data + BYTE_OFF_HEADING, &raw_heading, sizeof(int16_t)); std::memcpy(cf.data + BYTE_OFF_ROT, &raw_rot, sizeof(int8_t)); std::memcpy(cf.data + BYTE_OFF_LENGTH, &raw_length, sizeof(int16_t)); - std::memcpy(cf.data + BYTE_OFF_WIDTH, &raw_width, sizeof(uint8_t)); + std::memcpy(cf.data + BYTE_OFF_WIDTH, &raw_width, sizeof(uint16_t)); std::memcpy(cf.data + BYTE_OFF_IDX, &raw_idx, sizeof(int8_t)); std::memcpy(cf.data + BYTE_OFF_NUM_SHIPS, &raw_num_ships, sizeof(int8_t)); @@ -601,6 +601,56 @@ void AISShips::checkBounds() const //AISShips private END //AISShips END +//PwrMode START +//PwrMode public START + +PwrMode::PwrMode(const CanFrame & cf) : PwrMode(static_cast(cf.can_id)) +{ + uint8_t raw_mode; + + std::memcpy(&raw_mode, cf.data + BYTE_OFF_MODE, sizeof(uint8_t)); + + mode_ = raw_mode; + + checkBounds(); +} + +PwrMode::PwrMode(uint8_t mode, CanId id) : BaseFrame(id, CAN_BYTE_DLEN_), mode_(mode) { checkBounds(); } + +CanFrame PwrMode::toLinuxCan() const +{ + uint8_t raw_mode = mode_; + + CanFrame cf = BaseFrame::toLinuxCan(); + std::memcpy(cf.data + BYTE_OFF_MODE, &raw_mode, sizeof(uint8_t)); + + return cf; +} + +std::string PwrMode::debugStr() const +{ + std::stringstream ss; + ss << BaseFrame::debugStr() << "\n" + << "Power mode: " << mode_; + return ss.str(); +} + +// PwrMode public END +// PwrMode private START + +PwrMode::PwrMode(CanId id) : BaseFrame(std::span{PWR_MODE_IDS}, id, CAN_BYTE_DLEN_) {} + +void PwrMode::checkBounds() const +{ + auto err = utils::isOutOfBounds(mode_, POWER_MODE_LOW, POWER_MODE_NORMAL); + if (err) { + std::string err_msg = err.value(); + throw std::out_of_range("Power mode value is out of bounds!\n" + debugStr() + "\n" + err_msg); + } +} +// PwrMode private END +// PwrMode END + // DesiredHeading START // DesiredHeading public START diff --git a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp index 02ab69cfd..84fbfd96b 100644 --- a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp @@ -80,9 +80,6 @@ class CanTransceiverIntf : public NetNode std::make_pair(CanId::SAIL_WIND, std::function([this](const CanFrame & frame) { publishWindSensor(frame); })), - std::make_pair(CanId::DATA_WIND, std::function([this](const CanFrame & frame) { - publishWindSensor(frame); - })), std::make_pair( CanId::GENERIC_SENSOR_START, std::function([this](const CanFrame & frame) { publishGeneric(frame); })), @@ -152,6 +149,9 @@ class CanTransceiverIntf : public NetNode // Mock CAN file descriptor for simulation int sim_intf_fd_; + // Saved power mode state + uint8_t set_pwr_mode = CAN_FP::PwrMode::POWER_MODE_NORMAL; + /** * @brief Publish AIS ships * @@ -186,6 +186,15 @@ class CanTransceiverIntf : public NetNode msg::HelperBattery & bat_msg = batteries_; bat_msg = bat.toRosMsg(); batteries_pub_->publish(batteries_); + // Voltage < 10V means low power mode + // If in low power mode, power mode will only change back to normal if voltage reaches >= 12V. + if (bat_msg.voltage < 10) { //NOLINT(readability-magic-numbers) + set_pwr_mode = CAN_FP::PwrMode::POWER_MODE_LOW; + } else if (bat_msg.voltage >= 12) { //NOLINT(readability-magic-numbers) + set_pwr_mode = CAN_FP::PwrMode::POWER_MODE_NORMAL; + } + CAN_FP::PwrMode power_mode(set_pwr_mode, CAN_FP::CanId::PWR_MODE); + can_trns_->send(power_mode.toLinuxCan()); } /** @@ -279,17 +288,6 @@ class CanTransceiverIntf : public NetNode generic_sensors_pub_->publish(generic_sensors_); } - /** - * @brief SailCmd subscriber callback - * - * @param sail_cmd_ - */ - void subSailCmdCb(const msg::SailCmd & sail_cmd_input) - { - sail_cmd_ = sail_cmd_input; - boat_sim_input_msg_.set__sail_cmd(sail_cmd_); - } - // SIMULATION CALLBACKS // /** @@ -302,6 +300,19 @@ class CanTransceiverIntf : public NetNode boat_sim_input_pub_->publish(boat_sim_input_msg); } + /** + * @brief SailCmd subscriber callback + * + * @param sail_cmd_ + */ + void subSailCmdCb(const msg::SailCmd & sail_cmd_input) + { + sail_cmd_ = sail_cmd_input; + boat_sim_input_msg_.set__sail_cmd(sail_cmd_); + + can_trns_->send(CAN_FP::MainTrimTab(sail_cmd_input, CanId::MAIN_TR_TAB).toLinuxCan()); + } + /** * @brief Mock AIS topic callback * @@ -317,7 +328,11 @@ class CanTransceiverIntf : public NetNode * * @param desired_heading desired_heading received from the Desired Heading topic */ - void subDesiredHeadingCb(msg::DesiredHeading desired_heading) { boat_sim_input_msg_.set__heading(desired_heading); } + void subDesiredHeadingCb(msg::DesiredHeading desired_heading) + { + boat_sim_input_msg_.set__heading(desired_heading); + can_trns_->send(CAN_FP::DesiredHeading(desired_heading, CanId::MAIN_TR_TAB).toLinuxCan()); + } /** * @brief Mock GPS topic callback diff --git a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp index cb55536bf..17414b152 100644 --- a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp +++ b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp @@ -32,6 +32,8 @@ class TestCanFrameParser : public ::testing::Test }; /** + * @brief Test ROS<->CAN Battery translations work as expected for valid input values. + * Treat both batteries as one combined battery. * @brief Test ROS<->CAN Battery translations work as expected for valid input values. * Treat both batteries as one combined battery. */ @@ -539,6 +541,55 @@ TEST_F(TestCanFrameParser, TestGPSInvalid) }; } +/** + * @brief Test NET<->CAN PwrMode translations work as expected for valid input values + * + */ +TEST_F(TestCanFrameParser, PwrModeTestValid) +{ + constexpr std::size_t NUM_MODES = CAN_FP::PwrMode::PWR_MODES.size(); //NOLINT(readability-magic-numbers) + constexpr std::array expected_modes{ + CAN_FP::PwrMode::POWER_MODE_LOW, CAN_FP::PwrMode::POWER_MODE_NORMAL}; + + for (size_t i = 0; i < NUM_MODES; i++) { + CAN_FP::CanId id = CAN_FP::CanId::PWR_MODE; + uint8_t expected_mode = expected_modes[i]; + + CAN_FP::PwrMode pwr_mode_inst = CAN_FP::PwrMode(expected_mode, id); + CAN_FP::CanFrame cf = pwr_mode_inst.toLinuxCan(); + + EXPECT_EQ(cf.can_id, static_cast(id)); + EXPECT_EQ(cf.len, CAN_FP::PwrMode::CAN_BYTE_DLEN_); + + uint8_t raw_mode; + std::memcpy(&raw_mode, cf.data + CAN_FP::PwrMode::BYTE_OFF_MODE, sizeof(int8_t)); + + EXPECT_EQ(raw_mode, expected_mode); + } +} + +/** + * @brief Test the behavior of the WindSensor class when given invalid input values + * + */ +TEST_F(TestCanFrameParser, TestPwrModeInvalid) +{ + CAN_FP::CanId invalid_id = CAN_FP::CanId::RESERVED; + + CAN_FP::CanFrame cf{.can_id = static_cast(invalid_id)}; + + EXPECT_THROW(CAN_FP::WindSensor tmp(cf), CAN_FP::CanIdMismatchException); + + std::vector invalid_modes{CAN_FP::PwrMode::POWER_MODE_LOW - 1, CAN_FP::PwrMode::POWER_MODE_NORMAL + 1}; + + CAN_FP::CanId valid_id = CAN_FP::CanId::PWR_MODE; + + // Set a valid speed for this portion + for (uint8_t invalid_mode : invalid_modes) { + EXPECT_THROW(CAN_FP::PwrMode tmp(invalid_mode, valid_id), std::out_of_range); + }; +} + /** * @brief Test ROS<->CAN AISShips translations work as expected for valid input values * @@ -926,10 +977,10 @@ class TestCanTransceiver : public ::testing::Test }; /** - * @brief Test that callbacks can be properly registered and invoked on desired CanIds + * @brief Test that callback can be properly registered and invoked on BMS_DATA_FRAME CanId * */ -TEST_F(TestCanTransceiver, TestNewDataValid) +TEST_F(TestCanTransceiver, TestNewBatteryValid) { volatile bool is_cb_called = false; @@ -950,6 +1001,181 @@ TEST_F(TestCanTransceiver, TestNewDataValid) EXPECT_TRUE(is_cb_called); } +/** + * @brief Test that callback can be properly registered and invoked on MAIN_HEADING CanId + * + */ +TEST_F(TestCanTransceiver, TestNewHeadingValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::MAIN_HEADING, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::MAIN_HEADING)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on MAIN_TR_TAB CanId + * + */ +TEST_F(TestCanTransceiver, TestNewTrimTabValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::MAIN_TR_TAB, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::MAIN_TR_TAB)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on SAIL_AIS CanId + * + */ +TEST_F(TestCanTransceiver, TestNewAISValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::SAIL_AIS, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::SAIL_AIS)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on SAIL_WIND CanId + * + */ +TEST_F(TestCanTransceiver, TestNewSailWindValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::SAIL_WIND, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::SAIL_WIND)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on RUDDER_DATA_FRAME CanId + * + */ +TEST_F(TestCanTransceiver, TestNewRudderDataValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::RUDDER_DATA_FRAME, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::RUDDER_DATA_FRAME)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on PATH_GPS_DATA_FRAME CanId + * + */ +TEST_F(TestCanTransceiver, TestNewGPSValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::PATH_GPS_DATA_FRAME, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::PATH_GPS_DATA_FRAME)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + +/** + * @brief Test that callback can be properly registered and invoked on DATA_WIND CanId + * + */ +TEST_F(TestCanTransceiver, TestNewDataWindValid) +{ + volatile bool is_cb_called = false; + + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + is_cb_called = true; + }; + canbus_t_->registerCanCbs({{ + std::make_pair(CAN_FP::CanId::DATA_WIND, test_cb), + }}); + + // just need a valid and matching ID for this test + CAN_FP::CanFrame dummy_frame{.can_id = static_cast(CAN_FP::CanId::DATA_WIND)}; + + canbus_t_->send(dummy_frame); + + std::this_thread::sleep_for(SLEEP_TIME); + + EXPECT_TRUE(is_cb_called); +} + /** * @brief Test that the CanTransceiver ignores IDs that we don't register callbacks for *