C_RESULT ardrone_general_navdata_process( const navdata_unpacked_t* const pnd )
{
navdata_mode_t current_navdata_state = NAVDATA_BOOTSTRAP;
/* Makes sure the navdata stream will be resumed if the drone is disconnected and reconnected.
* Allows changing the drone battery during debugging sessions. */
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
{
current_navdata_state = NAVDATA_BOOTSTRAP;
}
else
{
current_navdata_state = (ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK)) ? NAVDATA_DEMO : NAVDATA_FULL ;
}
if (current_navdata_state == NAVDATA_BOOTSTRAP && configState == MULTICONFIG_IDLE && navdataState == NAVDATA_REQUEST_IDLE)
{
navdataState = NAVDATA_REQUEST_NEEDED;
}
/* Multiconfig settings */
int configIndex, userNeedInit, appNeedInit;
userNeedInit = 0; appNeedInit = 0;
switch (configState)
{
case MULTICONFIG_GOT_DRONE_VERSION:
PRINTDBG ("Checking drone version ...\n");
// Check if drone version is >= 1.6
if (versionSupportsMulticonfiguration (ardrone_control_config.num_version_soft))
{
PRINTDBG ("Drone supports multiconfig\n");
configState = MULTICONFIG_IN_PROGRESS_LIST;
ARDRONE_TOOL_CUSTOM_CONFIGURATION_GET (configurationCallback);
droneSupportsMulticonfig = 1;
}
else
{
PRINTDBG ("Drone does not support multiconfig\n");
// Drone does not support multiconfig ... don't call init functions because we don't want to mess up things in default config
configState = MULTICONFIG_REQUEST_NAVDATA;
}
break;
case MULTICONFIG_GOT_IDS_LIST:
// At this point, we're sure that the AR.Drone supports multiconfiguration, so we'll wheck if our ids exists, and send them.
PRINTDBG ("Got AR.Drone ID list. Switch->%d,%d,%d. ND->%d\n", sesSwitch, usrSwitch, appSwitch, navdataNeeded);
if (1 == sesSwitch)
{
switchToSession(); // Go to session ...
}
if (1 == appSwitch)
{
if (0 != strcmp(ardrone_control_config_default.application_id, app_id)) // Check for application only if we're not asking for the default one
{
appNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_APPLI].nb_configurations; configIndex++) // Check all existing app_ids
{
PRINTDBG ("Checking application %s (desc : %s)\n", available_configurations[CAT_APPLI].list[configIndex].id,
available_configurations[CAT_APPLI].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_APPLI].list[configIndex].id, app_id))
{
PRINTDBG ("Found our application ... should not init\n");
appNeedInit = 0;
break;
}
}
switchToApplication();
}
else
{
PRINTDBG ("We're requesting default application (%s), do nothing.\n", app_id);
}
}
if (1 == usrSwitch)
{
if (0 != strcmp(ardrone_control_config_default.profile_id, usr_id)) // Check for user only if we're not asking for the default one
{
userNeedInit = 1;
for (configIndex = 0; configIndex < available_configurations[CAT_USER].nb_configurations; configIndex++) // Check all existing user_ids
{
PRINTDBG ("Checking user %s (desc : %s)\n", available_configurations[CAT_USER].list[configIndex].id,
available_configurations[CAT_USER].list[configIndex].description);
if (0 == strcmp(available_configurations[CAT_USER].list[configIndex].id, usr_id))
{
PRINTDBG ("Found our user ... should not init\n");
userNeedInit = 0;
break;
}
}
switchToUser();
}
else
{
PRINTDBG ("We're requesting default user (%s), do nothing.\n", usr_id);
}
}
if (1 == appNeedInit)
{
// Send application defined default values
ardrone_tool_send_application_default();
PRINTDBG ("Creating app. profile on AR.Drone\n");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_desc, app_name, NULL);
}
if (1 == userNeedInit)
{
// Send user defined default values
ardrone_tool_send_user_default();
PRINTDBG ("Creating usr. profile on AR.Drone\n");
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_desc, usr_name, NULL);
}
if (1 == sesSwitch)
{
if (0 != strcmp(ardrone_control_config_default.session_id, ses_id)) // Send session info only if session is not the default one
{
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (session_desc, ses_name, NULL);
// Send session specific default values
ardrone_tool_send_session_default();
}
else
{
PRINTDBG ("We're requesting default session (%s), do nothing.\n", ses_id);
}
}
configState = MULTICONFIG_IN_PROGRESS_IDS;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
case MULTICONFIG_GOT_CURRENT_IDS:
if (0 != strcmp(ardrone_control_config.session_id, ses_id) ||
0 != strcmp(ardrone_control_config.profile_id, usr_id) ||
0 != strcmp(ardrone_control_config.application_id, app_id))
{
configState = MULTICONFIG_GOT_DRONE_VERSION; // We failed at setting up the application ids ... restart (but assume that drone supports multiconfig as we already checked)
}
else if (1 == navdataNeeded)
{
configState = MULTICONFIG_REQUEST_NAVDATA;
}
else
{
configState = MULTICONFIG_IDLE;
}
break;
case MULTICONFIG_NEEDED:
PRINTDBG ("Need to check multiconfig ... request config file\n");
// Get config file for reset
configState = MULTICONFIG_IN_PROGRESS_VERSION;
ARDRONE_TOOL_CONFIGURATION_GET (configurationCallback);
break;
case MULTICONFIG_REQUEST_NAVDATA:
PRINTDBG ("Send application navdata demo/options\n");
// Send application navdata demo/options to start navdatas from AR.Drone
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
if (TRUE == ardrone_application_default_config.navdata_demo)
{ // Send navdata options only for navdata demo mode
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, NULL);
}
configState = MULTICONFIG_IDLE;
break;
case MULTICONFIG_IDLE:
case MULTICONFIG_IN_PROGRESS_LIST:
case MULTICONFIG_IN_PROGRESS_VERSION:
case MULTICONFIG_IN_PROGRESS_IDS:
default:
break;
}
/* Navdata request settings */
switch (navdataState)
{
case NAVDATA_REQUEST_NEEDED:
PRINTDBG ("Resetting navdatas to %s\n", (0 == ardrone_application_default_config.navdata_demo) ? "full" : "demo");
navdataState = NAVDATA_REQUEST_IN_PROGRESS;
switchToSession(); // Resend session id when reconnecting.
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(navdata_demo, &ardrone_application_default_config.navdata_demo, NULL);
if (TRUE == ardrone_application_default_config.navdata_demo)
{ // Send navdata options only for navdata demo mode
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_options, &ardrone_application_default_config.navdata_options, navdataCallback);
}
break;
case NAVDATA_REQUEST_IN_PROGRESS:
case NAVDATA_REQUEST_IDLE:
default:
break;
}
return C_OK;
}
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;
}
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;
}
C_RESULT ardrone_navdata_file_process( const navdata_unpacked_t* const pnd )
{
uint32_t i;
char str[50];
int32_t* locked_ptr;
screen_point_t* point_ptr;
struct timeval time;
gettimeofday(&time,NULL);
if( navdata_file_private == NULL )
return C_FAIL;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP) )
return C_OK;
if( navdata_file == NULL )
{
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
// Handle the case where user asked for a new navdata file
if( navdata_file != navdata_file_private )
{
fclose(navdata_file);
navdata_file = navdata_file_private;
if( ardrone_get_mask_from_state(pnd->ardrone_state, ARDRONE_NAVDATA_DEMO_MASK) )
{
printf("Receiving navdata demo\n");
}
else
{
printf("Receiving all navdata\n");
}
ardrone_navdata_file_print_version();
}
vp_os_memset(&str[0], 0, sizeof(str));
fprintf( navdata_file,"\n" );
fprintf( navdata_file, "%u; %u", (unsigned int) pnd->navdata_demo.ctrl_state, (unsigned int) pnd->ardrone_state );
sprintf( str, "%d.%06d", (int)((pnd->navdata_time.time & TSECMASK) >> TSECDEC), (int)(pnd->navdata_time.time & TUSECMASK) );
fprintf( navdata_file, ";%s", str );
fprintf( navdata_file, "; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u",
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_X],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Y],
(unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Z],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_X],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Y],
(unsigned int) pnd->navdata_raw_measures.raw_gyros[GYRO_Z],
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[0],
(unsigned int) pnd->navdata_raw_measures.raw_gyros_110[1],
(unsigned int) pnd->navdata_raw_measures.vbat_raw,
(unsigned int) pnd->navdata_phys_measures.alim3V3,
(unsigned int) pnd->navdata_phys_measures.vrefEpson,
(unsigned int) pnd->navdata_phys_measures.vrefIDG,
(unsigned int) pnd->navdata_raw_measures.flag_echo_ini,
(unsigned int) pnd->navdata_raw_measures.us_debut_echo,
(unsigned int) pnd->navdata_raw_measures.us_fin_echo,
(unsigned int) pnd->navdata_raw_measures.us_association_echo,
(unsigned int) pnd->navdata_raw_measures.us_distance_echo,
(unsigned int) pnd->navdata_raw_measures.us_courbe_temps,
(unsigned int) pnd->navdata_raw_measures.us_courbe_valeur,
(unsigned int) pnd->navdata_raw_measures.us_courbe_ref );
fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f",
pnd->navdata_phys_measures.accs_temp,
(unsigned int)pnd->navdata_phys_measures.gyro_temp,
pnd->navdata_phys_measures.phys_accs[ACC_X],
pnd->navdata_phys_measures.phys_accs[ACC_Y],
pnd->navdata_phys_measures.phys_accs[ACC_Z],
pnd->navdata_phys_measures.phys_gyros[GYRO_X],
pnd->navdata_phys_measures.phys_gyros[GYRO_Y],
pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_gyros_offsets.offset_g[GYRO_X],
pnd->navdata_gyros_offsets.offset_g[GYRO_Y],
pnd->navdata_gyros_offsets.offset_g[GYRO_Z] );
fprintf( navdata_file, "; % f; % f",
pnd->navdata_euler_angles.theta_a,
pnd->navdata_euler_angles.phi_a);
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %06d; %06d; %06d",
(int) pnd->navdata_references.ref_theta,
(int) pnd->navdata_references.ref_phi,
(int) pnd->navdata_references.ref_psi,
(int) pnd->navdata_references.ref_theta_I,
(int) pnd->navdata_references.ref_phi_I,
(int) pnd->navdata_references.ref_pitch,
(int) pnd->navdata_references.ref_roll,
(int) pnd->navdata_references.ref_yaw );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f",
pnd->navdata_trims.euler_angles_trim_theta,
pnd->navdata_trims.euler_angles_trim_phi,
pnd->navdata_trims.angular_rates_trim_r );
fprintf( navdata_file, "; %04d; %04d; %04d; %04d; %04d; %04u",
(int) pnd->navdata_rc_references.rc_ref_pitch,
(int) pnd->navdata_rc_references.rc_ref_roll,
(int) pnd->navdata_rc_references.rc_ref_yaw,
(int) pnd->navdata_rc_references.rc_ref_gaz,
(int) pnd->navdata_rc_references.rc_ref_ag,
(unsigned int) ui_get_user_input() );
fprintf( navdata_file, "; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03u; %03d; % f; % f; %03d; %03d; %03d; % f; %03d; %03d; %03d; % f; %04d; %04d; %04d; %04d",
(unsigned int) pnd->navdata_pwm.motor1,
(unsigned int) pnd->navdata_pwm.motor2,
(unsigned int) pnd->navdata_pwm.motor3,
(unsigned int) pnd->navdata_pwm.motor4,
(unsigned int) pnd->navdata_pwm.sat_motor1,
(unsigned int) pnd->navdata_pwm.sat_motor2,
(unsigned int) pnd->navdata_pwm.sat_motor3,
(unsigned int) pnd->navdata_pwm.sat_motor4,
(int) pnd->navdata_pwm.gaz_feed_forward,
pnd->navdata_pwm.gaz_altitude,
pnd->navdata_pwm.altitude_integral,
pnd->navdata_pwm.vz_ref,
(int) pnd->navdata_pwm.u_pitch,
(int) pnd->navdata_pwm.u_roll,
(int) pnd->navdata_pwm.u_yaw,
pnd->navdata_pwm.yaw_u_I,
(int) pnd->navdata_pwm.u_pitch_planif,
(int) pnd->navdata_pwm.u_roll_planif,
(int) pnd->navdata_pwm.u_yaw_planif,
pnd->navdata_pwm.u_gaz_planif,
(int) pnd->navdata_pwm.current_motor1,
(int) pnd->navdata_pwm.current_motor2,
(int) pnd->navdata_pwm.current_motor3,
(int) pnd->navdata_pwm.current_motor4 );
fprintf( navdata_file, "; %04d; %f; %04d; %04u",
(int) pnd->navdata_altitude.altitude_vision,
pnd->navdata_altitude.altitude_vz,
(int) pnd->navdata_altitude.altitude_ref,
(unsigned int) pnd->navdata_altitude.altitude_raw );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %04u; %f; %f; %04u",
pnd->navdata_altitude.obs_accZ,
pnd->navdata_altitude.obs_alt,
pnd->navdata_altitude.obs_x.v[0],
pnd->navdata_altitude.obs_x.v[1],
pnd->navdata_altitude.obs_x.v[2],
pnd->navdata_altitude.obs_state,
pnd->navdata_altitude.est_vb.v[0],
pnd->navdata_altitude.est_vb.v[1],
pnd->navdata_altitude.est_state );
vp_os_memset(&str[0], 0, sizeof(str));
sprintf( str, "%d.%06d", (int)((pnd->navdata_vision.time_capture & TSECMASK) >> TSECDEC), (int)(pnd->navdata_vision.time_capture & TUSECMASK) );
fprintf( navdata_file, "; % 8.6f; % 8.6f; % 8.6f; %u; %u; % f;% f;% f;% f; % f; % f; % f; %u; % f; % f; % f; % f; % f; % f; % d; %s",
pnd->navdata_vision_raw.vision_tx_raw,
pnd->navdata_vision_raw.vision_ty_raw,
pnd->navdata_vision_raw.vision_tz_raw,
(unsigned int) pnd->navdata_vision.vision_state,
(unsigned int) pnd->vision_defined,
pnd->navdata_vision.vision_phi_trim,
pnd->navdata_vision.vision_phi_ref_prop,
pnd->navdata_vision.vision_theta_trim,
pnd->navdata_vision.vision_theta_ref_prop,
pnd->navdata_vision.body_v.x,
pnd->navdata_vision.body_v.y,
pnd->navdata_vision.body_v.z,
(unsigned int) pnd->navdata_vision.new_raw_picture,
pnd->navdata_vision.theta_capture,
pnd->navdata_vision.phi_capture,
pnd->navdata_vision.psi_capture,
pnd->navdata_vision.delta_phi,
pnd->navdata_vision.delta_theta,
pnd->navdata_vision.delta_psi,
(int)pnd->navdata_vision.altitude_capture,
str );
fprintf( navdata_file, "; %04u",
(unsigned int) pnd->navdata_demo.vbat_flying_percentage );
fprintf( navdata_file, "; % f; % f; % f",
pnd->navdata_demo.theta,
pnd->navdata_demo.phi,
pnd->navdata_demo.psi );
fprintf( navdata_file, "; %04d",
(int) pnd->navdata_demo.altitude );
fprintf( navdata_file, "; %f; %f; %f ",
pnd->navdata_demo.vx,
pnd->navdata_demo.vy,
pnd->navdata_demo.vz );
fprintf( navdata_file, "; %04u", (unsigned int) pnd->navdata_demo.num_frames );
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.detection_camera_rot.m11,
pnd->navdata_demo.detection_camera_rot.m12,
pnd->navdata_demo.detection_camera_rot.m13,
pnd->navdata_demo.detection_camera_rot.m21,
pnd->navdata_demo.detection_camera_rot.m22,
pnd->navdata_demo.detection_camera_rot.m23,
pnd->navdata_demo.detection_camera_rot.m31,
pnd->navdata_demo.detection_camera_rot.m32,
pnd->navdata_demo.detection_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.detection_camera_trans.x,
pnd->navdata_demo.detection_camera_trans.y,
pnd->navdata_demo.detection_camera_trans.z);
fprintf( navdata_file, "; %04u; %04u",
(unsigned int) pnd->navdata_demo.detection_tag_index,
(unsigned int) pnd->navdata_demo.detection_camera_type);
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f; %f; %f; %f", pnd->navdata_demo.drone_camera_rot.m11,
pnd->navdata_demo.drone_camera_rot.m12,
pnd->navdata_demo.drone_camera_rot.m13,
pnd->navdata_demo.drone_camera_rot.m21,
pnd->navdata_demo.drone_camera_rot.m22,
pnd->navdata_demo.drone_camera_rot.m23,
pnd->navdata_demo.drone_camera_rot.m31,
pnd->navdata_demo.drone_camera_rot.m32,
pnd->navdata_demo.drone_camera_rot.m33);
fprintf( navdata_file, "; %f; %f; %f", pnd->navdata_demo.drone_camera_trans.x,
pnd->navdata_demo.drone_camera_trans.y,
pnd->navdata_demo.drone_camera_trans.z);
fprintf( navdata_file, "; %d; %f; %f; %f; %f",
(int)nd_iphone_flag,
nd_iphone_phi,
nd_iphone_theta,
nd_iphone_gaz,
nd_iphone_yaw);
fprintf( navdata_file, "; %d; %d; %d; %d; %d; %f; %d",
pnd->navdata_video_stream.quant,
pnd->navdata_video_stream.frame_size,
pnd->navdata_video_stream.frame_number,
pnd->navdata_video_stream.atcmd_ref_seq,
pnd->navdata_video_stream.atcmd_mean_ref_gap,
pnd->navdata_video_stream.atcmd_var_ref_gap,
pnd->navdata_video_stream.atcmd_ref_quality);
locked_ptr = (int32_t*) &pnd->navdata_trackers_send.locked[0];
point_ptr = (screen_point_t*) &pnd->navdata_trackers_send.point[0];
for(i = 0; i < DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT; i++)
{
fprintf( navdata_file, "; %d; %u; %u",
(int) *locked_ptr++,
(unsigned int) point_ptr->x,
(unsigned int) point_ptr->y );
point_ptr++;
}
fprintf( navdata_file, "; %u", (unsigned int) pnd->navdata_vision_detect.nb_detected );
for(i = 0 ; i < 4 ; i++)
{
fprintf( navdata_file, "; %u; %u; %u; %u; %u; %u; %f",
(unsigned int) pnd->navdata_vision_detect.type[i],
(unsigned int) pnd->navdata_vision_detect.xc[i],
(unsigned int) pnd->navdata_vision_detect.yc[i],
(unsigned int) pnd->navdata_vision_detect.width[i],
(unsigned int) pnd->navdata_vision_detect.height[i],
(unsigned int) pnd->navdata_vision_detect.dist[i],
pnd->navdata_vision_detect.orientation_angle[i]);
}
fprintf( navdata_file, "; %f; %f; %f; %f; %f; %f",
pnd->navdata_vision_perf.time_szo,
pnd->navdata_vision_perf.time_corners,
pnd->navdata_vision_perf.time_compute,
pnd->navdata_vision_perf.time_tracking,
pnd->navdata_vision_perf.time_trans,
pnd->navdata_vision_perf.time_update );
for(i = 0 ; i < NAVDATA_MAX_CUSTOM_TIME_SAVE ; i++)
{
fprintf( navdata_file, "; %f", pnd->navdata_vision_perf.time_custom[i]);
}
fprintf( navdata_file, "; %d", (int) pnd->navdata_watchdog.watchdog );
fprintf( navdata_file, "; %u", (unsigned int) num_picture_decoded );
sprintf( str, "%d.%06d", (int)time.tv_sec, (int)time.tv_usec);
fprintf( navdata_file, "; %s", str );
return C_OK;
}
