/*---------------------------------------------------------------------------------------------------------------------
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;
}
