mirror of https://github.com/ArduPilot/ardupilot
92 lines
1.9 KiB
C++
92 lines
1.9 KiB
C++
/*
|
|
socketcan transport for SITL CAN
|
|
*/
|
|
#include "CAN_SocketCAN.h"
|
|
|
|
#if HAL_NUM_CAN_IFACES && HAL_CAN_WITH_SOCKETCAN
|
|
|
|
#include <net/if.h>
|
|
#include <poll.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <sys/ioctl.h>
|
|
#include <linux/can.h>
|
|
#include <errno.h>
|
|
#include <stdlib.h>
|
|
#include "CAN_SocketCAN.h"
|
|
|
|
/*
|
|
initialise socketcan transport
|
|
*/
|
|
bool CAN_SocketCAN::init(uint8_t instance)
|
|
{
|
|
struct sockaddr_can addr {};
|
|
struct ifreq ifr {};
|
|
int ret;
|
|
|
|
fd = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
|
|
if (fd < 0) {
|
|
goto fail;
|
|
}
|
|
|
|
snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "vcan%u", instance);
|
|
ret = ioctl(fd, SIOCGIFINDEX, &ifr);
|
|
if (ret == -1) {
|
|
goto fail;
|
|
}
|
|
|
|
addr.can_family = AF_CAN;
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
|
|
|
ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
|
|
if (ret == -1) {
|
|
goto fail;
|
|
}
|
|
|
|
return true;
|
|
|
|
fail:
|
|
if (fd != -1) {
|
|
close(fd);
|
|
fd = -1;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
send a CAN frame
|
|
*/
|
|
bool CAN_SocketCAN::send(const AP_HAL::CANFrame &frame)
|
|
{
|
|
if (frame.canfd) {
|
|
// not supported on socketcan
|
|
return false;
|
|
}
|
|
struct can_frame transmit_frame {};
|
|
transmit_frame.can_id = frame.id;
|
|
transmit_frame.can_dlc = frame.dlc;
|
|
|
|
const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc);
|
|
memcpy(transmit_frame.data, frame.data, data_length);
|
|
|
|
return ::write(fd, &transmit_frame, sizeof(transmit_frame)) == sizeof(transmit_frame);
|
|
}
|
|
|
|
/*
|
|
receive a CAN frame
|
|
*/
|
|
bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame)
|
|
{
|
|
struct can_frame receive_frame;
|
|
const ssize_t ret = ::read(fd, &receive_frame, sizeof(receive_frame));
|
|
if (ret != sizeof(receive_frame)) {
|
|
return false;
|
|
}
|
|
|
|
// run constructor to initialise
|
|
new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false);
|
|
return true;
|
|
}
|
|
|
|
#endif // HAL_NUM_CAN_IFACES
|