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