1 #ifndef H_SOCKETCAN_DRIVER 2 #define H_SOCKETCAN_DRIVER 5 #include <boost/bind.hpp> 8 #include <sys/socket.h> 12 #include <linux/can.h> 13 #include <linux/can/raw.h> 14 #include <linux/can/error.h> 29 can_err_mask_t mask = 0;
31 #define add_bit(e) mask |= (settings->get_optional(entry + "/" + #e, (defaults & e) != 0) ? e : 0) 47 : loopback_(false), sc_(-1), error_mask_(0), fatal_error_mask_(0)
61 [[deprecated(
"provide settings explicitly")]]
virtual bool init(
const std::string &device,
bool loopback)
override {
66 ROSCANOPEN_ERROR(
"socketcan_interface",
"settings must not be a null pointer");
69 const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT
74 const can_err_mask_t report_errors = ( CAN_ERR_LOSTARB
80 can_err_mask_t fatal_error_mask =
parse_error_mask(settings,
"fatal_error_mask", fatal_errors) | CAN_ERR_BUSOFF;
81 can_err_mask_t error_mask =
parse_error_mask(settings,
"error_mask", report_errors | fatal_error_mask) | fatal_error_mask;
82 return init(device, loopback, error_mask, fatal_error_mask);
88 return init(
device_, loopback_, error_mask_, fatal_error_mask_);
99 if( internal_error & CAN_ERR_TX_TIMEOUT){
100 str +=
"TX timeout (by netdevice driver);";
103 if( internal_error & CAN_ERR_LOSTARB){
104 str +=
"lost arbitration;";
107 if( internal_error & CAN_ERR_CRTL){
108 str +=
"controller problems;";
111 if( internal_error & CAN_ERR_PROT){
112 str +=
"protocol violations;";
115 if( internal_error & CAN_ERR_TRX){
116 str +=
"transceiver status;";
119 if( internal_error & CAN_ERR_BUSOFF){
123 if( internal_error & CAN_ERR_RESTARTED){
124 str +=
"controller restarted;";
136 bool init(
const std::string &device,
bool loopback, can_err_mask_t error_mask, can_err_mask_t fatal_error_mask) {
141 loopback_ = loopback;
142 error_mask_ = error_mask;
143 fatal_error_mask_ = fatal_error_mask;
145 int sc = socket( PF_CAN, SOCK_RAW, CAN_RAW );
147 setErrorCode(boost::system::error_code(sc,boost::system::system_category()));
152 strcpy(ifr.ifr_name, device_.c_str());
153 int ret = ioctl(sc, SIOCGIFINDEX, &ifr);
156 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
160 ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
161 &error_mask,
sizeof(error_mask));
164 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
170 int recv_own_msgs = 1;
171 ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs,
sizeof(recv_own_msgs));
174 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
180 struct sockaddr_can addr = {0};
181 addr.can_family = AF_CAN;
182 addr.can_ifindex = ifr.ifr_ifindex;
183 ret = bind( sc, (
struct sockaddr*)&addr,
sizeof(addr) );
186 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
191 boost::system::error_code ec;
216 can_frame frame = {0};
218 frame.can_dlc = msg.
dlc;
221 for(
int i=0; i < frame.can_dlc;++i)
222 frame.data[i] = msg.
data[i];
224 boost::system::error_code ec;
225 boost::asio::write(
socket_, boost::asio::buffer(&frame,
sizeof(frame)),boost::asio::transfer_all(), ec);
239 for(
int i=0;i<frame_.can_dlc && i < 8; ++i){
243 if(frame_.can_id & CAN_ERR_FLAG){
244 input_.
id = frame_.can_id & CAN_EFF_MASK;
247 if (frame_.can_id & fatal_error_mask_) {
static can_err_mask_t parse_error_mask(SettingsConstSharedPtr settings, const std::string &entry, can_err_mask_t defaults)
virtual bool translateError(unsigned int internal_error, std::string &str)
std::array< value_type, 8 > data
array for 8 data bytes with bounds checking
#define ROSCANOPEN_ERROR(name, args)
void setInternalError(unsigned int internal_error)
can_err_mask_t getErrorMask() const
virtual bool doesLoopBack() const
bool init(const std::string &device, bool loopback, can_err_mask_t error_mask, can_err_mask_t fatal_error_mask)
virtual bool init(const std::string &device, bool loopback) override
std::shared_ptr< SocketCANInterface > SocketCANInterfaceSharedPtr
virtual bool isReady() const
virtual void triggerReadSome()
std::shared_ptr< ThreadedSocketCANInterface > ThreadedSocketCANInterfaceSharedPtr
void setErrorCode(const boost::system::error_code &error)
enum can::State::DriverState driver_state
void setDriverState(State::DriverState state)
std::shared_ptr< const Settings > SettingsConstSharedPtr
can_err_mask_t getFatalErrorMask() const
std::shared_ptr< SocketCANDriver > SocketCANDriverSharedPtr
can_err_mask_t fatal_error_mask_
void readFrame(const boost::system::error_code &error)
void frameReceived(const boost::system::error_code &error)
virtual bool enqueue(const Frame &msg)
unsigned char dlc
len of data
static SettingsConstSharedPtr create()
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override
can_err_mask_t error_mask_
boost::asio::posix::stream_descriptor socket_