Commit de4c2723 authored by Simon Krissel's avatar Simon Krissel
Browse files

Basic CAN link

parent d7df11f7
CFLAGS += -Wall
CFLAGS += -std=gnu99
CFLAGS += -g
CFLAGS += -I../../include
CFLAGS += -I../../driver/include
CFLAGS += --include=../../include/generated/autoconf.h
LDLIBS += -lpthread
all: can_simu_transmitter
can_simu_transmitter: can_simu_transmitter.c
.PHONY: clean
clean:
rm -f can_simu_transmitter
/* SPDX-License-Identifier: MIT */
/*
* Author: Jonathan Klamroth <jonathan.klamroth@student.hs-rm.de>
* Date: 2021
*/
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <assert.h>
#include <pthread.h>
#include <signal.h>
#include <errno.h>
#include <time.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <system.h>
#include <motorcontroller_can.h>
#include <arpa/inet.h>
#define HEARTBEAT_WATCHDOG_TIMEOUT_MS (500)
#define SERVERPORT (19991)
static uint16_t mc_status = 0x0000;
static uint32_t mc_torque = 0;
static int32_t mc_speed = 0;
static float mc_speed_f = 0;
static int32_t mc_count = 0;
static float mc_distance_f = 0;
static uint16_t mc_mcu_temp = 1;
static uint16_t mc_motor_temp = 2;
static uint16_t mc_current_phase_a = 3;
static uint16_t mc_current_phase_b = 4;
static uint16_t mc_current_phase_c = 5;
static uint16_t mc_voltage_phase_a = 6;
static uint16_t mc_voltage_phase_b = 7;
static uint16_t mc_voltage_phase_c = 8;
static uint16_t mc_voltage_battery = 9;
static pthread_cond_t heartbeat_watchdog_cond;
static bool heartbeat_watchdog_triggered = false;
pthread_mutex_t mutex;
void set_speed(int val) {
int ret;
if((ret = pthread_mutex_lock(&mutex)) != 0) {
fprintf(stderr, "Error: pthread_mutex_lock(): %d\n", ret);
exit(EXIT_FAILURE);
}
mc_speed = val;
if((ret = pthread_mutex_unlock(&mutex)) != 0) {
fprintf(stderr, "Error: pthread_mutex_unlock(): %d\n", ret);
exit(EXIT_FAILURE);
}
}
int32_t get_speed() {
int ret;
int32_t val;
if((ret = pthread_mutex_lock(&mutex)) != 0) {
fprintf(stderr, "Error: pthread_mutex_lock(): %d\n", ret);
exit(EXIT_FAILURE);
}
val = mc_speed;
if((ret = pthread_mutex_unlock(&mutex)) != 0) {
fprintf(stderr, "Error: pthread_mutex_unlock(): %d\n", ret);
exit(EXIT_FAILURE);
}
return val;
}
void gazebo_create_socket(int* serverfd) {
// Create a IPv4-TCP socket
if ((*serverfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
perror("Error: socket()");
exit(EXIT_FAILURE);
}
struct sockaddr_in sin;
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = htonl(INADDR_ANY);
sin.sin_port = htons(SERVERPORT);
// Now bind the host address
if (bind(*serverfd, (struct sockaddr *) &sin, sizeof(sin)) < 0) {
perror("Error: bind()");
exit(EXIT_FAILURE);
}
// Signal the OS to listen on the port
if(listen(*serverfd, 5) < 0) {
perror("Error: listen()");
exit(EXIT_FAILURE);
}
return;
}
int gazebo_accept_client(int serverfd) {
struct sockaddr_in cli_addr;
unsigned int clilen = sizeof(cli_addr);
printf("Waiting for client.\n");
// Sleep until a new client is waiting to be accepted
int newsockfd = accept(serverfd, (struct sockaddr *) &cli_addr, &clilen);
if (newsockfd < 0) {
perror("Error: accept()");
return -1;
}
// Print the adress and port of the client
char addr_str[32];
inet_ntop(AF_INET, &cli_addr.sin_addr, addr_str, 32);
printf("Client connected: %s:%i\n", addr_str, cli_addr.sin_port);
// Set a timeout for the socket
struct timeval timeout;
timeout.tv_sec = 5;
timeout.tv_usec = 0;
if (setsockopt (newsockfd, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout)) < 0) {
perror("Error: setsockopt()");
}
if (setsockopt (newsockfd, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout)) < 0) {
perror("Error: setsockopt()");
}
return newsockfd;
}
void encode_int32(uint8_t* buf, uint32_t data) {
data = htonl(data);
buf[3] = (data >> 24) & 0xFF;
buf[2] = (data >> 16) & 0xFF;
buf[1] = (data >> 8 ) & 0xFF;
buf[0] = data & 0xFF;
}
int gazebo_send(int sockfd, int32_t msg) {
int32_t ret;
// Sanity check
if (sockfd >= 0) {
uint8_t buf[sizeof(msg)+1];
encode_int32(buf, msg);
buf[sizeof(buf)-1] = '\0';
ret = send(sockfd, buf, sizeof(buf), MSG_NOSIGNAL);
if (ret <= 0) {
perror("Error: send()");
return -1;
}
} else {
fprintf(stderr, "Error: %s:%d Could not get send message, because not connected\n", __FILE__, __LINE__);
return -1;
}
return 0;
}
static void* gazebo_thread (void *arg) {
int serverfd;
gazebo_create_socket(&serverfd);
int client = gazebo_accept_client(serverfd);
int32_t message = 0;
for (;;) {
if(heartbeat_watchdog_triggered) {
message = 0;
} else {
message = get_speed();
}
if(gazebo_send(client, message) != 0) {
close(client);
printf("Disconnected.\n");
client = gazebo_accept_client(serverfd);
}
usleep(50000); //0.05 seconds / 20x per second
}
return NULL;
}
// copy/pasted from src/motorcontroller.c and adapted to struct can_frame
static int32_t motorcontroller_can_msg_bits(struct can_frame *msg, uint64_t value, uint8_t offset, uint8_t length) {
uint32_t tmp;
uint32_t mask;
int bits;
int i;
if (msg == NULL || (offset+length) > 8*msg->can_dlc) {
return -1;
}
for (i=0; i<msg->can_dlc; i++) {
if (offset >= 8) {
offset -= 8;
continue;
}
bits = 8-offset;
if (bits > length) {
bits = length;
}
mask = BITS_MASK(bits, offset);
tmp = BITS(value, mask, offset);
value >>= bits;
msg->data[i] = (msg->data[i] & ~mask) | tmp;
offset = 0;
length -= bits;
if (length == 0) {
break;
}
}
assert(length == 0);
return 0;
}
// copy/pasted from src/motorcontroller.c and adapted to struct can_frame
static uint64_t motorcontroller_can_msg_bits_inv(struct can_frame *msg, uint8_t offset, uint8_t length, bool sign_extend) {
uint64_t value = 0;
uint64_t tmp;
int i;
int shift;
if (msg == NULL || length == 0) {
return 0;
}
for (i=0; i<msg->can_dlc; i++) {
shift = i*8 - offset;
tmp = msg->data[i] & BITS_MASK(8, 0);
tmp = (shift > 0 ? tmp<<shift : tmp>>(-shift));
value |= tmp;
}
value &= ~(UINT64_MAX<<length);
if (sign_extend) {
if (value & BIT64(length-1)) {
value |= (UINT64_MAX<<length);
}
}
return value;
}
static void * heartbeat_watchdog_thread (void *arg) {
int ret;
pthread_mutex_t mutex;
struct timespec ts_timeout;
ret = pthread_cond_init(&heartbeat_watchdog_cond, NULL);
assert(ret == 0);
ret = pthread_mutex_init(&mutex, NULL);
assert(ret == 0);
ret = pthread_mutex_lock(&mutex);
assert(ret == 0);
for (;;) {
ret = clock_gettime(CLOCK_REALTIME, &ts_timeout);
assert(ret >= 0);
ts_timeout.tv_nsec += (HEARTBEAT_WATCHDOG_TIMEOUT_MS % 1000) * 1000000UL;
ts_timeout.tv_sec += HEARTBEAT_WATCHDOG_TIMEOUT_MS / 1000;
if (ts_timeout.tv_nsec >= 1000000000UL) {
ts_timeout.tv_sec += 1;
ts_timeout.tv_nsec -= 1000000000UL;
}
ret = pthread_cond_timedwait(&heartbeat_watchdog_cond, &mutex, &ts_timeout);
if (ret == ETIMEDOUT) {
if (!heartbeat_watchdog_triggered) {
heartbeat_watchdog_triggered = true;
printf("heartbeat watchdog triggered\n");
}
} else {
assert(ret == 0);
heartbeat_watchdog_triggered = false;
}
}
return NULL;
}
static void heartbeat_watchdog_reset (void) {
int ret;
ret = pthread_cond_signal(&heartbeat_watchdog_cond);
assert(ret == 0);
}
static int can_init (const char *ifname) {
int s;
int ret;
struct sockaddr_can addr;
struct ifreq ifr;
s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
assert(s >= 0);
strcpy(ifr.ifr_name, ifname);
ret = ioctl(s, SIOCGIFINDEX, &ifr);
assert(ret >= 0);
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
ret = bind(s, (struct sockaddr *) &addr, sizeof(addr));
assert(ret >= 0);
return s;
}
// very simple simulation of a motor
static void * motor_sim_thread (void *arg) {
uint32_t t;
float torque;
for (;;) {
if (mc_status & MOTORCONTROLLER_STATUS_ENABLED) {
t = mc_torque;
} else {
t = 0;
}
if (t != 0) {
torque = (((int32_t) t)-1500)/200.0; // +/- 1.0
if (torque > 0.001 || torque < -0.001) {
mc_speed_f += torque/200;
}
}
mc_speed_f -= mc_speed_f/300.0;
if (mc_speed_f > 0.001 || mc_speed_f < 0.001) {
mc_distance_f += mc_speed_f/100.0;
}
set_speed((mc_speed_f*1000)+0.5); // mm/s
mc_count = (mc_distance_f*1000)+0.5; // mm
usleep(10*1000);
}
return NULL;
}
static void * recv_thread (void *arg) {
int s;
struct can_frame frame;
int nbytes;
int node_id, id, group_id;
uint16_t control;
uint32_t torque;
s = *((int*) arg);
for (;;) {
nbytes = read(s, &frame, sizeof(struct can_frame));
assert(nbytes >= 0);
node_id = BITS_INV(frame.can_id, BITS_MASK(5, 0), 0);
id = BITS_INV(frame.can_id, BITS_MASK(3, 5), 5);
group_id = BITS_INV(frame.can_id, BITS_MASK(3, 8), 8);
if (node_id != CONFIG_MOTORCONTROLLER_CAN_TX_NODE_ID) {
continue;
}
if (group_id == MOTORCONTROLLER_CAN_GROUP_ID_TX_CONTROL) {
switch (id) {
case MOTORCONTROLLER_CAN_ID_TX_CONTROL:
control = motorcontroller_can_msg_bits_inv(&frame, 0, 16, false);
printf("set control: %04x\n", control);
if (control & MOTORCONTROLLER_CONTROL_ENABLE) {
torque = 0;
mc_status |= MOTORCONTROLLER_STATUS_ENABLED;
}
if (control & MOTORCONTROLLER_CONTROL_DISABLE) {
mc_status &= ~MOTORCONTROLLER_STATUS_ENABLED;
}
break;
case MOTORCONTROLLER_CAN_ID_TX_TORQUE:
torque = motorcontroller_can_msg_bits_inv(&frame, 0, 32, false);
printf("set torque: %u [speed=%.3f] [hb_wdt_tgrd=%d]\n", torque, mc_speed_f, heartbeat_watchdog_triggered);
// TODO
mc_torque = torque;
break;
}
} else if (group_id == MOTORCONTROLLER_CAN_GROUP_ID_TX_GENERIC) {
switch (id) {
case MOTORCONTROLLER_CAN_ID_TX_HEARTBEAT:
heartbeat_watchdog_reset();
break;
}
}
}
}
static void * heartbeat_thread (void *arg) {
int s;
struct can_frame frame;
int nbytes;
s = *((int*) arg);
frame.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_GENERIC, MOTORCONTROLLER_CAN_ID_RX_HEARTBEAT, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame.can_dlc = 0;
for (;;) {
nbytes = write(s, &frame, sizeof(struct can_frame));
assert(nbytes >= 0);
usleep(100*1000);
}
}
static void * status_thread (void *arg) {
int s;
int ret;
struct can_frame frame_status, frame_speed, frame_temperature, frame_current, frame_voltage;
int nbytes;
s = *((int*) arg);
frame_status.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_STATUS, MOTORCONTROLLER_CAN_ID_RX_STATUS, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame_status.can_dlc = 2;
frame_speed.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_STATUS, MOTORCONTROLLER_CAN_ID_RX_SPEED, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame_speed.can_dlc = 7;
frame_temperature.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_STATUS, MOTORCONTROLLER_CAN_ID_RX_TEMPERATURE, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame_temperature.can_dlc = 3;
frame_current.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_STATUS, MOTORCONTROLLER_CAN_ID_RX_CURRENT, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame_current.can_dlc = 5;
frame_voltage.can_id = MOTORCONTROLLER_CAN_ID(MOTORCONTROLLER_CAN_GROUP_ID_RX_STATUS, MOTORCONTROLLER_CAN_ID_RX_VOLTAGE, CONFIG_MOTORCONTROLLER_CAN_RX_NODE_ID);
frame_voltage.can_dlc = 6;
for (;;) {
ret = motorcontroller_can_msg_bits(&frame_status, mc_status, 0, 16);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_speed, get_speed(), 0, 18);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_speed, mc_count, 18, 32);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_temperature, mc_mcu_temp, 0, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_temperature, mc_motor_temp, 12, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_current, mc_current_phase_a, 0, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_current, mc_current_phase_b, 12, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_current, mc_current_phase_c, 24, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_voltage, mc_voltage_phase_a, 0, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_voltage, mc_voltage_phase_b, 12, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_voltage, mc_voltage_phase_c, 24, 12);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_voltage, mc_voltage_battery, 36, 12);
assert(ret >= 0);
nbytes = write(s, &frame_status, sizeof(struct can_frame));
assert(nbytes >= 0);
nbytes = write(s, &frame_speed, sizeof(struct can_frame));
assert(nbytes >= 0);
nbytes = write(s, &frame_temperature, sizeof(struct can_frame));
assert(nbytes >= 0);
nbytes = write(s, &frame_current, sizeof(struct can_frame));
assert(nbytes >= 0);
nbytes = write(s, &frame_voltage, sizeof(struct can_frame));
assert(nbytes >= 0);
usleep(10*1000);
}
}
int main (int argc, char *argv[]) {
int s;
int ret;
pthread_t heartbeat_watchdog_thread_id;
pthread_t motor_sim_thread_id;
pthread_t recv_thread_id;
pthread_t heartbeat_thread_id;
pthread_t status_thread_id;
pthread_t gazebo_thread_id;
assert(argc == 2);
s = can_init(argv[1]);
assert(s >= 0);
if((ret = pthread_mutex_init(&mutex, NULL)) != 0) { //mutex for motor_sim and gazebo threads
fprintf(stderr, "Error: pthread_mutex_init(): %d\n", ret);
exit(EXIT_FAILURE);
}
ret = pthread_create(&heartbeat_watchdog_thread_id, NULL, heartbeat_watchdog_thread, NULL);
assert(ret >= 0);
ret = pthread_create(&motor_sim_thread_id, NULL, motor_sim_thread, NULL);
assert(ret >= 0);
ret = pthread_create(&recv_thread_id, NULL, recv_thread, &s);
assert(ret >= 0);
ret = pthread_create(&heartbeat_thread_id, NULL, heartbeat_thread, &s);
assert(ret >= 0);
ret = pthread_create(&status_thread_id, NULL, status_thread, &s);
assert(ret >= 0);
ret = pthread_create(&gazebo_thread_id, NULL, gazebo_thread, NULL);
assert(ret >= 0);
for(;;);
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment