Merge branch 'testing' into 'master'

Finished development of can transmitter

See merge request skris001/carolocupFirmware!1

.gitignore

@@ -19,3 +19,6 @@ install
 .frama-c
 .wp-session_*
 .lia.cache
+misc/can_simu_transmitter/protobuf
+misc/can_simu_transmitter/can_simu_transmitter
+.vscode

misc/can_simu_transmitter/Makefile

+
+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
+

misc/can_simu_transmitter/can.proto

+syntax = "proto2";
+
+message CAN {
+    required int32 speed = 1;
+    required int32 mc_count = 2;
+}

misc/can_simu_transmitter/can_simu_transmitter.c

+#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;
+