void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
//serial_port_flush_output(fmssp0);
serial_port_close(fmssp0);
fmssp0 = serial_port_new();
if (portnum == 0) {
printf("opening ttyUSB0 on uart0 at %d\n",GPS_BAUD);
serial_port_open_raw(fmssp0,"/dev/ttyUSB0",GPS_BAUD);
}
if (portnum == 1) {
printf("opening ttyUSB1 on uart0 at %d\n",GPS_BAUD);
serial_port_open_raw(fmssp0,"/dev/ttyUSB1",GPS_BAUD);
}
uart0_fd = (int)fmssp0->fd;
// initialize the transmit data queue
uart0_tx_extract_idx = 0;
uart0_tx_insert_idx = 0;
uart0_tx_running = FALSE;
// initialize the receive data queue
uart0_rx_extract_idx = 0;
uart0_rx_insert_idx = 0;
}
static void __attribute__((noreturn)) die(int code) {
zdestroy(zsock_uart, NULL);
zdestroy(zsock_log, NULL);
zdestroy(zsock_lisa, NULL);
serial_port_close();
printf("%d TX fails; %d RX fails.\n", txfails, rxfails);
printf("Moriturus te saluto!\n");
exit(code);
}
void uart_periph_set_baudrate(struct uart_periph* p, uint16_t baud, bool_t hw_flow_control __attribute__ ((unused))) {
struct FmsSerialPort* fmssp;
// close serial port if already open
if (p->reg_addr != NULL) {
fmssp = (struct FmsSerialPort*)(p->reg_addr);
serial_port_close(fmssp);
serial_port_free(fmssp);
}
// open serial port
fmssp = serial_port_new();
// use register address to store SerialPort structure pointer...
p->reg_addr = (void*)fmssp;
//TODO: set device name in application and pass as argument
printf("opening %s on uart0 at %d\n",p->dev,baud);
serial_port_open_raw(fmssp,p->dev,baud);
}
int main(
int argc,
char *argv[]
) {
int ret = EXIT_FAILURE;
serial_port_t port;
struct model *model = malloc(sizeof(struct model));
struct activity *activity = malloc(sizeof(struct activity));
model->num_theta_region = CONF_NUM_THETA_REGION;
char chosen_username[NAME_LEN];
char chosen_activity[NAME_LEN];
if (prompt_activity_name(chosen_activity))
goto ERROR;
if (prompt_user_name(chosen_username))
goto ERROR;
strncpy(activity->name, chosen_activity, NAME_LEN);
strncpy(activity->user, chosen_username, NAME_LEN);
activity->name[strlen(chosen_activity)] = '\0';
activity->user[strlen(chosen_username)] = '\0';
if (init_model(model))
goto ERROR;
if (init_activity(activity, model))
goto ERROR;
if (check_file_exists(FILE_CONF_CALIBRATION))
goto ERROR;
if (serial_port_open(CONF_SERIAL_PORT, &port))
goto ERROR;
if (serial_port_configure(port))
goto ERROR;
if (real_time_listen(port, activity, model))
goto ERROR;
file_write_activity_code(activity->files[FILE_ACTIVITY_CODE], activity, model);
ret = EXIT_SUCCESS;
ERROR:
serial_port_close(port);
free_activity(activity);
free_model(model);
return ret;
}
void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud)
{
struct SerialPort *port;
// close serial port if already open
if (periph->reg_addr != NULL) {
port = (struct SerialPort *)(periph->reg_addr);
serial_port_close(port);
serial_port_free(port);
}
// open serial port
port = serial_port_new();
// use register address to store SerialPort structure pointer...
periph->reg_addr = (void *)port;
//TODO: set device name in application and pass as argument
// FIXME: paparazzi baud is 9600 for B9600 while open_raw needs 12 for B9600
// /printf("opening %s on uart0 at termios.h baud value=%d\n", periph->dev, baud);
int ret = serial_port_open_raw(port, periph->dev, baud);
if (ret != 0) {
TRACE("Error opening %s code %d\n", periph->dev, ret);
}
}
int at_serial_port_execute_command(serial_port_t *port, at_serial_port_command_t *at_command)
{
int ret = 0;
serial_port_open(port);
if (!serial_port_is_open(port)) {
piksi_log(LOG_ERR, "Unable to open port: %s", port->port_name);
return -1;
}
int n = (int)write(port->fd, at_command->command, strlen(at_command->command));
n += (int)write(port->fd, "\r", 1);
if (n != (int)strlen(at_command->command) + 1) {
piksi_log(LOG_ERR,
"Unable to write command '%s' to port '%s'",
at_command->command,
port->port_name);
ret = -1;
goto cleanup;
}
at_serial_port_wait_command_response(port, at_command);
if (!at_command->complete) {
piksi_log(LOG_ERR,
"Unable to resolve result of command '%s' to port '%s'. %d characters returned",
at_command->command,
port->port_name,
at_command->result_length);
ret = -1;
goto cleanup;
}
cleanup:
serial_port_close(port);
return ret;
}
int main(int argc, char *argv[]){
write_uart_error_ptr = &write_uart_error; //initialize the function pointer to write error
write_udp_error_ptr = &write_udp_error;
write_decode_error_ptr = &write_decode_error;
write_log_error_ptr = &write_log_error;
//parse arguments
if(argc == 4){
//first argument is always name of program or empty string
connection.server_ip=argv[1];
connection.port_number_lisa_to_pc=atoi(argv[2]);
connection.port_number_pc_to_lisa=atoi(argv[3]);
}else{
printf("wrong parameters: server ip - send port number - receive port number\n");
exit(EXIT_FAILURE);
}
//init log (mount sd card if necessary)
int err = init_log();
LOG_err_handler(err,write_log_error_ptr);
if(err != LOG_ERR_NONE){
exit(EXIT_FAILURE); //mounting SD card failed
}
enable_ptp();
#if LOGGING > 0
//init circular data log buffers
cbInit(cb_read_lisa, CBSIZE);
cbInit(cb_write_lisa, CBSIZE);
cbInit(cb_read_ground, CBSIZE);
cbInit(cb_write_ground, CBSIZE);
#endif
err = serial_port_setup();
UART_err_handler(err,write_uart_error_ptr);
if(err != UART_ERR_NONE){
exit(EXIT_FAILURE);
}
//thread variables
pthread_t thread_lisa_to_pc,thread_data_logging_lisa,thread_data_logging_ground;
//create a second thread which executes lisa_to_pc
if(pthread_create(&thread_lisa_to_pc, NULL, lisa_to_pc,NULL)) {
error_write(FILENAME,"error creating lisa thread");
exit(EXIT_FAILURE);
}
#if LOGGING > 0
//create a third thread which executes data_logging_lisa
if(pthread_create(&thread_data_logging_lisa, NULL, data_logging_lisa,NULL)) {
error_write(FILENAME,"error creating lisa logging thread");
exit(EXIT_FAILURE);
}
//create a fourth thread which executes data_logging_groundstation
if(pthread_create(&thread_data_logging_ground, NULL, data_logging_groundstation,NULL)) {
error_write(FILENAME,"error creating groundstation logging thread");
exit(EXIT_FAILURE);
}
#endif
/*-------------------------START OF FIRST THREAD: PC TO LISA------------------------*/
static UDP udp_server;
uint8_t input_stream[OUTPUT_BUFFER];
ElemType cb_elem = {0};
//init the data decode pointers
init_decoding();
UDP_err_handler(openUDPServerSocket(&udp_server,connection.port_number_pc_to_lisa,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1){
//1. retreive UDP data form PC from ethernet port.
err=receiveUDPServerData(&udp_server,(void *)&input_stream,sizeof(input_stream)); //blocking !!!
UDP_err_handler(err,write_udp_error_ptr);
if(err==UDP_ERR_NONE){
#if LOGGING > 0
if(!cbIsFull(cb_write_ground)){
memcpy (&cb_elem.value, &input_stream, sizeof(input_stream));
cbWrite(cb_write_ground, &cb_elem);
}else{
if(reading_flag_ground==0){
switch_cb_ground_pointers();
}else{
printf("GROUND WRITE WAS NOT READY \n");
}
}
#endif
int new_length = strip_timestamp(input_stream); //lisa expects a package without a timestamp
UART_err_handler(serial_port_write(input_stream,new_length),write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPServerSocket(&udp_server),write_udp_error_ptr);
/*------------------------END OF FIRST THREAD------------------------*/
//wait for the second thread to finish
if(pthread_join(thread_lisa_to_pc, NULL)) {
error_write(FILENAME,"error joining thread_lisa_to_pc");
}
#if LOGGING > 0
//wait for the third thread to finish
if(pthread_join(thread_data_logging_lisa, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_lisa");
}
//wait for the fourth thread to finish
if(pthread_join(thread_data_logging_ground, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_ground");
}
//free circular buffers
cbFree(cb_read_lisa);
cbFree(cb_write_lisa);
cbFree(cb_read_ground);
cbFree(cb_write_ground);
#endif
return 0;
}
/************************************
* FUNCTIONS
* **********************************/
static void *lisa_to_pc(void *arg){
/*-------------------------START OF SECOND THREAD: LISA TO PC------------------------*/
static UDP udp_client;
int message_length;
ElemType cb_elem = {0};
uint8_t input_buffer[INPUT_BUFFER_SIZE];
UDP_err_handler(openUDPClientSocket(&udp_client,connection.server_ip,connection.port_number_lisa_to_pc,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1)
{
message_length = serial_input_get_lisa_data(input_buffer); //blocking !!!
if(message_length > 0){
//to test latency from lisa pull pin high when airspeed package arrives
/*if(serial_input.buffer[3]==AIRSPEED_ETS){
if (test==0){
system("echo 1 > /sys/class/gpio/gpio60/value");
test=1;
}else{
system("echo 0 > /sys/class/gpio/gpio60/value");
test=0;
}
}*/
//add timestamp
message_length=add_timestamp(input_buffer);
//send data to eth port using UDP
UDP_err_handler(sendUDPClientData(&udp_client,input_buffer,message_length),write_udp_error_ptr);
#if LOGGING > 0
//write the data to circual buffer for log thread
if(!cbIsFull(cb_write_lisa)){
memcpy (&cb_elem.value, input_buffer, message_length);
cbWrite(cb_write_lisa, &cb_elem);
}else{
if(reading_flag_lisa==0){
switch_cb_lisa_pointers();
//printf("switching lisa pointers\n");
}else{
printf("LISA WRITE WAS NOT READY \n");
exit(1); //FOR DEBUGGING
}
}
#endif
}else{
//send error message to server: not receiving data on uart port
printf("error on uart, see log...\n"); //FOR DEBUGGING
UART_err_handler(message_length,write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPClientSocket(&udp_client),write_udp_error_ptr);
return NULL;
/*------------------------END OF SECOND THREAD------------------------*/
}