DEFINE_THREAD_ROUTINE( navdata_update, nomParams )
{
C_RESULT res;
int32_t i, size;
uint32_t cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
struct timeval tv;
#ifdef _WIN32
int timeout_for_windows=1000/*milliseconds*/;
#endif
navdata_t* navdata = (navdata_t*) &navdata_buffer[0];
tv.tv_sec = 1/*second*/;
tv.tv_usec = 0;
res = C_OK;
if( VP_FAILED(vp_com_open(COM_NAVDATA(), &navdata_socket, &navdata_read, &navdata_write)) )
{
printf("VP_Com : Failed to open socket for navdata\n");
res = C_FAIL;
}
if( VP_SUCCEEDED(res) )
{
PRINT("Thread navdata_update in progress...\n");
#ifdef _WIN32
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout_for_windows, sizeof(timeout_for_windows));
/* Added by Stephane to force the drone start sending data. */
if(navdata_write)
{
int sizeinit = 5;
navdata_write( (void*)&navdata_socket, (int8_t*)"Init", &sizeinit );
}
#else
setsockopt((int32_t)navdata_socket.priv, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof(tv));
#endif
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
// if init failed for an handler we set its process function to null
// We keep its release function for cleanup
if( VP_FAILED( ardrone_navdata_handler_table[i].init(ardrone_navdata_handler_table[i].data) ) )
ardrone_navdata_handler_table[i].process = NULL;
i ++;
}
navdata_thread_in_pause = FALSE;
while( VP_SUCCEEDED(res)
&& !ardrone_tool_exit()
&& bContinue )
{
if(navdata_thread_in_pause)
{
vp_os_mutex_lock(&navdata_client_mutex);
num_retries = NAVDATA_MAX_RETRIES + 1;
vp_os_cond_wait(&navdata_client_condition);
vp_os_mutex_unlock(&navdata_client_mutex);
}
if( navdata_read == NULL )
{
res = C_FAIL;
continue;
}
size = NAVDATA_MAX_SIZE;
navdata->header = 0; // Soft reset
res = navdata_read( (void*)&navdata_socket, (int8_t*)&navdata_buffer[0], &size );
#ifdef _WIN32
if( size <= 0 )
#else
if( size == 0 )
#endif
{
// timeout
PRINT("Timeout\n");
ardrone_navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
num_retries++;
}
else
num_retries = 0;
if( VP_SUCCEEDED( res ) )
{
if( navdata->header == NAVDATA_HEADER )
{
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// reset sequence number because of com watchdog
// This code is mandatory because we can have a com watchdog without detecting it on mobile side :
// Reconnection is fast enough (less than one second)
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
ardrone_tool_send_com_watchdog(); // acknowledge
}
if( navdata->sequence > sequence )
{
i = 0;
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &navdata_buffer[0], size - sizeof(navdata_cks_t) );
if( cks == navdata_cks )
{
while( ardrone_navdata_handler_table[i].init != NULL )
{
if( ardrone_navdata_handler_table[i].process != NULL )
ardrone_navdata_handler_table[i].process( &navdata_unpacked );
i++;
}
}
else
{
PRINT("[Navdata] Checksum failed : %d (distant) / %d (local)\n", navdata_cks, cks);
}
}
else
{
PRINT("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
// remaining = sizeof(navdata);
sequence = navdata->sequence;
}
}
}
// Release resources alllocated by handlers
i = 0;
while( ardrone_navdata_handler_table[i].init != NULL )
{
ardrone_navdata_handler_table[i].release();
i ++;
}
}
vp_com_close(COM_NAVDATA(), &navdata_socket);
DEBUG_PRINT_SDK("Thread navdata_update ended\n");
return (THREAD_RET)res;
}
static void* navdata_loop(void *arg)
{
uint8_t msg[NAVDATA_BUFFER_SIZE];
navdata_unpacked_t navdata_unpacked;
unsigned int cks, navdata_cks, sequence = NAVDATA_SEQUENCE_DEFAULT-1;
int sockfd = -1, addr_in_size;
struct sockaddr_in *my_addr, *from;
INFO("NAVDATA thread starting (thread=%d)...\n", (int)pthread_self());
navdata_open_server();
addr_in_size = sizeof(struct sockaddr_in);
navdata_t* navdata = (navdata_t*) &msg[0];
from = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
my_addr = (struct sockaddr_in *)vp_os_malloc(addr_in_size);
assert(from);
assert(my_addr);
vp_os_memset((char *)my_addr,(char)0,addr_in_size);
my_addr->sin_family = AF_INET;
my_addr->sin_addr.s_addr = htonl(INADDR_ANY);
my_addr->sin_port = htons( NAVDATA_PORT );
if((sockfd = socket (AF_INET, SOCK_DGRAM, 0)) < 0){
INFO ("socket: %s\n", strerror(errno));
goto fail;
};
if(bind(sockfd, (struct sockaddr *)my_addr, addr_in_size) < 0){
INFO ("bind: %s\n", strerror(errno));
goto fail;
};
{
struct timeval tv;
// 1 second timeout
tv.tv_sec = 1;
tv.tv_usec = 0;
setsockopt( sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
}
INFO("[NAVDATA] Ready to receive incomming data\n");
while ( nav_thread_alive ) {
int size;
size = recvfrom (sockfd, &msg[0], NAVDATA_BUFFER_SIZE, 0, (struct sockaddr *)from,
(socklen_t *)&addr_in_size);
//INFO ("SIZE %d \n", size);
if( size == 0)
{
INFO ("Navdata lost connection \n");
navdata_open_server();
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
}
// if (size == 24)
// {
// const char cmds[] = "AT*COMWDG=0\r";
// at_write ((int8_t*)cmds, strlen( cmds ));
//
// const char cmds2[] = "AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\r";
// at_write ((int8_t*)cmds2, strlen( cmds2 ));
// }
if( navdata->header == NAVDATA_HEADER )
{
mykonos_state = navdata->ardrone_state;
if(ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_COM_WATCHDOG_MASK) )
{
// INFO ("[NAVDATA] Detect com watchdog\n");
sequence = NAVDATA_SEQUENCE_DEFAULT-1;
if( ardrone_get_mask_from_state(navdata->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) == FALSE )
{
//INFO ("[NAVDATA] Send com watchdog\n");
const char cmds[] = "AT*COMWDG=0\r";
at_write ((int8_t*)cmds, strlen( cmds ));
}
}
if( navdata->sequence > sequence )
{
if ( ardrone_get_mask_from_state( mykonos_state, ARDRONE_NAVDATA_DEMO_MASK ))
{
ardrone_navdata_unpack_all(&navdata_unpacked, navdata, &navdata_cks);
cks = ardrone_navdata_compute_cks( &msg[0], size - sizeof(navdata_cks_t) );
if( cks != navdata_cks )
{
INFO("NAVADATA wrong CKS\n");
}
//INFO("VALUE = %f \n", navdata_unpacked.navdata_demo.theta);
}
}
else
{
INFO ("[Navdata] Sequence pb : %d (distant) / %d (local)\n", navdata->sequence, sequence);
}
sequence = navdata->sequence;
}
}
fail:
vp_os_free(from);
vp_os_free(my_addr);
if (sockfd >= 0){
close(sockfd);
}
if (navdata_udp_socket >= 0){
close(navdata_udp_socket);
navdata_udp_socket = -1;
}
INFO("NAVDATA thread stopping\n");
return NULL;
}
