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
