C_RESULT update_gamepad(void) {
static int32_t start;
static float phi, theta, gaz, yaw;
static int cmd_mode = 0;
static int32_t animation = 0;
static char *animations[] = {
"PHI_M30_DEG","PHI_30_DEG","THETA_M30_DEG","THETA_30_DEG",
"THETA_20DEG_YAW_200DEG","THETA_20DEG_YAW_M200DEG",
"Turnaround","Turnaround & Down","Turn Shake","Turn Dance",
"Roll Dance","Tilt Dance","VZ_DANCE","Wave",
"PHI_THETA_MIXED","DOUBLE_PHI_THETA_MIXED",
"Front Flip","Back Flip","Left Flip","Right Flip"
};
struct js_event js_e_buffer[64];
ssize_t 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)
bool_t refresh_values = FALSE;
int32_t idx = 0;
for (idx = 0; idx < res / sizeof(struct js_event); idx++) {
unsigned char type = js_e_buffer[idx].type;
unsigned char number = js_e_buffer[idx].number;
short value = js_e_buffer[idx].value;
if ( type & JS_EVENT_INIT ) {
break;
}
else if ( type & JS_EVENT_BUTTON ) {
switch( number ) {
case BUTTON_START :
if( value == 1 ){
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
g_autopilot = FALSE;
}
break;
case BUTTON_SELECT :
if( value == 1 ){
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
return C_OK;
}
case BUTTON_ZAP:
if( value == 1 ){
// Using the zap button to pass in a notification to the agent
// instead of changing the camera (maybe i should move zap to a hat)
// a button that sets a auto-hover mode might be good too, but tomorrow
// note: in the event you could do it fast enough (you can't) you would overwrite
// a previous press before it gets sent
g_pass_button = BUTTON_ZAP;
}
break;
case BUTTON_HOVER:
// reset values and set hover mode regardless of current command mode
// this operates outside of the normal handling so refreshing values isn't necessary
// autopilot will still be cancelled though
if( value == 1 ){
g_autopilot = FALSE;
phi = 0; theta = 0; gaz = 0; yaw = 0;
ardrone_tool_set_progressive_cmd(0,0,0,0,0,0,0);
}
break;
case BUTTON_UP:
if( value == 1 ){
phi = 0; theta = -0.1; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_DOWN:
if( value == 1 ){
phi = 0; theta = 0.1; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_LEFT:
if( value == 1 ){
phi = -0.1; theta = 0; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_RIGHT:
if( value == 1 ){
phi = 0.1; theta = 0; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_L1:
if( value == 1 ){
animation++;
if( animation >= ARDRONE_NB_ANIM_MAYDAY ){
animation = 0;
}
PRINT("Switched flight animation to %s\n", animations[animation]);
}
break;
case BUTTON_R1:
if( value == 1 ){
PRINT("Performing flight animation %s\n", animations[animation]);
ardrone_at_set_anim( animation, MAYDAY_TIMEOUT[animation] );
}
break;
case BUTTON_L2:
if( value == 1 ){
phi = 0; theta = 0; gaz = 0; yaw = -0.2;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_R2:
if( value == 1 ){
phi = 0; theta = 0; gaz = 0; yaw = 0.2;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_FTRIM:
if( value == 1 ){
ardrone_at_set_flat_trim();
PRINT("Flat trim request sent\n");
}
break;
case BUTTON_AUTO:
if( value == 1 ){
if (g_autopilot) {
refresh_values = TRUE;
}
else {
g_autopilot = TRUE;
}
}
break;
case BUTTON_LHAT:
if( value == 1 ){
if( cmd_mode == 0 ){
PRINT("ACTIVE mode set. auto-hover disabled\n");
cmd_mode = 1;
}else{
PRINT("ASSISTED mode set. auto-hover enabled\n");
cmd_mode = 0;
}
}
}
}
else if (type & JS_EVENT_AXIS ) {
if (number == AXIS_PHI || number == AXIS_THETA || number == AXIS_GAZ || number == AXIS_YAW) {
refresh_values = TRUE;
float angle = value / (float)SHRT_MAX;
switch (number) {
case AXIS_PHI:
phi = angle;
break;
case AXIS_THETA:
theta = angle;
break;
case AXIS_GAZ:
gaz = ( angle * -1 );
break;
case AXIS_YAW:
yaw = angle;
break;
}
}
}
} // loop over events
if (refresh_values) {
g_autopilot = FALSE;
if( phi == 0 && theta == 0 && gaz == 0 && yaw == 0 ){
ardrone_tool_set_progressive_cmd(cmd_mode,0,0,0,0,0,0);
}else{
ardrone_tool_set_progressive_cmd(1,phi,theta,gaz,yaw,0,0);
}
//set(phi, theta, gaz, yaw);
}
return C_OK;
}
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");
}
}
}
}
