uint32_t CANDev::waitForReply(uint32_t can_id, uint8_t *data) {
struct can_frame frame;
// search frame buffer
for(size_t i = 0; i < frame_buf.size(); i++) {
if(frame_buf[i].can_id == can_id) {
memcpy(data, frame_buf[i].data, frame_buf[i].can_dlc);
frame_buf.erase(frame_buf.begin()+i);
return frame_buf[i].can_dlc;
}
}
// wait for new data
while(1) {
#if !defined(HAVE_RTNET)
size_t ret = read(dev, (void *)&frame, sizeof(frame));
#else
size_t ret = rt_dev_recv(dev, (void *)&frame, sizeof(frame), 0);
#endif
if(ret != sizeof(frame)) {
continue;
}
if(frame.can_id == can_id) {
memcpy(data, frame.data, frame.can_dlc);
return frame.can_dlc;
}
frame_buf.push_back(frame);
}
}
uint32_t CANDev::readMultiFrameData(uint32_t can_id, uint8_t *data,
const CANDev::Header &header) {
struct can_frame frame;
if (multi_buf_size <= 0) {
while (1) {
size_t ret = rt_dev_recv(dev, reinterpret_cast<void*>(&frame),
sizeof(frame), 0);
if (ret != sizeof(frame)) {
return 0;
}
if (frame.can_id == can_id && header[0] == frame.data[0]
&& header[1] == frame.data[1] && header[2] == frame.data[2]
&& header[3] == frame.data[3]) {
multi_frame_buf[0] = frame;
multi_buf_size = 1;
break;
}
}
}
while (1) {
size_t ret = rt_dev_recv(dev, reinterpret_cast<void*>(&frame),
sizeof(frame), 0);
if (ret != sizeof(frame)) {
return 0;
}
if (frame.can_id == can_id) {
multi_frame_buf[multi_buf_size] = frame;
multi_buf_size++;
if (multi_buf_size == multi_frame_buf.size()) {
for (int i = 0, frame_idx = 0; i < multi_nbytes; i += 8, frame_idx++) {
memcpy(&data[i], multi_frame_buf[frame_idx].data,
std::min(8u, multi_nbytes - i));
}
multi_buf_size = 0;
return multi_nbytes;
}
}
}
}
int CANSocket::receiveRaw(int& busId, unsigned char* data, size_t& len, bool blocking) const
{
BARRETT_SCOPED_LOCK(mutex);
struct can_frame frame;
int ret = rt_dev_recv(handle, (void *) &frame, sizeof(can_frame_t), blocking ? 0 : MSG_DONTWAIT);
if (ret < 0) {
switch (ret) {
case -EAGAIN: // -EWOULDBLOCK
//logMessage("CANSocket::%s: "
// "rt_dev_recv(): no data available during non-blocking read")
// % __func__;
return 1;
break;
case -ETIMEDOUT:
logMessage("CANSocket::%s: "
"rt_dev_recv(): timed out")
% __func__;
return 2;
break;
case -EBADF:
logMessage("CANSocket::%s: "
"rt_dev_recv(): aborted because socket was closed")
% __func__;
return 2;
break;
default:
logMessage("CANSocket::%s: "
"rt_dev_recv(): (%d) %s")
% __func__ % -ret % strerror(-ret);
return 2;
break;
}
}
busId = frame.can_id;
len = frame.can_dlc;
memcpy(data, frame.data, len);
if (frame.can_id & CAN_ERR_FLAG) {
if (frame.can_id & CAN_ERR_BUSOFF) {
logMessage("CANSocket::%s: bus-off") % __func__;
}
if (frame.can_id & CAN_ERR_CRTL) {
logMessage("CANSocket::%s: controller problem") % __func__;
}
return 2;
}
return 0;
}
GDC_return_codes GDCNetwork::checkFootSensorSingleReceivedMessage(int foot_number)
{
char response_buffer[MAX_MSG_SIZE];
memset(response_buffer, 0, MAX_MSG_SIZE);
int size;
bool received = false;
//while there are messages
while(1)
{
size = rt_dev_recv(receiving_sockets_foot_sensor_[foot_number], response_buffer, MAX_MSG_SIZE, 0);
if(size < 0)
{
if(received)
return SUCCESS;
else if ((size == -EWOULDBLOCK) | (size == -EAGAIN))
{
// no msg was available and we did not receive any yet
//////////////////Removed - Akshara.
//printf("No message for foot number >%i<.\n", foot_number);
return UDP_TIMEOUT_ERROR;
}
else
{
printf("GDCNetwork::checkFootSensorSingleReceivedMessage>> error in receiving message - error %d\n", size);
return UDP_RECEIVE_SYSCALL_ERROR;//another error occured
}
}
else
{
//update state
FootSensorMsg tmp_msg;
tmp_msg.setReceivedMessage(response_buffer, size, foot_sensor_states_[foot_number]);
++num_of_received_foot_messages_[foot_number];
received = true;
}
}
return SUCCESS;
}
GDC_return_codes GDCNetwork::checkForSingleReceivedMessage(int card_number)
{
char response_buffer[MAX_MSG_SIZE];
memset(response_buffer, 0, MAX_MSG_SIZE);
int size;
bool received = false;
//while there are messages so we guarantee we flush the buffer
while(1)
{
size = rt_dev_recv(receiving_sockets_[card_number], response_buffer, MAX_MSG_SIZE, 0);
if(size < 0)
{
if(received)
return SUCCESS;
else if ((size == -EWOULDBLOCK) | (size == -EAGAIN))
{ // no msg was available
///////////////////// To avoid output - Akshara
//printf("No message in socket >%i<.\n", card_number);
return UDP_TIMEOUT_ERROR;
}
else
{
printf("error in receiving message - error %d\n", size);
return UDP_RECEIVE_SYSCALL_ERROR;//another error occured
}
}
else
{
GDCMsg tmp_msg;
tmp_msg.setReceivedMessage(response_buffer, size, gdc_card_states_[card_number]);
++num_of_received_messages_[card_number];
received = true;
}
}
return SUCCESS;
}
uint32_t CANDev::waitForReply(uint32_t can_id, uint8_t *data) {
struct can_frame frame;
// search frame buffer
for (size_t i = 0; i < buf_size; i++) {
if (frame_buf[i].can_id == can_id) {
memcpy(data, frame_buf[i].data, frame_buf[i].can_dlc);
uint8_t can_dlc = frame_buf[i].can_dlc;
// erase
for (int j = i + 1; j < buf_size; j++) {
frame_buf[j - 1] = frame_buf[j];
}
buf_size--;
return can_dlc;
}
}
// wait for new data
while (1) {
size_t ret = rt_dev_recv(dev, reinterpret_cast<void*>(&frame),
sizeof(frame), 0);
if (ret != sizeof(frame)) {
return 0;
}
if (frame.can_id == can_id) {
memcpy(data, frame.data, frame.can_dlc);
return frame.can_dlc;
}
if (buf_size >= frame_buf.size()) {
// std::cout << "CANDev::waitForReply(" << dev_name_ << ", " << name_ << "): buffer is full" << std::endl;
// std::cout << std::endl;
return 0;
}
frame_buf[buf_size] = frame;
buf_size++;
}
}
int main(int argc, char *argv[])
{
RT_TASK *task;
RTIME trp, max = 0, min = 1000000000, avrg = 0;
int sockfd, ret, hard_timer_running, i;
struct sockaddr_in local_addr, server_addr;
struct { long long count; char msg[100]; } msg = { 0LL, "this message was sent using rtnet-rtai." };
signal(SIGTERM, sigh);
signal(SIGINT, sigh);
signal(SIGHUP, sigh);
/* Set address structures to zero. */
memset(&local_addr, 0, sizeof(struct sockaddr_in));
memset(&server_addr, 0, sizeof(struct sockaddr_in));
/* Check arguments and set addresses. */
if (argc == 6) {
local_addr.sin_family = AF_INET;
local_addr.sin_addr.s_addr = INADDR_ANY;
local_addr.sin_port = htons(atoi(argv[1]));
server_addr.sin_family = AF_INET;
inet_aton(argv[2], &server_addr.sin_addr);
server_addr.sin_port = htons(atoi(argv[3]));
NR_TRIPS = atoi(argv[4]);
PERIOD = atoi(argv[5])*1000LL;
} else {
fprintf(stderr,
"Usage: "
"%s <local-port> "
"<server-ip> <server-port> "
"<number-of-trips> <sending-period-us>\n",
argv[0]);
return 1;
}
/* Create new socket. */
sockfd = rt_dev_socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
printf("Error opening socket: %d\n", sockfd);
return 1;
}
/* Link the Linux process to RTAI. */
if (!(hard_timer_running = rt_is_hard_timer_running())) {
start_rt_timer(0);
}
task = rt_thread_init(nam2num("SMPCLT"), 1, 0, SCHED_OTHER, 0xFF);
if (task == NULL) {
rt_dev_close(sockfd);
printf("CANNOT LINK LINUX SIMPLECLIENT PROCESS TO RTAI\n");
return 1;
}
/* Lock allocated memory into RAM. */
printf("RTnet, simpleclient for RTAI (user space).\n");
mlockall(MCL_CURRENT|MCL_FUTURE);
/* Switch over to hard realtime mode. */
rt_make_hard_real_time();
/* Bind socket to local address specified as parameter. */
ret = rt_dev_bind(sockfd, (struct sockaddr *)&local_addr, sizeof(struct sockaddr_in));
/* Specify destination address for socket; needed for rt_socket_send(). */
rt_dev_connect(sockfd, (struct sockaddr *) &server_addr, sizeof(struct sockaddr_in));
/* Send messages */
for (i = 1; i <= NR_TRIPS && !end; i++) {
rt_sleep(nano2count(PERIOD));
msg.count = i;
trp = rt_get_time_ns();
rt_dev_send(sockfd, &msg, sizeof(long long) + strlen(msg.msg) + 1, 0);
rt_dev_recv(sockfd, &msg, sizeof(msg), 0);
trp = (msg.count - trp)/1000;
if (trp < min) min = trp;
if (i > 3 && trp > max) max = trp;
avrg += trp;
printf("Client received: trip time %lld (us), %s\n", trp, msg.msg);
}
msg.count = -1;
rt_dev_send(sockfd, &msg, sizeof(long long) + strlen(msg.msg) + 1, 0);
/* Switch over to soft realtime mode. */
rt_make_soft_real_time();
/* Close socket, must be in soft-mode because socket was created as non-rt. */
rt_dev_close(sockfd);
/* Unlink the Linux process from RTAI. */
if (!hard_timer_running) {
stop_rt_timer();
}
rt_task_delete(task);
printf("Min trip time %lld, Max trip time %lld, Average trip time %lld\n", min, max, avrg/i);
return 0;
}
