C_RESULT update_gamepad(void)
{
static int32_t x = 0, y = 0;
static bool_t refresh_values = FALSE;
ssize_t res;
static struct js_event js_e_buffer[64];
static int32_t start = 0;
input_state_t* input_state;
static int32_t theta_trim = 0;
static int32_t phi_trim = 0;
static int32_t yaw_trim = 0;
res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
if( !res || (res < 0 && errno == EAGAIN) )
return C_OK;
if( res < 0 )
return C_FAIL;
if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
return C_OK;
// Buffer decomposition in blocs (if the last is incomplete, it's ignored)
int32_t idx = 0;
refresh_values = FALSE;
input_state = ardrone_tool_get_input_state();
for (idx = 0; idx < res / sizeof(struct js_event); idx++)
{
if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
{
break;
}
else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
{
switch( js_e_buffer[idx].number )
{
case PAD_AG :
ardrone_tool_set_ui_pad_ag(js_e_buffer[idx].value);
break;
case PAD_AB :
ardrone_tool_set_ui_pad_ab(js_e_buffer[idx].value);
break;
case PAD_AD :
ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
break;
case PAD_AH :
ardrone_tool_set_ui_pad_ah(js_e_buffer[idx].value);
break;
case PAD_L1 :
if( js_e_buffer[idx].value )
{
yaw_trim = 0;
if( input_state->r2 )
{
yaw_trim = -1;
}
else
{
ardrone_tool_set_ui_pad_l1(1);
}
// ui_pad_yaw_trim( yaw_trim );
}
else
{
yaw_trim = 0;
ardrone_tool_set_ui_pad_l1(0);
// ui_pad_yaw_trim( yaw_trim );
}
break;
case PAD_R1 :
if( js_e_buffer[idx].value )
{
yaw_trim = 0;
if( input_state->r2 )
{
yaw_trim = 1;
}
else
{
ardrone_tool_set_ui_pad_r1(1);
}
// ui_pad_yaw_trim( yaw_trim );
}
else
{
yaw_trim = 0;
ardrone_tool_set_ui_pad_r1(0);
// ui_pad_yaw_trim( yaw_trim );
}
break;
case PAD_L2 :
ardrone_tool_set_ui_pad_l2(js_e_buffer[idx].value);
if( !js_e_buffer[idx].value )
{
ardrone_at_set_pmode( MiscVar[0] );
ardrone_at_set_ui_misc( MiscVar[0], MiscVar[1], MiscVar[2], MiscVar[3] );
}
break;
case PAD_R2 :
ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
if( !js_e_buffer[idx].value )
ardrone_at_set_flat_trim();
break;
case PAD_SELECT :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
break;
case PAD_START :
if( js_e_buffer[idx].value )
{
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
}
break;
default:
break;
}
}
else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
{
refresh_values = TRUE;
switch( js_e_buffer[idx].number )
{
case PAD_X:
x = ( js_e_buffer[idx].value + 1 ) >> 15;
break;
case PAD_Y:
y = ( js_e_buffer[idx].value + 1 ) >> 15;
break;
default:
break;
}
}
else
{// TODO: default: ERROR (non-supported)
}
}
if(refresh_values)// Axis values to refresh
void checkErrors(void)
{
input_state_t* input_state = ardrone_tool_get_input_state();
strcpy(ctrldata.error_msg, "");
if(!ctrldata.wifiReachabled)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_WIFI_NOT_REACHABLED]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else
{
if(ctrldata.needAnimation >= 0)
{
ardrone_at_set_anim(ctrldata.needAnimation, MAYDAY_TIMEOUT[ctrldata.needAnimation]);
ctrldata.needAnimation = -1;
}
if(ctrldata.needVideoSwitch >= 0)
{
ardrone_at_zap(ctrldata.needVideoSwitch);
ctrldata.needVideoSwitch = -1;
}
if(ctrldata.needChangeCameraDetection >= 0)
{
ardrone_at_cad(ctrldata.needChangeCameraDetection, ctrldata.tag_size);
ctrldata.needChangeCameraDetection = -1;
}
if(ctrldata.needLedAnimation >= 0)
{
ardrone_at_set_led_animation(ctrldata.needLedAnimation, ctrldata.ledAnimFreq, ctrldata.ledAnimDuration);
ctrldata.needLedAnimation = -1;
}
if(ctrldata.needSetEmergency)
{
ctrldata.isInEmergency = ctrlnavdata.emergencyLanding;
ardrone_tool_set_ui_pad_select(1);
ctrldata.needSetEmergency = FALSE;
}
if( ctrldata.needSetNavdataDemo)
{
if( control_ack_configure_navdata_demo(ctrldata.navdata_demo))
ctrldata.needSetNavdataDemo = FALSE;
}
else if( ctrldata.needSetPairing )
{
if(control_ack_configure_mac_address((const char*)&iphone_mac_address[0]))
ctrldata.needSetPairing = FALSE;
}
else if(ctrldata.needSetFrequency)
{
if(control_ack_configure_ultrasound_frequency(!ctrldata.is_client))
ctrldata.needSetFrequency = FALSE;
}
else if(ctrldata.needSetManualTrim)
{
if(control_ack_configure_manual_trim(ctrldata.manual_trim))
{
ctrldata.needSetManualTrim = FALSE;
ctrldata.manual_trim_enabled = ctrldata.manual_trim;
}
}
else if(ctrldata.needGetConfiguration)
{
//PRINT("Request configuration file\n");
if(control_get_configuration())
ctrldata.needGetConfiguration = FALSE;
}
if((ctrldata.framecounter % (kFPS / 2)) == 0)
{
if(ctrlnavdata.bootstrap)
{
setNavdataDemo(TRUE);
ctrldata.needSetFrequency = TRUE;
ctrldata.needGetConfiguration = TRUE;
ctrldata.needSetPairing = TRUE;
}
}
if(ardrone_navdata_client_get_num_retries() >= NAVDATA_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_CANT_CONNECT_TO_TOY]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else if(ctrlnavdata.emergencyLanding)
{
if(!ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
strcpy(ctrldata.emergency_msg, "reset");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PRESS_RESET]);
strcat(ctrldata.error_msg, "\n");
if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cutoutProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CUT_OUT]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.motorsProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_MOTORS]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cameraProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CAMERA]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_WATCHDOG]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcVersionProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_VERSION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.anglesProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_TOO_MUCH_ANGLE]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.userEmergency)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_USER]);
strcat(ctrldata.error_msg, "\n");
}
else
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_UNKNOWN]);
strcat(ctrldata.error_msg, "\n");
}
resetControlData();
ardrone_tool_start_reset();
if(ctrlnavdata.startPressed)
switchTakeOff();
}
else if(!ctrlnavdata.emergencyLanding)
{
if(ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
if(video_stage_get_num_retries() > VIDEO_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_NO_VIDEO_CONNECTION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.visionProblem && ctrlnavdata.flyingState)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_VISION]);
strcat(ctrldata.error_msg, "\n");
}
if(!ctrlnavdata.timerElapsed)
strcpy(ctrldata.emergency_msg, "emergency");
if(input_state->start)
{
if(ctrlnavdata.startPressed)
{
strcpy(ctrldata.takeoff_msg, "take_land");
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_START_NOT_RECEIVED]);
strcat(ctrldata.error_msg, "\n");
}
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
}
}
}
}
