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;
}
bool configCb(ardrone2_mudd::Config::Request &req,
ardrone2_mudd::Config::Response &res)
{
std::vector<std::string> command;
std::istringstream iss(req.command);
std::copy(std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>(),
std::back_inserter<std::vector<std::string> >(command));
printf("Command: %s \n", command[0].c_str());
if (command[0].compare("camera") == 0) {
int channel = atoi(command[1].c_str());
if ((channel >= 0) && (channel <= 4))
{
if (channel == 4)
currentCamera = (currentCamera + 1)%3;
else
currentCamera = channel;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &channel, NULL);
}
} else if (command[0].compare("anim") == 0) {
char buffer[128];
int anim = atoi(command[1].c_str());
snprintf(buffer,sizeof(buffer),"%i,%i",anim, MAYDAY_TIMEOUT[anim]);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(flight_anim, buffer, NULL);
} else if (command[0].compare("retrim") == 0) {
ardrone_at_set_flat_trim();
}
return true;
}
int _stdcall SendFlatTrim()
{
ardrone_at_set_flat_trim();
printf("Sent flat trim\n");
return 0;
}
bool flatTrimCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
{
vp_os_mutex_lock(&twist_lock);
ardrone_at_set_flat_trim();
vp_os_mutex_unlock(&twist_lock);
fprintf(stderr, "\nFlat Trim Set.\n");
}
void ARDroneRos::trimCallback(const std_msgs::Empty &msg)
{
boost::lock_guard<boost::mutex> guard (control_mutex_);
if(current_control_msg_.airborne == false)
{
ardrone_at_set_flat_trim();
}
}
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;
}
void parrot_ardrone_ctrl_set_flat_trim()
{
ardrone_at_set_flat_trim();
}
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
DEFINE_THREAD_ROUTINE(ihm, data)
{
C_RESULT res;
WSADATA wsaData = {0};
int iResult = 0;
/* Initializes Windows socket subsystem */
iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
if (iResult != 0)
{
wprintf(L"WSAStartup failed: %d\n", iResult);
return 1;
}
/* Initializes communication sockets */
res = test_drone_connection(); // Nick disabled the press enter (wait)
if(res!=0)
{
printf("%s","Could not detect the drone version ... press <Enter> to try connecting anyway.\n");
getchar();
//WSACleanup();
exit(-1);
}
res = ardrone_tool_setup_com( NULL );
if( FAILED(res) )
{
PRINT("Wifi initialization failed.\n");
return -1;
}
START_THREAD(video_stage, 0);
res = ardrone_tool_init(WIFI_ARDRONE_IP, strlen(WIFI_ARDRONE_IP), NULL, ARDRONE_CLIENT_APPNAME, ARDRONE_CLIENT_USRNAME);
ardrone_tool_set_refresh_time(20); // 20 ms
ardrone_at_reset_com_watchdog();
// config
ardrone_control_config.euler_angle_max = 0.20943951f; // 12 degrees
ardrone_control_config.video_channel = ZAP_CHANNEL_VERT;
ardrone_control_config.video_codec = UVLC_CODEC; //P264_CODEC;
ardrone_control_config.navdata_demo = FALSE;
ardrone_control_config.altitude_max = 10000;
ardrone_control_config.control_vz_max = 1000.0f;
ardrone_control_config.outdoor = FALSE;
//ardrone_control_config.flight_without_shell = TRUE;
ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &ardrone_control_config.video_channel, (ardrone_tool_configuration_callback) ardrone_demo_config_callback);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_channel, &ardrone_control_config.video_channel, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT(video_codec, &ardrone_control_config.video_codec, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &ardrone_control_config.navdata_demo, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (altitude_max, &ardrone_control_config.altitude_max, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (control_vz_max, &ardrone_control_config.control_vz_max, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (outdoor, &ardrone_control_config.outdoor, NULL);
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_without_shell, &ardrone_control_config.flight_without_shell, NULL);
ARDRONE_TOOL_CONFIGURATION_ADDEVENT (euler_angle_max, &ardrone_control_config.euler_angle_max, NULL);
// flat trim
ardrone_at_set_flat_trim();
//SetEvent(ardrone_ready);
//ardrone_demo_redirect_to_interface = 1;
while( VP_SUCCEEDED(res) && ardrone_tool_exit() == FALSE )
{
res = ardrone_tool_update();
}
JOIN_THREAD(video_stage);
res = ardrone_tool_shutdown();
WSACleanup();
return (THREAD_RET)res;
}
C_RESULT update_gamepad(void) {
static int32_t start;
static float phi, theta, gaz, yaw;
static int cmd_mode = 0;
static int32_t animation = 0;
static char *animations[] = {
"PHI_M30_DEG","PHI_30_DEG","THETA_M30_DEG","THETA_30_DEG",
"THETA_20DEG_YAW_200DEG","THETA_20DEG_YAW_M200DEG",
"Turnaround","Turnaround & Down","Turn Shake","Turn Dance",
"Roll Dance","Tilt Dance","VZ_DANCE","Wave",
"PHI_THETA_MIXED","DOUBLE_PHI_THETA_MIXED",
"Front Flip","Back Flip","Left Flip","Right Flip"
};
struct js_event js_e_buffer[64];
ssize_t 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)
bool_t refresh_values = FALSE;
int32_t idx = 0;
for (idx = 0; idx < res / sizeof(struct js_event); idx++) {
unsigned char type = js_e_buffer[idx].type;
unsigned char number = js_e_buffer[idx].number;
short value = js_e_buffer[idx].value;
if ( type & JS_EVENT_INIT ) {
break;
}
else if ( type & JS_EVENT_BUTTON ) {
switch( number ) {
case BUTTON_START :
if( value == 1 ){
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
g_autopilot = FALSE;
}
break;
case BUTTON_SELECT :
if( value == 1 ){
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
return C_OK;
}
case BUTTON_ZAP:
if( value == 1 ){
// Using the zap button to pass in a notification to the agent
// instead of changing the camera (maybe i should move zap to a hat)
// a button that sets a auto-hover mode might be good too, but tomorrow
// note: in the event you could do it fast enough (you can't) you would overwrite
// a previous press before it gets sent
g_pass_button = BUTTON_ZAP;
}
break;
case BUTTON_HOVER:
// reset values and set hover mode regardless of current command mode
// this operates outside of the normal handling so refreshing values isn't necessary
// autopilot will still be cancelled though
if( value == 1 ){
g_autopilot = FALSE;
phi = 0; theta = 0; gaz = 0; yaw = 0;
ardrone_tool_set_progressive_cmd(0,0,0,0,0,0,0);
}
break;
case BUTTON_UP:
if( value == 1 ){
phi = 0; theta = -0.1; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_DOWN:
if( value == 1 ){
phi = 0; theta = 0.1; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_LEFT:
if( value == 1 ){
phi = -0.1; theta = 0; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_RIGHT:
if( value == 1 ){
phi = 0.1; theta = 0; gaz = 0; yaw = 0;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_L1:
if( value == 1 ){
animation++;
if( animation >= ARDRONE_NB_ANIM_MAYDAY ){
animation = 0;
}
PRINT("Switched flight animation to %s\n", animations[animation]);
}
break;
case BUTTON_R1:
if( value == 1 ){
PRINT("Performing flight animation %s\n", animations[animation]);
ardrone_at_set_anim( animation, MAYDAY_TIMEOUT[animation] );
}
break;
case BUTTON_L2:
if( value == 1 ){
phi = 0; theta = 0; gaz = 0; yaw = -0.2;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_R2:
if( value == 1 ){
phi = 0; theta = 0; gaz = 0; yaw = 0.2;
}else{
phi = 0; theta = 0; gaz = 0; yaw = 0;
}
refresh_values = TRUE;
break;
case BUTTON_FTRIM:
if( value == 1 ){
ardrone_at_set_flat_trim();
PRINT("Flat trim request sent\n");
}
break;
case BUTTON_AUTO:
if( value == 1 ){
if (g_autopilot) {
refresh_values = TRUE;
}
else {
g_autopilot = TRUE;
}
}
break;
case BUTTON_LHAT:
if( value == 1 ){
if( cmd_mode == 0 ){
PRINT("ACTIVE mode set. auto-hover disabled\n");
cmd_mode = 1;
}else{
PRINT("ASSISTED mode set. auto-hover enabled\n");
cmd_mode = 0;
}
}
}
}
else if (type & JS_EVENT_AXIS ) {
if (number == AXIS_PHI || number == AXIS_THETA || number == AXIS_GAZ || number == AXIS_YAW) {
refresh_values = TRUE;
float angle = value / (float)SHRT_MAX;
switch (number) {
case AXIS_PHI:
phi = angle;
break;
case AXIS_THETA:
theta = angle;
break;
case AXIS_GAZ:
gaz = ( angle * -1 );
break;
case AXIS_YAW:
yaw = angle;
break;
}
}
}
} // loop over events
if (refresh_values) {
g_autopilot = FALSE;
if( phi == 0 && theta == 0 && gaz == 0 && yaw == 0 ){
ardrone_tool_set_progressive_cmd(cmd_mode,0,0,0,0,0,0);
}else{
ardrone_tool_set_progressive_cmd(1,phi,theta,gaz,yaw,0,0);
}
//set(phi, theta, gaz, yaw);
}
return C_OK;
}
void flattrimCallback(const std_msgs::Empty &msg)
{
vp_os_mutex_lock(&twist_lock);
ardrone_at_set_flat_trim();
vp_os_mutex_unlock(&twist_lock);
}
