/*---------------------------------------------------------------------------------------------------------------------
Navdata handling function, which is called every time navdata are received
---------------------------------------------------------------------------------------------------------------------*/
inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
static int cpt=0;
const navdata_demo_t* const nd = &navdata->navdata_demo;
/** ======= INSERT USER CODE HERE ========== **/
ARWin32Demo_AcquireConsole();
if ((cpt++)==90) { system("cls"); cpt=0; }
ARWin32Demo_SetConsoleCursor(0,0); // Print at the top of the console
printf("=================================\n");
printf("Navdata for flight demonstrations\n");
printf("=================================\n");
printf("Control state : %s \n",ctrl_state_str(nd->ctrl_state));
printf("Battery level : %i/100 \n",nd->vbat_flying_percentage);
printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f \n",nd->theta,nd->phi,nd->psi);
printf("Altitude : %i \n",nd->altitude);
printf("Speed : [vX] %4.3f [vY] %4.3f [vZ] %4.3f \n",nd->vx,nd->vy,nd->vz);
ARWin32Demo_ReleaseConsole();
/** ======= INSERT USER CODE HERE ========== **/
return C_OK;
}
/* Receving navdata during the event loop */
C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata )
{
static int cpt=0;
return C_OK;
const navdata_demo_t*nd = &navdata->navdata_demo;
const navdata_vision_detect_t*nv = &navdata->navdata_vision_detect;
printf("==========================\nNavdata for flight demonstrations \n==========================\n");
printf("Control state : %s\n",ctrl_state_str(nd->ctrl_state));
printf("Battery level : %i %%\n",nd->vbat_flying_percentage);
printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f\n",nd->theta,nd->phi,nd->psi);
printf("Altitude : %i\n",nd->altitude);
printf("Speed : [vX] %4.3f [vY] %4.3f [vZPsi] %4.3f\n",nd->vx,nd->vy,nd->vz);
printf("Position : [x] %4.3f [Y] %4.3f [Z] %4.3f\n",
nd->drone_camera_trans.x,
nd->drone_camera_trans.y,
nd->drone_camera_trans.z);
printf("Detec. trans. : [x] %4.3f [Y] %4.3f [Z] %4.3f\n",
nd->detection_camera_trans.x,
nd->detection_camera_trans.y,
nd->detection_camera_trans.z);
printf("Orientation :\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n",
nd->drone_camera_rot.m11,nd->drone_camera_rot.m12,nd->drone_camera_rot.m13,
nd->drone_camera_rot.m21,nd->drone_camera_rot.m22,nd->drone_camera_rot.m23,
nd->drone_camera_rot.m31,nd->drone_camera_rot.m32,nd->drone_camera_rot.m33 );
printf("Detec. rot. :\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n%4.3f | %4.3f | %4.3f\n",
nd->detection_camera_rot.m11,nd->detection_camera_rot.m12,nd->detection_camera_rot.m13,
nd->detection_camera_rot.m21,nd->detection_camera_rot.m22,nd->detection_camera_rot.m23,
nd->detection_camera_rot.m31,nd->detection_camera_rot.m32,nd->detection_camera_rot.m33 );
printf("=====================\nNavdata for detection \n=====================\n");
printf("Type : %i\n",nd->detection_camera_type);
printf("Tag index: %i\n",nd->detection_tag_index);
printf("Targets : %i\n",nv->nb_detected);
printf("Target 1 : [X] %4i [Y] %4i\n",nv->xc[0],nv->yc[0]);
printf("Target 2 : [X] %4i [Y] %4i\n",nv->xc[1],nv->yc[1]);
printf("\033[27A");
if ((cpt++)==100)
{
cpt=system("clear"); // Avoid compiler warning (unused return value)
cpt=0;
}
return C_OK;
}
/**
* Navdata handling callback
* This callback is registered to ARDroneTool in the BEGIN_NAVDATA_HANDLER_TABLE in navdata_client.c
* @param pnd Pointer to the ready-to-use navdata structure
* @return Always returns C_OK.
*/
C_RESULT navdata_ihm_process( const navdata_unpacked_t* const pnd )
{
//char text_buffer[1024];
const navdata_vision_detect_t* pndvision = &pnd->navdata_vision_detect;
gdk_threads_enter();
// States' display update for IHM
vp_os_memset( label_mykonos_state_value, '\0', sizeof(label_mykonos_state_value) );
sprintf(label_mykonos_state_value, "%08x", pnd->ardrone_state);
strcpy(label_ctrl_state_value, ctrl_state_str( pnd->navdata_demo.ctrl_state ));
set_control_state(pnd->navdata_demo.ctrl_state);
ihm_val_idx--;
if (ihm_val_idx < 0) ihm_val_idx = KIHM_N_PT2PLOT-1;
// Angular rates received and displayed in deg/s
ihm_CA[KIHM_CURVE_GYRO].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_X]);
ihm_CA[KIHM_CURVE_GYRO].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Y]);
ihm_CA[KIHM_CURVE_GYRO].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
// Accelerations are received in mg
// They are displayed in g: conversion gain is 1/1000
ihm_CA[KIHM_CURVE_ACC].tval[0][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_X]) / KIHM_MIL_F;
ihm_CA[KIHM_CURVE_ACC].tval[1][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Y]) / KIHM_MIL_F;
ihm_CA[KIHM_CURVE_ACC].tval[2][ihm_val_idx] = ((double) pnd->navdata_phys_measures.phys_accs[ACC_Z]) / KIHM_MIL_F;
// Filtered temperature is drawn in the Vbat window
ihm_CA[KIHM_CURVE_VBAT].tval[0][ihm_val_idx] = ((double) (pnd->navdata_phys_measures.accs_temp)); // Filtered temperature
ihm_CA[KIHM_CURVE_VBAT].tval[1][ihm_val_idx] = ((double) (pnd->navdata_demo.vbat_flying_percentage)) ; // Filtered vbat in %
ihm_CA[KIHM_CURVE_THETA].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.theta_a) / KIHM_MIL_F; // theta accelerometer value
ihm_CA[KIHM_CURVE_PHI ].tval[2][ihm_val_idx] = ((double) pnd->navdata_euler_angles.phi_a) / KIHM_MIL_F; // phi accelerometer value
ihm_CA[KIHM_CURVE_THETA].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.theta) / KIHM_MIL_F; // theta fused value
ihm_CA[KIHM_CURVE_PHI ].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.phi) / KIHM_MIL_F; // phi fused value
ihm_psi = ((double) pnd->navdata_demo.psi) / KIHM_MIL_F;
earef.theta = pnd->navdata_references.ref_theta;
ihm_CA[KIHM_CURVE_THETA].tval[1][ihm_val_idx] = ((double) earef.theta) / KIHM_MIL_F; // theta reference value
earef.phi = pnd->navdata_references.ref_phi;
ihm_CA[KIHM_CURVE_PHI ].tval[1][ihm_val_idx] = ((double) earef.phi) / KIHM_MIL_F; // phi reference value
earef.psi = pnd->navdata_references.ref_psi;
wref.p = pnd->navdata_references.ref_roll;
wref.q = pnd->navdata_references.ref_pitch;
wref.r = pnd->navdata_references.ref_yaw;
rc_ref_gaz = pnd->navdata_rc_references.rc_ref_gaz;
ihm_CA[KIHM_CURVE_ALTITUDE].tval[0][ihm_val_idx] = ((double) pnd->navdata_demo.altitude);
ihm_CA[KIHM_CURVE_ALTITUDE].tval[1][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_vision);
ihm_CA[KIHM_CURVE_ALTITUDE].tval[2][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_ref);
ihm_CA[KIHM_CURVE_ALTITUDE].tval[3][ihm_val_idx] = ((double) pnd->navdata_altitude.altitude_raw);
ihm_CA[KIHM_CURVE_VZ].tval[1][ihm_val_idx] = (double) (pnd->navdata_altitude.altitude_vz); // Fusion Vz
tab_trims[0] = (double)pnd->navdata_trims.euler_angles_trim_theta;
tab_trims[1] = (double)pnd->navdata_trims.euler_angles_trim_phi;
tab_trims[2] = (double)pnd->navdata_trims.angular_rates_trim_r;
ihm_CA[KIHM_CURVE_PWM].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.motor1); // PWM motor 1
ihm_CA[KIHM_CURVE_PWM].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.motor2); // PWM motor 2
ihm_CA[KIHM_CURVE_PWM].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.motor3); // PWM motor 3
ihm_CA[KIHM_CURVE_PWM].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.motor4); // PWM motor 4
ihm_CA[KIHM_CURVE_CURRENT].tval[0][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor1); // current motor 1
ihm_CA[KIHM_CURVE_CURRENT].tval[1][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor2); // current motor 2
ihm_CA[KIHM_CURVE_CURRENT].tval[2][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor3); // current motor 3
ihm_CA[KIHM_CURVE_CURRENT].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.current_motor4); // current motor 4
ihm_CA[KIHM_CURVE_VZ].tval[3][ihm_val_idx] = (double) (pnd->navdata_pwm.vz_ref); // VZ reference
ihm_CA[KIHM_CURVE_VX].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tx_raw); // VISION trans. en x
ihm_CA[KIHM_CURVE_VY].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_ty_raw); // VISION trans. en y
ihm_CA[KIHM_CURVE_VZ].tval[0][ihm_val_idx] = (double) (pnd->navdata_vision_raw.vision_tz_raw); // VISION trans. en z
if( pnd->vision_defined )
{
if( fabsf(pnd->navdata_vision_raw.vision_tx_raw) <= 10.0f && fabsf(pnd->navdata_vision_raw.vision_ty_raw) <= 10.0f )
{
ihm_dir = HOVER_VAL;
}
else
{
ihm_dir = (double) atan2f(pnd->navdata_vision_raw.vision_ty_raw, pnd->navdata_vision_raw.vision_tx_raw);
}
}
else
{
ihm_dir = NOT_DEF_VAL;
}
//Vision States' display update for IHM
vp_os_memset( label_vision_state_value, '\0', sizeof(label_vision_state_value) );
sprintf(label_vision_state_value, "%08x", pnd->navdata_vision.vision_state);
ihm_CA[KIHM_CURVE_VX].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vx); // Fusion Vx
ihm_CA[KIHM_CURVE_VY].tval[1][ihm_val_idx] = (double) (pnd->navdata_demo.vy); // Fusion Vy
#ifdef PC_USE_VISION
draw_trackers_cfg.num_points = NUM_MAX_SCREEN_POINTS;
vp_os_memset(&draw_trackers_cfg.locked[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t));
vp_os_memcpy(&draw_trackers_cfg.locked[0], pnd->navdata_trackers_send.locked, NUM_MAX_SCREEN_POINTS * sizeof(uint32_t));
vp_os_memset(&draw_trackers_cfg.points[0], 0, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t));
vp_os_memcpy(&draw_trackers_cfg.points[0], pnd->navdata_trackers_send.point, NUM_MAX_SCREEN_POINTS * sizeof(screen_point_t));
int i;
draw_trackers_cfg.detected = pnd->navdata_vision_detect.nb_detected;
for(i = 0 ; i < pnd->navdata_vision_detect.nb_detected ; i++)
{
draw_trackers_cfg.type[i] = pnd->navdata_vision_detect.type[i];
draw_trackers_cfg.patch_center[i].x = pnd->navdata_vision_detect.xc[i];
draw_trackers_cfg.patch_center[i].y = pnd->navdata_vision_detect.yc[i];
draw_trackers_cfg.width[i] = pnd->navdata_vision_detect.width[i];
draw_trackers_cfg.height[i] = pnd->navdata_vision_detect.height[i];
}
set_draw_trackers_config(&draw_trackers_cfg);
#endif
/*Stephane*/
extern char label_detection_state_value[32];
switch(pnd->navdata_demo.detection_camera_type)
{
case CAD_TYPE_NONE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Detection disabled"); break;
case CAD_TYPE_VISION: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching 2D tags"); break;
case CAD_TYPE_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break;
case CAD_TYPE_ORIENTED_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching roundels"); break;
case CAD_TYPE_H_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break;
case CAD_TYPE_H_ORIENTED_COCARDE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching H roundels"); break;
case CAD_TYPE_STRIPE: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (15Hz)"); break;
case CAD_TYPE_STRIPE_V: snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Searching ground stripe (60Hz)"); break;
default : snprintf(label_detection_state_value,sizeof(label_detection_state_value),"Navdata error");
}
char nbDetectedTags_label_buffer[1024];
if (pndvision->nb_detected==1 && ( pnd->navdata_demo.detection_camera_type==CAD_TYPE_ORIENTED_COCARDE || pnd->navdata_demo.detection_camera_type==CAD_TYPE_H_ORIENTED_COCARDE ) )
{ snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg)",pndvision->nb_detected,pndvision->orientation_angle[0]); }
else if (pndvision->nb_detected==1 && (pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE || pnd->navdata_demo.detection_camera_type==CAD_TYPE_STRIPE_V))
{ snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i (%4.2f deg - w: %i)",
pndvision->nb_detected,
pndvision->orientation_angle[0],
pndvision->width[0]);
}
else
{ snprintf(nbDetectedTags_label_buffer,sizeof(nbDetectedTags_label_buffer),"Found : %i",pndvision->nb_detected); }
if(nbDetectedTags_label!=NULL)
gtk_label_set_text((GtkLabel*)nbDetectedTags_label,(const gchar*)nbDetectedTags_label_buffer);
gdk_threads_leave();
return C_OK;
}
