Skip to content

Commit

Permalink
Implemented power mode frame and ROS callbacks send data to CAN (#428)
Browse files Browse the repository at this point in the history
  • Loading branch information
@lross03 @vaibhavambastha
lross03 authored and vaibhavambastha committed Nov 2, 2024
1 parent 0763857 commit 004d4d3
Show file tree
Hide file tree
Showing 4 changed files with 377 additions and 19 deletions.
69 changes: 68 additions & 1 deletion src/network_systems/projects/can_transceiver/inc/can_frame_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)"},
Expand Down Expand Up @@ -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.
*
Expand Down
52 changes: 51 additions & 1 deletion src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down Expand Up @@ -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

Expand Down
45 changes: 30 additions & 15 deletions src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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); })),
Expand Down Expand Up @@ -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
*
Expand Down Expand Up @@ -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());
}

/**
Expand Down Expand Up @@ -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 //

/**
Expand All @@ -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
*
Expand All @@ -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
Expand Down
Loading

0 comments on commit 004d4d3

Please sign in to comment.