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"); } } } }