Commit 69ad1d80 authored by Simon Krissel's avatar Simon Krissel
Browse files

Added receiver

parent 7bf0310a
/* SPDX-License-Identifier: MIT */
/*
* Author: Jonathan Klamroth <jonathan.klamroth@student.hs-rm.de>
* Date: 2021
*/
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
......@@ -14,6 +9,7 @@
#include <signal.h>
#include <errno.h>
#include <time.h>
#include <stdatomic.h>
#include <net/if.h>
#include <sys/types.h>
......@@ -29,17 +25,17 @@
#include <arpa/inet.h>
// Protobuf
#include "can.pb-c.h"
#include "protobuf/can.pb-c.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;
// 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;
......@@ -51,40 +47,9 @@ 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;
static atomic_bool heartbeat_watchdog_triggered = false;
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;
}
static atomic_bool running = true;
void gazebo_create_socket(int* serverfd) {
// Create a IPv4-TCP socket
......@@ -170,23 +135,114 @@ int gazebo_send(int sockfd, int32_t msg) {
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;
for (;;) {
while(running) {
if(heartbeat_watchdog_triggered) {
message = 0;
} else {
message = get_speed();
message = mc_torque;
}
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
}
......@@ -278,7 +334,7 @@ static void * heartbeat_watchdog_thread (void *arg) {
ret = pthread_mutex_lock(&mutex);
assert(ret == 0);
for (;;) {
while(running) {
ret = clock_gettime(CLOCK_REALTIME, &ts_timeout);
assert(ret >= 0);
......@@ -335,40 +391,6 @@ static int can_init (const char *ifname) {
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;
......@@ -379,7 +401,7 @@ static void * recv_thread (void *arg) {
s = *((int*) arg);
for (;;) {
while(running) {
nbytes = read(s, &frame, sizeof(struct can_frame));
assert(nbytes >= 0);
......@@ -407,8 +429,7 @@ static void * recv_thread (void *arg) {
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
printf("set torque: %u [hb_wdt_tgrd=%d]\n", torque, heartbeat_watchdog_triggered);
mc_torque = torque;
break;
}
......@@ -420,6 +441,7 @@ static void * recv_thread (void *arg) {
}
}
}
return 0;
}
static void * heartbeat_thread (void *arg) {
......@@ -432,12 +454,13 @@ static void * heartbeat_thread (void *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 (;;) {
while(running) {
nbytes = write(s, &frame, sizeof(struct can_frame));
assert(nbytes >= 0);
usleep(100*1000);
}
return 0;
}
static void * status_thread (void *arg) {
......@@ -463,11 +486,11 @@ static void * status_thread (void *arg) {
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 (;;) {
while(running) {
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);
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);
......@@ -519,6 +542,7 @@ static void * status_thread (void *arg) {
usleep(10*1000);
}
return 0;
}
......@@ -526,7 +550,6 @@ 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;
......@@ -537,17 +560,9 @@ int main (int argc, char *argv[]) {
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);
......@@ -560,7 +575,17 @@ int main (int argc, char *argv[]) {
ret = pthread_create(&gazebo_thread_id, NULL, gazebo_thread, NULL);
assert(ret >= 0);
for(;;);
while(running) {
;
// TODO
}
pthread_join(heartbeat_watchdog_thread_id, NULL);
pthread_join(recv_thread_id, NULL);
pthread_join(heartbeat_thread_id, NULL);
pthread_join(status_thread_id, NULL);
pthread_join(gazebo_thread_id, NULL);
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