static void *server_to_planebone(void *connection){
/*-------------------------START OF SECOND THREAD: SERVER TO PLANEBONE------------------------*/
Connection *conn=(Connection *)connection;
static UDP udp_client;
//1. read data from i don't now where
uint8_t encoded_data[MAX_OUTPUT_STREAM_SIZE];
UDP_err_handler(openUDPClientSocket(&udp_client,conn->planebone_ip,conn->port_number_server_to_planebone,UDP_SOCKET_TIMEOUT),write_error_ptr);
int i=0;
while(1)
{
Output output;
//create test data
output.message.servo_1=-i;
output.message.servo_2=i;
output.message.servo_3=i;
output.message.servo_4=i;
output.message.servo_5=i;
output.message.servo_6=i;
output.message.servo_7=0;
i=i+900;
if(i>12800){
i=0;
}
//2. encode the data
DEC_err_handler(data_encode(output.raw,sizeof(output.raw),encoded_data,SERVER,SERVO_COMMANDS),write_error_ptr);
/*printf("OUTPUT RAW:");
int j;
for(j=0;j<encoded_data[1];j++){
printf("%d ",encoded_data[j]);
}
printf("\n");*/
//3. send data to eth port using UDP
UDP_err_handler(sendUDPClientData(&udp_client,&encoded_data,sizeof(encoded_data)),write_error_ptr);
//usleep(20000); //60 hz
sleep(1);
}
UDP_err_handler(closeUDPClientSocket(&udp_client),write_error_ptr);
return NULL;
/*------------------------END OF SECOND THREAD------------------------*/
}
static void sendError(DEC_errCode err,library lib){
static UDP udp_client;
int message_length;
uint8_t encoded_data[MAX_STREAM_SIZE];
Data data;
Beagle_error error_message;
//encode an error package
error_message.library=lib;
error_message.error_code=err;
data_encode((uint8_t *)&error_message,sizeof(error_message),encoded_data,2,2);
message_length=sizeof(encoded_data);
//send errorcode to server, no error handling here otherwise we get infinite loop try to send error
openUDPClientSocket(&udp_client,connection.server_ip,connection.port_number_lisa_to_pc,UDP_SOCKET_TIMEOUT);
sendUDPClientData(&udp_client,&encoded_data,message_length);
closeUDPClientSocket(&udp_client);
}
int main(int argc, char *argv[]){
int port_number;
char *ip_address;
if(argc == 3){
//first argument is always name of program or empty string
ip_address=argv[1];
port_number=atoi(argv[2]);
}else{
printf("wrong parameters: enter destination ip adress and port number\n");
exit(1);
}
union Serial_output {
char buffer[30];
uint32_t set_servo_buffer[7];
} serial_output;
openUDPClientSocket(&udp_client,ip_address,port_number);
while(1){
usleep(500);
serial_output.set_servo_buffer[0]=100;
serial_output.set_servo_buffer[1]=0;
serial_output.set_servo_buffer[2]=0;
serial_output.set_servo_buffer[3]=0;
serial_output.set_servo_buffer[4]=0;
serial_output.set_servo_buffer[5]=0;
serial_output.set_servo_buffer[6]=0;
serial_output.buffer[28]=0;
serial_output.buffer[29]=0;
sendUDPClientData(&udp_client,&serial_output,sizeof(serial_output));
}
closeUDPClientSocket(&udp_client);
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------------------------*/
}