DEFINE_THREAD_ROUTINE( comm_control, data ) { int seq_no = 0,prev_start = 0; control_data_t l_control; PRINT( "Initilizing Thread 1\n" ); while( end_all_threads == 0 ) { vp_os_delay(10); //PRINT("E:%d,S:%d\n\n",end_all_threads,control_data.start ); vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex l_control = control_data; vp_os_mutex_unlock( &control_data_lock ); if( prev_start == 0 && l_control.start == 1 ) //To Start Drone { ardrone_tool_set_ui_pad_start( 1 ); prev_start = 1; } else if( prev_start == 1 && l_control.start == 0 ) //To Stop the Drone { ardrone_tool_set_ui_pad_start( 0 ); prev_start = 0; } //PRINT("YAW : %f\n\n",l_control.yaw); ardrone_at_set_progress_cmd( ++seq_no,l_control.roll,l_control.pitch,l_control.gaz,l_control.yaw); //command to make drone move. //ardrone_at_set_progress_cmd( ++seq_no,0,0,0,-1); } PRINT( "Communication Thread Ending\n" ); return C_OK; }
void ardrone_academy_navdata_userbox_cb(bool_t result) { if(result) { LOCK_ND_MUTEX (); if(navdata_state.userbox_state == USERBOX_STATE_STARTING) { PRINT("Userbox started\n"); navdata_state.userbox_state = USERBOX_STATE_STARTED; switch (navdata_state.takeoff_state) { case TAKEOFF_STATE_WAIT_USERBOX: navdata_state.takeoff_state = TAKEOFF_STATE_IDLE; ardrone_tool_set_ui_pad_start(1); break; case TAKEOFF_STATE_CANCELLING: navdata_state.takeoff_state = TAKEOFF_STATE_IDLE; ardrone_tool_set_ui_pad_start(0); break; default: break; } if(navdata_state.record_state == RECORD_STATE_WAIT_USERBOX) { navdata_state.record_state = RECORD_STATE_IDLE; if (FALSE == navdata_state.usbRecordInProgress) { ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time); } } } else if(navdata_state.userbox_state == USERBOX_STATE_STOPPING) { PRINT("Userbox stopped\n"); academy_download_resume (); if(navdata_state.takeoff_state == TAKEOFF_STATE_WAIT_USERBOX || TAKEOFF_STATE_CANCELLING == navdata_state.takeoff_state) { navdata_state.takeoff_state = TAKEOFF_STATE_IDLE; ardrone_tool_set_ui_pad_start(0); } if(navdata_state.record_state == RECORD_STATE_WAIT_USERBOX) { navdata_state.record_state = RECORD_STATE_IDLE; ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time); navdata_state.usbRecordInProgress = FALSE; } navdata_state.userbox_state = USERBOX_STATE_STOPPED; } UNLOCK_ND_MUTEX (); } }
C_RESULT ardrone_control() { const char * linefiller=" "; static int nCountFrequency=0; static bool function_first_call = true; nCountFrequency++; if(nCountFrequency%50==0) { printf("过去了 %d 秒!!!!\n",nCountFrequency/50); if(nCountFrequency%5000==0) nCountFrequency=0; } if (function_first_call) { printf("Sending flat trim - make sure the drone is horizontal at client startup.\n"); ardrone_at_set_flat_trim(); //向无人机发送确定无人机是水平躺着,每次无人机启动都要发送,不可在无人机飞行时调用此函数 function_first_call=false; // vp_os_memcpy(previous_keyboardState,keyboardState,sizeof(keyboardState)); return C_OK; } ARWin32Demo_AcquireConsole(); ARWin32Demo_SetConsoleCursor(0,12); if(emergency!=emergency_p) { //以下代码 待服务器端完成后,需再次修改 if(emergency=0) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else//若之前按下,现在没按,说明是emergency状态转常规飞行 { ardrone_tool_set_ui_pad_select(0); } } if((takeoff!=takeoff_p)&&takeoff==1) { start^=1; ardrone_tool_set_ui_pad_start(start); printf("飞机起飞 %i.%s\n",start,linefiller); } if((trim!=trim_p)&&trim==1) { ardrone_at_set_flat_trim(); printf("水平矫正.%s\n",linefiller); } ardrone_at_set_progress_cmd(hover,roll, pitch, gaz, yaw); printf("[Pitch %f] [Roll %f] [Yaw %f] [Gaz %f]%\n",pitch,roll,yaw,gaz); ARWin32Demo_ReleaseConsole(); return C_OK; }
/* This function is called in the update function, at a refresh rate of 20 ms */ C_RESULT ardrone_tool_update_custom() { // read from memdb and send to ardrone char buffer[1024]; int hover = 0; // boolean indicating whether it's hovering or not float phi; // left/right angle float theta; // front/back angle float gaz; // verticle speed float yaw; // angular speed while (db_tryget(db, "drone_command", buffer, sizeof(buffer)) != -1) { sscanf(buffer, "%d,%f,%f,%f,%f", &hover, &phi, &theta, &gaz, &yaw); if (hover == 1) // hover { ardrone_at_set_progress_cmd(0, 0, 0, 0, 0); } else if (hover == -1) // land { ardrone_tool_set_ui_pad_start(0); } else // do stuff... { ardrone_at_set_progress_cmd(1, phi, theta, gaz, yaw); } } return C_OK; }
C_RESULT update_teleop(void) { // This function *toggles* the emergency state, so we only want to toggle the emergency // state when we are in the emergency state (because we want to get out of it). ardrone_tool_set_ui_pad_select(needs_reset); needs_reset = false; // This function sets whether or not the robot should be flying. If it is flying and you // send 0, the robot will slow down the motors and slowly descend to the floor. ardrone_tool_set_ui_pad_start(is_flying); float left_right = cmd_vel.linear.y; float front_back = cmd_vel.linear.x; float up_down = cmd_vel.linear.z; float turn = cmd_vel.angular.z; ///printf("LR:%f FB:%f UD:%f TURN:%f\n", left_right, front_back, up_down, turn); if(left_right == 0 && front_back == 0 && up_down == 0 && turn == 0) { printf("Drone im Hover Mode\n"); ardrone_at_set_progress_cmd(0, left_right, front_back, up_down, turn); }else{ ardrone_at_set_progress_cmd(1, left_right, front_back, up_down, turn); } return C_OK; }
int _stdcall SendLand() { ardrone_tool_set_ui_pad_start(0); need_update = TRUE; printf("Sent land\n"); return 0; }
int _stdcall SendTakeoff() { ardrone_tool_set_ui_pad_start(1); need_update = TRUE; printf("Sent takeoff\n"); return 0; }
int _stdcall SendEmergency() { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); need_update = TRUE; printf("Sent Emergency\n"); return 0; }
void switchTakeOff(void) { if(!ctrlnavdata.startPressed) { #ifdef DEBUG_CONTROL PRINT("START BUTTON\n"); #endif ardrone_tool_set_ui_pad_start( 1 ); } else { #ifdef DEBUG_CONTROL PRINT("STOP BUTTON\n"); #endif ardrone_tool_set_ui_pad_start( 0 ); } }
/* The delegate object calls this method during initialization of an ARDrone application */ C_RESULT ardrone_tool_init_custom(int argc, char **argv) { C_RESULT res; // Set drone to "takeoff" mode res = ardrone_tool_set_ui_pad_start(1); return res; }
void drone_land() { pthread_mutex_lock(&drone_sharedDataMutex); if (!drone_targetEmergency && !drone_emergency) { if (drone_atCommands++ < MAX_AT_COMMANDS_PER_NAVDATA) { ardrone_tool_set_ui_pad_start(0); } } pthread_mutex_unlock(&drone_sharedDataMutex); }
/* The delegate object calls this method when the event loop exit */ C_RESULT ardrone_tool_shutdown_custom() //Initiation Function { /* Relinquish all threads of your application */ JOIN_THREAD( video_stage ); JOIN_THREAD( thread1 ); ardrone_tool_set_ui_pad_start(0); /* Unregistering for the current device */ return C_OK; }
static void buttons_callback(GtkWidget *widget, gpointer data ) { static int value = 1; ardrone_tool_set_ui_pad_start(value); if (value) g_print("Taking off"); else g_print("Landing"); value = (value + 1) % 2; toggleButtonsState(); // We want only one button to be clickable }
C_RESULT automated_update(void) { if(!can_see_ball) { ardrone_tool_set_ui_pad_start(takeOffFlag = 0); return C_OK; } if(vertical > 0 && !takeOffFlag) ardrone_tool_set_ui_pad_start(takeOffFlag = !takeOffFlag); printf("horizontalStrafe: %f\nvertical: %f\n\n", horizontalStrafe, vertical); // if(vertical > 0) // { // if(1 || canFly) // { ardrone_tool_set_progressive_cmd( 1, /*roll*/horizontalStrafe, /*pitch*/ahead, /*gaz*/vertical, /*yaw*/horizontalTurn, /*psi*/0.0, /*psi_accuracy*/0.0); // } // printf("roll: %f, pitch: %f, gaz: %f, yaw: %f\n", // /*roll*/(float)(leftStickX)/32768.0f, // /*pitch*/(float)(leftStickY)/32768.0f, // /*gaz*/-(float)(rightStickY)/32768.0f, // /*yaw*/(float)(rightStickX)/32768.0f);*/ // } // else // { // ardrone_tool_set_ui_pad_start(takeOffFlag = 0); // } joypad_update(); return C_OK; }
C_RESULT update_teleop(void) { // This function *toggles* the emergency state, so we only want to toggle the emergency // state when we are in the emergency state (because we want to get out of it). //fprintf(stderr, "%d\n", int(needs_reset)); if (needs_reset) { // fprintf(stderr, "needs_reset true\n"); ardrone_tool_set_ui_pad_select(needs_reset); needs_reset = false; // fprintf(stderr, "needs_reset false\n"); } // ******dep****** This function sets whether or not the robot should be flying. If it is flying and you // ******dep****** send 0, the robot will slow down the motors and slowly descend to the floor. //rujian July 30 if (needs_takeoff) { ardrone_tool_set_ui_pad_start(true); needs_takeoff = false; } if (needs_land) { ardrone_tool_set_ui_pad_start(false); needs_land = false; } float left_right = cmd_vel.linear.y; float front_back = cmd_vel.linear.x; float up_down = cmd_vel.linear.z; float turn = cmd_vel.angular.z; ardrone_at_set_progress_cmd(do_not_hover, left_right, front_back, up_down, turn); //rujian July27 return C_OK; }
C_RESULT ardrone_tool_update_custom (void) { static int a=0; printf("%d\n",a); if ( ++a < 200 ) { ardrone_tool_set_ui_pad_start(1); } else { ardrone_tool_input_start_reset(); } return C_OK; }
DEFINE_THREAD_ROUTINE( thread1, data ) { int seq_no = 0,prev_start = 0; PRINT( "Initilizing Thread 1\n" ); while(1) { vp_os_delay(100); vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex if( prev_start == 0 && control_data.start == 1 ) { ardrone_tool_set_ui_pad_start( 1 ); prev_start = 1; } else if( prev_start == 1 && control_data.start == 0 ) { ardrone_tool_set_ui_pad_start( 0 ); prev_start = 0; } ardrone_at_set_progress_cmd( ++seq_no,control_data.roll, control_data.pitch, control_data.gaz, control_data.yaw); //command to make drone move. vp_os_mutex_unlock( &control_data_lock ); } }
void set_target_location(int32_t x, int32_t y, uint32_t maxX, uint32_t maxY, uint8_t sees_ball) { if(!(can_see_ball = sees_ball)) ardrone_tool_set_ui_pad_start(takeOffFlag = 0); float newHorizontalStrafe = (float)x / maxX; if(fabs(newHorizontalStrafe) > 0.15) horizontalStrafe = newHorizontalStrafe; else horizontalStrafe = 0; float newVertical = (float)y / maxY; if(fabs(newVertical) > 0.15) vertical = newVertical; else vertical = 0; }
void start_stop() { ARDRONE_STARTED = ARDRONE_STARTED ? 0 : 1; ardrone_tool_set_ui_pad_start(ARDRONE_STARTED); }
C_RESULT take_off(void *arg) { ardrone_tool_set_ui_pad_select(0); ardrone_tool_set_ui_pad_start(1); return C_OK; }
C_RESULT land(void *arg) { ardrone_tool_set_ui_pad_start(0); return C_OK; }
C_RESULT update_dx_keyboard(void) { static float roll = 0, pitch = 0, gaz=0, yaw=0; static int hovering=0; const char * linefiller=" "; /* Direct Input keyboard state */ char keyboardState[256]; static char previous_keyboardState[256]; static bool function_first_call = true; // Get the keyboard state hr = fDIKeyboard->GetDeviceState( sizeof(keyboardState), (LPVOID)&keyboardState ); if (FAILED(hr)) fDIKeyboard->Acquire(); if (function_first_call) { printf("Sending flat trim - make sure the drone is horizontal at client startup.\n"); ardrone_at_set_flat_trim(); function_first_call=false; vp_os_memcpy(previous_keyboardState,keyboardState,sizeof(keyboardState)); return C_OK; } ARWin32Demo_AcquireConsole(); ARWin32Demo_SetConsoleCursor(0,12); // Process the keyboard data #define EVENTKB(I) ( keyboardState[I] != previous_keyboardState[I] ) #define TESTKB(I) ( keyboardState[I] & 0x80 ) #define SETKB(I,J) if( keyboardState[I] & 0x80 ) J(1); else J(0); pitch = (TESTKB(DIK_NUMPAD2))? (+1.0f) : (TESTKB(DIK_NUMPAD8))? (-1.0f) : (TESTKB(DIK_K))? (+1.0f) : (TESTKB(DIK_I))? (-1.0f) :(0.0f) ; roll = (TESTKB(DIK_NUMPAD4))? (-1.0f) : (TESTKB(DIK_NUMPAD6))? (+1.0f) : (TESTKB(DIK_J))? (-1.0f) : (TESTKB(DIK_L))? (+1.0f) : (0.0f) ; yaw = (TESTKB(DIK_NUMPAD7))? (-1.0f) : (TESTKB(DIK_NUMPAD9))? (+1.0f) : (TESTKB(DIK_NUMPAD3))? (-1.0f) : (TESTKB(DIK_NUMPAD1))? (+1.0f) : (TESTKB(DIK_U))? (-1.0f) : (TESTKB(DIK_O))? (+1.0f) : (0.0f) ; gaz = (TESTKB(DIK_A))? (-1.0f) : (TESTKB(DIK_Q))? (1.0f) : (0.0f) ; hovering = (TESTKB(DIK_G))? (1):(0) ; keyboard_in_use = !((pitch+roll+gaz+yaw)==0); if (EVENTKB(DIK_ESCAPE)) { if (TESTKB(DIK_ESCAPE)) { exit_ihm_program=0; } } if (EVENTKB(DIK_TAB)) { if (TESTKB(DIK_TAB)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else { ardrone_tool_set_ui_pad_select(0); } } if (EVENTKB(DIK_SPACE) && TESTKB(DIK_SPACE)) { start^=1; ardrone_tool_set_ui_pad_start(start); printf("Sending start %i.%s\n",start,linefiller); } if (EVENTKB(DIK_F) && TESTKB(DIK_F)) { ardrone_at_set_flat_trim(); printf("Sending flat trim.%s\n",linefiller); } ardrone_tool_set_pcmd((hovering)?0:1,roll, pitch, gaz, yaw,0.0,0.0); printf("[Pitch %f] [Roll %f] [Yaw %f] [Gaz %f]%\n",pitch,roll,yaw,gaz); vp_os_memcpy(previous_keyboardState,keyboardState,sizeof(keyboardState)); ARWin32Demo_ReleaseConsole(); return C_OK; }
C_RESULT update_dx_gamepad(void) { /* static int32_t x = 0, y = 0; static bool_t refresh_values = FALSE; size_t res; static js_event js_e_buffer[64]; */ HRESULT hr; TCHAR strText[512] = {0}; // Device state text /* Direct Input gamepad state */ DIJOYSTATE2 js; static DIJOYSTATE2 previous_js; static bool function_first_call = true; static float roll = 0, pitch = 0, gaz=0, yaw=0; const char * linefiller=" "; // Poll the device to read the current state hr = g_pJoystick->Poll(); if( FAILED( hr ) ) { // DInput is telling us that the input stream has been // interrupted. We aren't tracking any state between polls, so // we don't have any special reset that needs to be done. We // just re-acquire and try again. hr = g_pJoystick->Acquire(); while( hr == DIERR_INPUTLOST ) hr = g_pJoystick->Acquire(); // hr may be DIERR_OTHERAPPHASPRIO or other errors. This // may occur when the app is minimized or in the process of // switching, so just try again later return C_OK; } // Get the input's device state if( FAILED( hr = g_pJoystick->GetDeviceState( sizeof( DIJOYSTATE2 ), &js ) ) ) return hr; // The device should have been acquired during the Poll() if (function_first_call) { printf("Sending flat trim - make sure the drone is horizontal at client startup.\n"); ardrone_at_set_flat_trim(); function_first_call=false; previous_js=js; return C_OK; } ARWin32Demo_AcquireConsole(); ARWin32Demo_SetConsoleCursor(0,14); // Process the gamepad data { switch(JoystickType) { /* Tests that if an event occured */ #define EVENTJS(I) ( js.rgbButtons[I-1] != previous_js.rgbButtons[I-1] ) #define TESTJS(I) ( js.rgbButtons[I-1] & 0x80 ) #define SETJS(I,J) if( js.rgbButtons[I-1] & 0x80 ) J(1); else J(0); /********************/ case GAMEPAD_PLAYSTATION3: pitch = js.lY/(1000.0f) /* belongs to [-1000:1000] */; roll = js.lX/(1000.0f) /* belongs to [-1000:1000] */; yaw = js.lZ/(1000.0f) /* belongs to [-1000:1000] */; gaz = js.lRz/(1000.0f) /* belongs to [-1000:1000] */; // Buttons hard coded for the Playstation3 SixAxis pad connected in USB mode on a Windows XP computer if (EVENTJS(9)) { if (TESTJS(9)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else { ardrone_tool_set_ui_pad_select(0); } } if (EVENTJS(10) && TESTJS(10)) { start^=1; ardrone_tool_set_ui_pad_start(start); printf("Sending start %i.%s\n",start,linefiller); } break; /********************/ case JOYSTICK_CYBORG_X: pitch = js.lY/(1000.0f) /* belongs to [-1000:1000] */; roll = js.lX/(1000.0f) /* belongs to [-1000:1000] */; yaw = js.lRz/(1000.0f) /* belongs to [-1000:1000] */; /* From MSDN : Direction controllers, such as point-of-view hats. The position is indicated in hundredths of a degree clockwise from north (away from the user). The center position is normally reported as - 1; but see Remarks. For indicators that have only five positions, the value for a controller is - 1, 0, 9,000, 18,000, or 27,000. */ switch (js.rgdwPOV[0]) { case 0: gaz = 1.0f; break; case 4500: gaz = 0.5f; break; case 13500: gaz = -0.5f; break; case 18000: gaz = -1.0f; break; case 22500: gaz = -0.5f; break; case 31500: gaz = +0.5f; break; default:gaz = 0.0f; } // Buttons hard coded for the Playstation3 SixAxis pad connected in USB mode on a Windows XP computer if (EVENTJS(2)||EVENTJS(3)||EVENTJS(4)||EVENTJS(5)) { if (TESTJS(2)||TESTJS(3)||TESTJS(4)||TESTJS(5)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else { ardrone_tool_set_ui_pad_select(0); } } if (EVENTJS(1) && TESTJS(1)) { start^=1; ardrone_tool_set_ui_pad_start(start); printf("Sending start %i.%s\n",start,linefiller); } break; /********************/ case GAMEPAD_LOGITECH_PRECISION: pitch = (float)js.lY/(1000.0f); roll = (float)js.lX/(1000.0f); yaw = (TESTJS(3))? (1.0f) : (TESTJS(1))? (-1.0f) : (0.0f) ; gaz = (TESTJS(4))? (1.0f) : (TESTJS(2))? (-1.0f) : (0.0f) ; if (EVENTJS(9)) { if (TESTJS(9)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else { ardrone_tool_set_ui_pad_select(0); } } if (EVENTJS(10) && TESTJS(10)) { start^=1; ardrone_tool_set_ui_pad_start(start); printf("Sending start %i.%s\n",start,linefiller); } if (EVENTJS(5) && TESTJS(5)) { ardrone_at_set_flat_trim(); printf("Sending flat trim.%s\n",linefiller); } break; /********************/ case GAMEPAD_RADIO_GP: pitch = (float)js.lZ/(1000.0f); roll = (float)js.lRz/(1000.0f); yaw = (float)js.lX/(1000.0f); gaz = -(float)js.lY/(1000.0f); if (EVENTJS(2)) { if (TESTJS(2)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending flat trim.%s\n",linefiller); ardrone_at_set_flat_trim(); } } if (EVENTJS(3)) { if (TESTJS(3)) { ardrone_tool_set_ui_pad_start(0); ardrone_tool_set_ui_pad_select(1); printf("Sending emergency.%s\n",linefiller); } else { ardrone_tool_set_ui_pad_select(0); } } if (EVENTJS(1)) { start= TESTJS(1)?1:0; // button 1 is an on/off switch that does not need to be maintained ardrone_tool_set_ui_pad_start(start); printf("Sending start %i.%s\n",start,linefiller); } break; default: pitch=roll=gaz=yaw=0.0f; /********************/ } // switch } // keyboardinuse if(!keyboard_in_use) { ardrone_tool_set_pcmd(1,roll, pitch, gaz, yaw, 0.0, 0.0); printf("[Pitch %f] [Roll %f] [Yaw %f] [Gaz %f]%\n",pitch,roll,yaw,gaz); } /* Keeps track of the last joystick state in a static variable */ previous_js=js; ARWin32Demo_ReleaseConsole(); return C_OK; }
C_RESULT ardrone_academy_navdata_process( const navdata_unpacked_t* const pnd ) { static bool_t prevCameraIsReady = FALSE; static bool_t prevDroneUsbRecording = FALSE; if (C_OK == TRYLOCK_ND_MUTEX ()) { input_state_t* input_state = ardrone_tool_input_get_state(); bool_t recordIsReady = ! ardrone_academy_navdata_get_record_ready(); bool_t recordIsInProgress = ardrone_academy_navdata_get_record_state(); // Save ARDrone State navdata_state.lastState = pnd->ardrone_state; // Save remaining USB time navdata_state.usbFreeTime = pnd->navdata_hdvideo_stream.usbkey_remaining_time; bool_t currentDroneUsbRecording = (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING == (NAVDATA_HDVIDEO_USBKEY_IS_RECORDING & pnd->navdata_hdvideo_stream.hdvideo_state)); // Check for record stop from drone if (navdata_state.usbRecordInProgress) { if (TRUE == prevDroneUsbRecording && FALSE == currentDroneUsbRecording) { navdata_state.droneStoppedUsbRecording = TRUE; navdata_state.record_state = RECORD_STATE_NEEDED; } } prevDroneUsbRecording = currentDroneUsbRecording; ardrone_academy_check_take_off_timeout (); // Take off and Emergency management if(navdata_state.takeoff_state == TAKEOFF_STATE_NEEDED) { if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) { navdata_state.needSetEmergency = TRUE; } else { if(recordIsInProgress) { if(!(pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START)) ardrone_tool_set_ui_pad_start(1); else ardrone_tool_set_ui_pad_start(0); navdata_state.takeoff_state = TAKEOFF_STATE_IDLE; } else { navdata_state.takeoff_state = TAKEOFF_STATE_WAIT_USERBOX; ardrone_academy_navdata_userbox_switch(); } } } if(navdata_state.needSetEmergency) { ardrone_tool_set_ui_pad_select(1); navdata_state.needSetEmergency = FALSE; } if(pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) { if(!navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT))) { ardrone_tool_set_ui_pad_select(0); } navdata_state.isEmergency = TRUE; navdata_state.isTakeOff = FALSE; if(!navdata_state.internalRecordInProgress && !navdata_state.usbRecordInProgress && (navdata_state.userbox_state == USERBOX_STATE_STARTED)) { PRINT("Emergency !! Stopping userbox...\n"); ardrone_academy_navdata_userbox_switch(); } ardrone_tool_input_start_reset(); } else // Not emergency landing { if(navdata_state.wasEmergency && (input_state->user_input & (1 << ARDRONE_UI_BIT_SELECT))) { ardrone_tool_set_ui_pad_select(0); } if(!(pnd->ardrone_state & ARDRONE_TIMER_ELAPSED)) navdata_state.isEmergency = FALSE; if(input_state->user_input & (1 << ARDRONE_UI_BIT_START)) { navdata_state.isTakeOff = (pnd->ardrone_state & ARDRONE_USER_FEEDBACK_START) ? TRUE : FALSE; } else { navdata_state.isTakeOff = FALSE; } } navdata_state.wasEmergency = (pnd->ardrone_state & ARDRONE_EMERGENCY_MASK) ? TRUE : FALSE; // Video record management bool_t usbRecordEnable = FALSE; if(navdata_state.record_state == RECORD_STATE_NEEDED && TAKEOFF_STATE_IDLE == navdata_state.takeoff_state) { bool_t continueRecord = TRUE; bool_t nextInternalRecordState = FALSE; if (IS_LEAST_ARDRONE2) { static codec_type_t oldCodec; codec_type_t cancelCodec; if (recordIsReady && ! navdata_state.usbRecordInProgress && !navdata_state.internalRecordInProgress) // We want to enable recording { if ((ARDRONE_USB_MASK == (pnd->ardrone_state & ARDRONE_USB_MASK)) && (0 < pnd->navdata_hdvideo_stream.usbkey_remaining_time) && (TRUE == ardrone_control_config.video_on_usb)) { usbRecordEnable = TRUE; } // Set the "non-record codec" to the codec defined as the application default oldCodec = ardrone_application_default_config.video_codec; cancelCodec = oldCodec; ardrone_control_config.video_codec = (TRUE == usbRecordEnable) ? usbRecordCodec : deviceWifiRecordCodec; PRINT ("Set codec %d -> %d\n", oldCodec, ardrone_control_config.video_codec); nextInternalRecordState = TRUE; } else // We want to disable recording { cancelCodec = ardrone_control_config.video_codec; ardrone_control_config.video_codec = oldCodec; PRINT ("Reset codec %d -> %d\n", cancelCodec, oldCodec); } bool_t addEventSucceeded = ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_codec, &ardrone_control_config.video_codec, NULL); if (FALSE == addEventSucceeded) { PRINT ("Unable to send codec switch ... retry later\n"); ardrone_control_config.video_codec = cancelCodec; continueRecord = FALSE; } } else if (IS_ARDRONE1) { nextInternalRecordState = !recordIsInProgress; } if (TRUE == continueRecord) { navdata_state.internalRecordInProgress = nextInternalRecordState; switch (navdata_state.userbox_state) { case USERBOX_STATE_STOPPED: navdata_state.record_state = RECORD_STATE_WAIT_USERBOX; navdata_state.usbRecordInProgress = usbRecordEnable; ardrone_academy_navdata_userbox_switch(); break; case USERBOX_STATE_STARTED: if(navdata_state.isTakeOff) { if(! recordIsReady) { PRINT("Userbox is started and record is started => Stop record\n"); ardrone_academy_navdata_recorder_enable(FALSE, navdata_state.userbox_time); navdata_state.usbRecordInProgress = FALSE; } else { PRINT("Userbox is started and record is stopped => Start record\n"); if (FALSE == usbRecordEnable) { // Only activate the local recorder if we don't record on USB ardrone_academy_navdata_recorder_enable(TRUE, navdata_state.userbox_time); navdata_state.usbRecordInProgress = FALSE; } else { navdata_state.usbRecordInProgress = TRUE; } } navdata_state.record_state = RECORD_STATE_IDLE; } else { navdata_state.record_state = RECORD_STATE_WAIT_USERBOX; ardrone_academy_navdata_userbox_switch(); } break; case USERBOX_STATE_STARTING: navdata_state.usbRecordInProgress = usbRecordEnable; navdata_state.record_state = RECORD_STATE_WAIT_USERBOX; break; case USERBOX_STATE_STOPPING: navdata_state.usbRecordInProgress = usbRecordEnable; // Should never be here PRINT ("Don't know what to do for USERBOX_STATE_STOPPING\n"); VP_OS_ASSERT (0 == 1); // Debug handler break; } } } // Screenshot management prevCameraIsReady = navdata_state.cameraIsReady; navdata_state.cameraIsReady = (pnd->ardrone_state & ARDRONE_CAMERA_MASK) ? TRUE : FALSE; if (TRUE == navdata_state.cameraIsReady && FALSE == prevCameraIsReady) { academy_download_resume (); } if((navdata_state.screenshot_state == SCREENSHOT_STATE_NEEDED) && navdata_state.cameraIsReady) { static char param[ARDRONE_DATE_MAXSIZE + 64]; static char date[ARDRONE_DATE_MAXSIZE]; time_t t = time(NULL); ardrone_time2date(t, ARDRONE_FILE_DATE_FORMAT, date); navdata_state.screenshot_state = SCREENSHOT_STATE_INPROGRESS; sprintf(param, "%d,%d,%d,%s", USERBOX_CMD_SCREENSHOT, 0, 0, date); ARDRONE_TOOL_CONFIGURATION_ADDEVENT(userbox_cmd, param, ardrone_academy_navdata_screenshot_cb); } // USB management navdata_state.usbIsReady = (pnd->ardrone_state & ARDRONE_USB_MASK) ? TRUE : FALSE; UNLOCK_ND_MUTEX (); } return C_OK; }
//NEW THREADS ADDED BY ME DEFINE_THREAD_ROUTINE(drone_logic, data){ //the game is active from start, but the logic will start only when a match is active struct timespec flying_sleep_time; flying_sleep_time.tv_sec = 0; flying_sleep_time.tv_nsec = 0000000; int emptiness_counter = 0; int shooting_counter = 0; int hovering = 0; //0 hover, 1 move float phi = 0.0; //left/right angle. Between [-1.0,+1.0], with negatives being leftward movement float theta = 0.0; //front/back angle. Between [-1.0, +1.0], with negatives being frontward movement float gaz = 0.0; //vertical speed. Between [-1.0, +1.0] float yaw = 0.0; //Angular speed. Between [-1.0, +1.0] while(game_active){ if(match_active){ if(takeoff){ //TODO:Uncomment this when you are ready to make the drone move autonomously //ardrone_tool_set_ui_pad_start(1); //ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0); takeoff = 0; } //--- CHASING ---// //NOTE: Hill have higher priority than enemy, this imply that if there is the enemy and an hill //the drone will choose the hill. //NOTE: doing so, the drone won't care if the enemy is standing between it and the hill if(hill_in_sight){ emptiness_counter = 0; shooting_counter = 0; //move toward the hill if((hill_distance > HILL_MIN_DISTANCE) && (hill_distance < HILL_MAX_DISTANCE)){ printf("MOVING TOWARD THE HILL\n"); hovering = 1; phi = 0.0; gaz = 0.0; //--- SET THE YAW ---// //This is to correct the direction of the drone if(abs(hill_offset_from_center) > ERROR_FROM_CENTER_FOR_HILL){ //TODO: find the right multiplier for yaw and discover which way the drone turn yaw = (hill_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007 //yaw has to be between -1.0 and +1.0 if(yaw > 1.0) { yaw = 1.0; } else if(yaw < -1.0){ yaw = -1.0; } } //--- SET THE APPROACHING SPEED ---// //The closer the drone is to the hill, the slower it goes //Need to be negative to move forward theta = -1*((hill_distance) / THETA_COEFF); //TODO: find the right theta_coeff if(theta > 0.0) { theta = -0.1; } else if(theta < -1.0){ //TODO: I may want to rethink the maximum speed that the drone can reach! theta = -1.0; } flying_sleep_time.tv_sec = 1; flying_sleep_time.tv_nsec = 0000000; //hover over the hill } else if(hill_distance < HILL_MIN_DISTANCE) { //ardrone_at_set_progress_cmd(0,0,0,0,0); //to hover printf("HOVERING ON TOP OF THE HILL\n"); hovering = 0; phi = 0.0; theta = 0.0; gaz = 0.0; yaw = 0.0; //TODO: if you are close enough, you have to switch the cam and then inizialize //the recognition procedure. (wait tot secs) //TODO: I have problem switching cam vp_os_mutex_lock(&drone_score_mutex); drone_add_score = 1; vp_os_mutex_unlock(&drone_score_mutex); //TODO: here, you switch the camera back flying_sleep_time.tv_sec = 5; flying_sleep_time.tv_nsec = 0000000; } } else if(enemy_in_sight){ emptiness_counter = 0; //back up! Too close to the enemy (we don't want to phisically hit the human player!) if(enemy_distance < ENEMY_MIN_DISTANCE){ shooting_counter = 0; hovering = 1; theta = 1.0; //Move backward at maximum speed phi = 0.0; gaz = 0.0; yaw = 0.0; //TODO: test this! flying_sleep_time.tv_sec = 2; flying_sleep_time.tv_nsec = 0000000; printf("BACKING UP FROM THE ENEMY\n"); } else if((enemy_distance > ENEMY_MIN_DISTANCE) && (enemy_distance < ENEMY_SHOOTING_DISTANCE)){ printf("INSIDE SHOOTING!!!!\n"); shooting_counter++; if(shooting_counter > 5){ //TODO: make the drone move //TODO:set some sleep time printf("ENOUGH WITH THE SHOOTING\n"); } else { //TODO: shoot!! //make animation. if(enemy_offset_from_center < ERROR_FROM_CENTER_FOR_ENEMY){ vp_os_mutex_lock(&enemy_score_mutex); enemy_lose_score = 1; vp_os_mutex_lock(&enemy_score_mutex); } printf("SHOOTING!!!!!!!!\n"); hovering = 0; phi = 0.0; theta = 0.0; gaz = 0.0; //--- SET THE YAW ---// //This is to correct the direction of the drone if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){ //TODO: find the right multiplier for yaw and discover wich way the drone turn yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007 //yaw has to be between -1.0 and +1.0 if(yaw > 1.0) { yaw = 1.0; } else if(yaw < -1.0){ yaw = -1.0; } } flying_sleep_time.tv_sec = 1; flying_sleep_time.tv_nsec = 0000000; } } else if((enemy_distance > ENEMY_SHOOTING_DISTANCE) && (enemy_distance < ENEMY_MAX_DISTANCE)){ shooting_counter = 0; //TODO: in this case I may want to just make the drone search for hills //TODO: it may escape, turning itself so the led won't face the enemy anymore //TODO: set some sleep time //--- SET THE YAW ---// //This is to correct the direction of the drone if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){ //TODO: find the right multiplier for yaw and discover wich way the drone turn //TODO: in this case, I won't to make the drone move away from the enemy, so the algorithm below //has to be changed yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007 //yaw has to be between -1.0 and +1.0 if(yaw > 1.0) { yaw = 1.0; } else if(yaw < -1.0){ yaw = -1.0; } } } //NOTHING IN SIGHT } else { //If nothing is in sight I set a counter that increment every time I pass directly from here //and make the drone rotate around itself. After tot passages with nothing in sight I take measure to land the drone. if(emptiness_counter == 0){ //TODO: set up everything for the algorithm to start emptiness_counter = 1; //yaw = something != to zero; //everything else if zero (I don't know about hovering... problably should be 1 here) } else { emptiness_counter++; //TODO: set some sleep time if(emptiness_counter > 10){ //TODO:land the drone and make the game stop //so set everything that should to zero } } } //---TELL THE DRONE TO MOVE---// //TODO: uncomment the line below when you're ready! //ardrone_at_set_progress_cmd(hovering,phi,theta,gaz,yaw); nanosleep(&flying_sleep_time, NULL); //NOTE: if the sleep remain under 2 secs it's ok. //Only the hill recognition can have more, because the drone can't be hurt at that time. //NOTE: I don't know why, but if I add this printf the condition below works //printf("%d",drone_wounded); //---BEING HIT---// if(drone_wounded){ //TODO: make the drone move as if it was being shot //maybe this should be moved in the flying thread vp_os_mutex_lock(&drone_wound_mutex); drone_wounded = 0; vp_os_mutex_unlock(&drone_wound_mutex); //TODO: you can choose between this animation, defined in Soft/Common/config.h /*ARDRONE_ANIM_PHI_M30_DEG= 0, A RDRONE_ANIM_PHI_30_DE*G, ARDRONE_ANIM_THETA_M30_DEG, ARDRONE_ANIM_THETA_30_DEG, ARDRONE_ANIM_THETA_20DEG_YAW_200DEG, ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG, ARDRONE_ANIM_TURNAROUND, ARDRONE_ANIM_TURNAROUND_GODOWN, ARDRONE_ANIM_YAW_SHAKE, ARDRONE_ANIM_YAW_DANCE, ARDRONE_ANIM_PHI_DANCE, ARDRONE_ANIM_THETA_DANCE, ARDRONE_ANIM_VZ_DANCE, ARDRONE_ANIM_WAVE, ARDRONE_ANIM_PHI_THETA_MIXED, ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED, ARDRONE_ANIM_FLIP_AHEAD, ARDRONE_ANIM_FLIP_BEHIND, ARDRONE_ANIM_FLIP_LEFT, ARDRONE_ANIM_FLIP_RIGHT, ARDRONE_NB_ANIM_MAYDAY*/ //anim_mayday_t param; //ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_anim, param, myCallback); ardrone_at_set_led_animation(BLINK_GREEN_RED, 0.25, 4); //TODO: freeze the drone for some time, also? //nanosleep(&shot_rumble_time, NULL); } //MATCH OVER } else { //land the drone ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0); ardrone_tool_set_ui_pad_start(0); //TODO: I may need some sleep time to make the drone land before closing everyting } } return C_OK; }
void ardronewin32_take_off() { ardrone_tool_set_ui_pad_start(1); }
void ardronewin32_land() { ardrone_tool_set_ui_pad_start(0); }
C_RESULT update_radioGP(void) { static float32_t roll = 0, pitch = 0, gaz=0, yaw=0; static bool_t refresh_values = FALSE; ssize_t res; static struct js_event js_e_buffer[64]; 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; 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 GP_BOARD_LEFT : ardrone_tool_set_ui_pad_start(js_e_buffer[idx].value); break; case GP_SIDE_RIGHT : ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value); break; case GP_IMPULSE : ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value); break; case GP_SIDE_LEFT_DOWN : ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value); break; case GP_SIDE_LEFT_UP : ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value); 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 GP_PITCH: pitch = js_e_buffer[idx].value; break; case GP_GAZ: gaz = js_e_buffer[idx].value; break; case GP_ROLL: roll = js_e_buffer[idx].value; break; case GP_PID: break; case GP_YAW: yaw = js_e_buffer[idx].value; break; default: break; } } else {// TODO: default: ERROR (non-supported) } } if(refresh_values)// Axis values to refresh { ardrone_at_set_progress_cmd( 1, roll/25000.0, pitch/25000.0, -gaz/25000.0, yaw/25000.0); } return C_OK; }
C_RESULT update_teleop(void) { // This function *toggles* the emergency state, so we only want to toggle the emergency // state when we are in the emergency state (because we want to get out of it). vp_os_mutex_lock(&twist_lock); if (needs_reset) { ardrone_tool_set_ui_pad_select(1); needs_reset = false; } else if (needs_takeoff) { ardrone_tool_set_ui_pad_start(1); needs_takeoff = false; } else if (needs_land) { ardrone_tool_set_ui_pad_start(0); needs_land = false; } else { float left_right = (float) cmd_vel.linear.y; float front_back = (float) cmd_vel.linear.x; float up_down = (float) cmd_vel.linear.z; float turn = (float) cmd_vel.angular.z; bool is_changed = !( (fabs(left_right - old_left_right) < _EPS) && (fabs(front_back - old_front_back) < _EPS) && (fabs(up_down - old_up_down) < _EPS) && (fabs(turn - old_turn) < _EPS) ); // These lines are for testing, they should be moved to configurations // Bit 0 of control_flag == 0: should we hover? // Bit 1 of control_flag == 1: should we use combined yaw mode? int32_t control_flag = 0x00; int32_t combined_yaw = 0x00; // Auto hover detection based on ~0 values for 4DOF cmd_vel int32_t hover = (int32_t) ( (fabs(left_right) < _EPS) && (fabs(front_back) < _EPS) && (fabs(up_down) < _EPS) && (fabs(turn) < _EPS) && // Set angular.x or angular.y to a non-zero value to disable entering hover // even when 4DOF control command is ~0 (fabs(cmd_vel.angular.x) < _EPS) && (fabs(cmd_vel.angular.y) < _EPS) ); control_flag |= ((1 - hover) << 0); control_flag |= (combined_yaw << 1); //ROS_INFO (">>> Control Flag: %d", control_flag); old_left_right = left_right; old_front_back = front_back; old_up_down = up_down; old_turn = turn; //is_changed = true; if ((is_changed) || (hover)) { ardrone_tool_set_progressive_cmd(control_flag, left_right, front_back, up_down, turn, 0.0, 0.0); } } vp_os_mutex_unlock(&twist_lock); return C_OK; }
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