Select Git revision
main.c 8.06 KiB
/*-----------------------------------------------------------------------------
* File: main.c
*
* This file is part of Sensapex smcp1_proxy codebase.
*
* Created by Veli-Matti Kananen
* Copyright (c) 2023-2024 Sensapex Oy. All rights reserved.
*---------------------------------------------------------------------------*/
#include "executor.h"
#include "functions.h"
#include "misc.h"
#include "smcp1.h"
#include "tmcl.h"
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <sys/time.h>
#define DEFAULT_SERIAL_PORT "/dev/ttyACM0"
#define DEFAULT_UDP_ADDRESS SMCP1_BROADCAST_ADDR
#define DEFAULT_UDP_PORT SMCP1_DEF_UDP_PORT
struct sigaction sa;
pthread_t threadUdp;
pthread_t threadExecComm;
pthread_t threadExec0;
pthread_t threadExec1;
pthread_t threadExec2;
pthread_t threadExec3;
static void printUsage();
static void heartBeatHandler(int signum);
static mcu_error initThreads();
int main(int argc, char *argv[])
{
int status = 0;
int opt;
static unsigned int udp_port = DEFAULT_UDP_PORT;
static char serial_device[20] = DEFAULT_SERIAL_PORT;
static char udp_address[20] = DEFAULT_UDP_ADDRESS;
static unsigned int heartbeat_freq_ms = 2000;
static unsigned int mode = 0;
printf("Starting smcpv1-proxy v%s\n", smcp1GetVersion());
while ((opt = getopt(argc, argv, "u:s:p:b:n:h")) != EOF && status == 0)
{
bool parseError = false;
char *endPtr;
switch (opt)
{
case 'u': // UDP address
strncpy(udp_address, optarg, sizeof(udp_address));
break;
case 's': // serial port
strncpy(serial_device, optarg, sizeof(serial_device));
break;
case 'p': // udp port
udp_port = strtoul(optarg, &endPtr, 10);
parseError = ((optarg == endPtr) || *endPtr != '\0');
break;
case 'n': // notification frequency in ms heartbeat_freq_ms = strtoul(optarg, &endPtr, 10);
parseError = ((optarg == endPtr) || *endPtr != '\0');
break;
case 'h':
printUsage();
status = 2;
break;
case '?': // invalid/unknown option
fprintf(stderr, "Invalid option '-%c'\n", optopt);
status = EXIT_FAILURE;
break;
default:
break;
}
if (parseError)
{
fprintf(stderr, "Invalid value '%s' for option '-%c'\n", optarg, opt);
status = EXIT_FAILURE;
}
}
printf("Starting with values : \n");
printf("\tUDP address : %s\n", udp_address);
printf("\tUDP port : %d\n", udp_port);
printf("\tSerial port : %s\n", serial_device);
printf("\tHeartbeat : %d ms\n", heartbeat_freq_ms);
if (status == 0)
{
printf("Proxy initialization...\r");
fflush(stdout);
proxyInit(udp_port, serial_device, udp_address);
printf("Proxy initialization successfull\n");
printf("System initialization...\r");
fflush(stdout);
initSystem();
printf("System initialization successfull\n");
// Create a heartbeat handler
sa.sa_handler = &heartBeatHandler;
sigaction(SIGALRM, &sa, NULL);
struct itimerval timer;
timer.it_value.tv_sec = 1; // First timer interval (1 sec)
timer.it_value.tv_usec = 0;
timer.it_interval.tv_sec = heartbeat_freq_ms / 1000; // Following timer intervals
timer.it_interval.tv_usec = (heartbeat_freq_ms % 1000) * 1000;
// Init smcpv1 layer
printf("About to initializate threads\r");
fflush(stdout);
initThreads();
printf("Threads initialization successfull\n");
// Start the timer
if (heartbeat_freq_ms)
{
setitimer(ITIMER_REAL, &timer, NULL);
}
PROXY_INIT_COMPLETE = 1;
printf("Proxy is running\n");
pthread_join(threadUdp, NULL);
printf("Closing program... Deinitializing proxy");
proxyDeinit();
printf("Closing program... Done");
} else
{
fprintf(stderr, "Some arguments are incorrect, cannot proceed.\n");
}
return status;
}
void printUsage()
{
printf("Usage: smcp1_proxy [OPTIONS]\n");
printf("Options:\n");
printf("\t-u UDP_ADDRESS\tSpecify the UDP address to use\n");
printf("\t-s SERIAL_PORT\tSpecify the serial port to use\n");
printf("\t-p UDP_PORT\t\tSpecify the UDP port to use\n");
printf("\t-m MODE\t\tSpecify the mode to use\n");
printf("\t-n NOTIF_FREQ\tSpecify the notification frequency to use, in ms\n");
printf("\t-h Display this help message\n");
printf("\nExample:\n");
printf("\tsmcp1_proxy -u 169.254.255.255 -s /dev/ttyS0 -p 55555\n");
}
void heartBeatHandler(int signum)
{
UNUSED(signum);
tmclHeartBeatHandler();
// we just "ping" the serial device to know if it is still plugged in.
int dataWrote = (int)write(serial_fd, NULL, 0);
if (dataWrote < 0)
{
// Handle error: serial device might not be connected, we quit.
SHOULD_EXIT = 1;
}
// Prints a changing heartbeat message to display that the program is still live if nothing is happening
static bool alternate = false;
if (alternate)
{
printf("\theartbeat. \t\t\r");
}
else
{
printf("\theartbeat...\t\t\r");
}
alternate = !alternate;
fflush(stdout);
}
// Create Threads
static void *start_Executor(void *argument)
{
mcu_executorStart((mcu_event_queue)argument);
}
mcu_error initThreads()
{
// ... init Executor
mcu_error status = mcu_executorInit();
assert(status == MCU_ERROR_NONE);
if (status == MCU_ERROR_NONE)
{
// Executor threads
printf("Creating thread for communication between motors...");
int error = pthread_create(&threadExecComm, NULL, start_Executor, (void *)MCU_EVENT_QUEUE_COMM);
if (error)
{
printf("Failed !\n");
} else
{
printf("Success\n");
}
if (!error)
{
printf("Creating thread for handling motor1...");
error = pthread_create(&threadExec0, NULL, start_Executor, (void *)MCU_EVENT_QUEUE_0);
if (error)
{
printf("Failed !\n");
}
else
{
printf("Success\n");
}
}
if (!error)
{
printf("Creating thread for handling motor2...");
error = pthread_create(&threadExec1, NULL, start_Executor, (void *)MCU_EVENT_QUEUE_1);
if (error)
{
printf("Failed !\n");
}
else
{
printf("Success\n");
}
}
if (!error)
{
printf("Creating thread for handling motor3...");
error = pthread_create(&threadExec2, NULL, start_Executor, (void *)MCU_EVENT_QUEUE_2);
if (error)
{
printf("Failed !\n");
}
else
{
printf("Success\n");
}
}
if (!error)
{
printf("Creating thread for handling motor4...");
error = pthread_create(&threadExec3, NULL, start_Executor, (void *)MCU_EVENT_QUEUE_3);
if (error)
{
printf("Failed !\n");
}
else
{
printf("Success\n");
}
}
// UDP thread
if (!error)
{
printf("Creating thread for handling UDP communication with input controller...");
error = pthread_create(&threadUdp, NULL, readUdp, NULL);
if (error)
{
printf("Failed !\n");
}
else
{
printf("Success\n");
} }
if (error)
{
status = MCU_ERROR_NOT_INITIALIZED;
perror("Failed to create some threads for UDP");
}
}
return status;
}