/*--------------------------------------------------------------------------------------------------------------------- Navdata handling function, which is called every time navdata are received ---------------------------------------------------------------------------------------------------------------------*/ inline C_RESULT demo_navdata_client_process( const navdata_unpacked_t* const navdata ) { static int cpt=0; const navdata_demo_t* const nd = &navdata->navdata_demo; /** ======= INSERT USER CODE HERE ========== **/ ARWin32Demo_AcquireConsole(); if ((cpt++)==90) { system("cls"); cpt=0; } ARWin32Demo_SetConsoleCursor(0,0); // Print at the top of the console printf("=================================\n"); printf("Navdata for flight demonstrations\n"); printf("=================================\n"); printf("Control state : %s \n",ctrl_state_str(nd->ctrl_state)); printf("Battery level : %i/100 \n",nd->vbat_flying_percentage); printf("Orientation : [Theta] %4.3f [Phi] %4.3f [Psi] %4.3f \n",nd->theta,nd->phi,nd->psi); printf("Altitude : %i \n",nd->altitude); printf("Speed : [vX] %4.3f [vY] %4.3f [vZ] %4.3f \n",nd->vx,nd->vy,nd->vz); ARWin32Demo_ReleaseConsole(); /** ======= INSERT USER CODE HERE ========== **/ return C_OK; }
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; }
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 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; }