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;
}
Ejemplo n.º 2
0
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;
}