Commit 27a4c119 authored by Simon Krissel's avatar Simon Krissel
Browse files

Merge branch 'testing' into 'master'

Finished development of can transmitter

See merge request skris001/carolocupFirmware!1
parents d7df11f7 cae2e715
......@@ -19,3 +19,6 @@ install
.frama-c
.wp-session_*
.lia.cache
misc/can_simu_transmitter/protobuf
misc/can_simu_transmitter/can_simu_transmitter
.vscode
CFLAGS += -Wall
CFLAGS += -std=gnu99
CFLAGS += -g
CFLAGS += -I../../include
CFLAGS += -I../../driver/include
CFLAGS += --include=../../include/generated/autoconf.h
CFLAGS += --include=protobuf/can.pb-c.h
LDLIBS += -lpthread
LDLIBS += -lprotobuf-c
all: can_proto can_simu_transmitter
can_simu_transmitter: protobuf/can.pb-c.c can_simu_transmitter.c
can_proto: can.proto
@mkdir -p protobuf/
protoc *.proto --c_out=./protobuf/ --python_out=./protobuf/
.PHONY: clean
clean:
rm -f can_simu_transmitter
syntax = "proto2";
message CAN {
required int32 speed = 1;
required int32 mc_count = 2;
}
#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 <stdatomic.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>
// Protobuf
#include "protobuf/can.pb-c.h"
#define HEARTBEAT_WATCHDOG_TIMEOUT_MS (500)
#define SERVERPORT (19991)
// These values are accessed by multiple threads, so they require synchronisation / atomic access
static atomic_ushort mc_status = 0x0000;
static atomic_uint mc_torque = 0;
static atomic_int mc_speed = 0;
static atomic_int mc_count = 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 atomic_bool heartbeat_watchdog_triggered = false;
static atomic_bool running = true;
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)];
encode_int32(buf, msg);
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;
}
int read_exactly_n_bytes(int client, uint32_t n, uint8_t *dst) {
uint32_t bytesRead = 0;
int32_t ret;
// Sanity check
if (client >= 0) {
while (bytesRead < n) {
ret = read(client, dst + bytesRead, n - bytesRead);
if (ret <= 0) {
// EINTR means the reading-process was interrupted but can continue.
if (errno != EINTR) {
perror("Error: read()");
printf("client disconnected\n");
close(client);
return -1;
}
}
bytesRead += ret;
}
} else {
fprintf(stderr, "Error: %s:%d Could not receive data, because no client is connected\n", __FILE__, __LINE__);
return -1;
}
return 0;
}
int gazebo_receive(int client) {
int32_t ret;
uint32_t payload_len = 0;
uint8_t len_arr[sizeof(payload_len)];
// Get the payload length (prefix)
ret = read_exactly_n_bytes(client, sizeof(payload_len), len_arr);
if(ret < 0) {
fprintf(stderr, "Error: %s:%d Could not get payload length\n", __FILE__, __LINE__);
return -2;
}
for(uint64_t i = 0; i < sizeof(payload_len); i++) {
payload_len |= len_arr[i] << i*8;
}
// Convert the payload length to host byteorder and construct the messagebuffer accordingly
payload_len = ntohl(payload_len);
uint8_t buffer[payload_len];
// Now read the payload from the client
ret = read_exactly_n_bytes(client, payload_len, buffer);
if(ret < 0) {
fprintf(stderr, "Error: %s:%d Could not get actual payload\n", __FILE__, __LINE__);
return -2;
}
// Convert the receved bytes into a protobuf-object
CAN *msg;
msg = can__unpack(NULL, payload_len, buffer);
if(msg == NULL) {
fprintf(stderr, "Error: %s:%d Error unpacking message\n", __FILE__, __LINE__);
return -1;
}
// Set the actual variables
mc_count = msg->mc_count;
mc_speed = msg->speed;
printf("Simulation: count: %i, speed: %i\n", mc_count, mc_speed);
// Free the message when we are finished
can__free_unpacked(msg, NULL);
return 0;
}
static void* gazebo_thread (void *arg) {
int serverfd;
int ret;
// Set the timeout for select() to 0 (non-blocking)
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 0;
gazebo_create_socket(&serverfd);
int client = gazebo_accept_client(serverfd);
int32_t message = 0;
while(running) {
if(heartbeat_watchdog_triggered) {
message = 1300;
} else {
message = htonl(mc_torque); // TODO protobuf?
printf("sending: %d\n", message);
}
if(gazebo_send(client, message) != 0) {
close(client);
printf("Disconnected.\n");
client = gazebo_accept_client(serverfd);
}
fd_set crfd;
FD_ZERO(&crfd);
FD_SET(client, &crfd);
// Peek the socket and check if new data is available
ret = select(client+1, &crfd, NULL, NULL, &timeout);
if (ret < 0) {
perror("Error: select():");
continue;
} else if(ret > 0) {
if(FD_ISSET(client, &crfd)) { // This SHOULD not be needed, since we only have one fd for reading
ret = gazebo_receive(client);
}
} // else: no data available
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);
while(running) {
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;
}
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);
while(running) {
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 [hb_wdt_tgrd=%d]\n", torque, heartbeat_watchdog_triggered);
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;
}
}
}
return 0;
}
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;
while(running) {
nbytes = write(s, &frame, sizeof(struct can_frame));
assert(nbytes >= 0);
usleep(100*1000);
}
return 0;
}
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;
while(running) {
ret = motorcontroller_can_msg_bits(&frame_status, mc_status, 0, 16);
assert(ret >= 0);
ret = motorcontroller_can_msg_bits(&frame_speed, mc_torque, 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);
}
return 0;
}
int main (int argc, char *argv[]) {
int s;
int ret;
pthread_t heartbeat_watchdog_thread_id;
pthread_t recv_thread_id;
pthread_t heartbeat_thread_id;
pthread_t status_thread_id;
pthread_t gazebo_thread_id;