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); }