Implemented power mode frame and ROS callbacks send data to CAN

src/network_systems/projects/can_transceiver/inc/can_frame_parser.h

@@ -52,7 +52,7 @@ static const std::map<CanId, std::string> 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<CanId, 1>   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<uint8_t, 2> 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.
  *

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<CanId>(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<float>(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
 

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<void(const CanFrame &)>([this](const CanFrame & frame) {
                                   publishWindSensor(frame);
                               })),
-               std::make_pair(CanId::DATA_WIND, std::function<void(const CanFrame &)>([this](const CanFrame & frame) {
-                                  publishWindSensor(frame);
-                              })),
                std::make_pair(
                  CanId::GENERIC_SENSOR_START,
                  std::function<void(const CanFrame &)>([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