void ardronewin32_recover(int send)
{
if (send == 1)
ardrone_tool_set_ui_pad_select(1);
else
ardrone_tool_set_ui_pad_select(0);
}
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_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 SendEmergency()
{
ardrone_tool_set_ui_pad_start(0);
ardrone_tool_set_ui_pad_select(1);
need_update = TRUE;
printf("Sent Emergency\n");
return 0;
}
DEFINE_THREAD_ROUTINE(drone_main_thread, data) {
ardrone_tool_set_ui_pad_select(0); // Do not send any emergency signal
ardrone_at_set_navdata_all();
// Uint32 emergencyPulseStartTime = SDL_GetTicks() + EMERGENCY_SIGNAL_PULSE_DURATION;
//
// int doRun = 1;
//
// while (doRun) {
// ardrone_tool_update();
//
// pthread_mutex_lock(&drone_sharedDataMutex);
// int emergencyPulseMS = SDL_GetTicks() - emergencyPulseStartTime;
// if ((drone_emergency != drone_targetEmergency) && ((emergencyPulseMS > 2 * EMERGENCY_SIGNAL_PULSE_DURATION) || (emergencyPulseMS < EMERGENCY_SIGNAL_PULSE_DURATION))) {
// ardrone_tool_set_ui_pad_select(1);
//
// if (emergencyPulseMS > 2 * EMERGENCY_SIGNAL_PULSE_DURATION) {
// emergencyPulseStartTime = SDL_GetTicks();
// }
// } else {
// ardrone_tool_set_ui_pad_select(0);
// }
//
// doRun = drone_doRun;
// pthread_mutex_unlock(&drone_sharedDataMutex);
// }
//
Uint32 startTime;
// Enforce landing...
startTime = SDL_GetTicks();
while ((!drone_landed) && (SDL_GetTicks() - startTime < DRONE_LAND_WAIT_LIMIT)) {
drone_land();
ardrone_tool_update();
}
if (!drone_landed) {
fprintf(stderr, "Warning! Drone could not land within %d seconds! Trying to set emergency flag...\n", DRONE_LAND_WAIT_LIMIT / 1000);
startTime = SDL_GetTicks();
while ((!(drone_emergency) || (drone_landed)) && (SDL_GetTicks() - startTime < DRONE_EMERGENCY_WAIT_LIMIT)) {
drone_setEmergency(1);
ardrone_tool_update();
}
if ((!(drone_emergency) || (drone_landed))) {
fprintf(stderr, "Error! Drone could not land and no emergency flag could be set. If it is still flying: Good luck!\n");
}
}
return 0;
}
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 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;
}
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_wiimote(void)
{
C_RESULT res = C_OK;
static struct cwiid_state state,previous_state;
static int control_mode = 1;
static int valid_domain = 0;
int8_t x, y;
static int start=0;
static int select=0;
static vec3 a={0,0,0},s={0,0,0};
static vec3sph r={0,0,0},pr={0,0,0},rref={0,0,0};
static float pitch=0.0f,roll=0.0f,gaz=0.0f,yaw=0.0f;
float tmp;
static int fly=0;
if (cwiid_get_state(wiimote, &state))
{
fprintf(stderr, "Error getting state\n");
res = C_FAIL;
}
#define SWITCHING(X) ((state.buttons&X) && !(previous_state.buttons&X))
#define RELEASING(X) ((!state.buttons&X) && (previous_state.buttons&X))
#define PRESSED(X) ((state.buttons&X))
static int flag_rumble=0;
#define RUMBLE_ON { if (!flag_rumble) {flag_rumble=1; } }
/* Sets how to use the wiimote */
#define MAXMODE 4
if (SWITCHING(CWIID_BTN_MINUS)) {
control_mode--;
if (control_mode<1) control_mode=MAXMODE;
leds(1<<(control_mode-1));
}
if (SWITCHING(CWIID_BTN_PLUS)) {
control_mode++;
if (control_mode>MAXMODE) control_mode=1;
leds(1<<(control_mode-1));
}
/* Gets gravitation G projection on x,y,z axis */
a.x = - (float32_t) ((((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
(wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X])));
a.y = - (float32_t) ((((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
(wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y])));
a.z = + (float32_t) ((((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
(wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z])));
s.x = (a.x<0.0f)?(1.0f):(-1.0f);
s.y = (a.y<0.0f)?(1.0f):(-1.0f);
s.z = (a.z<0.0f)?(1.0f):(-1.0f);
float ax2 = a.x*a.x;
float ay2 = a.y*a.y;
float az2 = a.z*a.z;
r.r = sqrtf((ax2+ay2)+az2);
switch(control_mode)
{
case 1:
case 2:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle gauche/droite
r.p = asin(a.y); if (isnan(r.p)) r.p=0.0f;
// Sur plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
case 3:
case 4:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle entre le projete de G sur le plan vertical longitudinal (yz) et l'axe z
r.p = acos(a.z/(sqrtf(az2+ay2))); if (isnan(r.p)) r.p=0.0f;
/* If wiimote faces the ground */
//if (a.z<0.0f) r.p= M_PI-r.p;
r.p*=s.y;
// Idem sur le plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
}
r.r = (r.r+pr.r)/2.0f;
r.t = (r.t+pr.t)/2.0f;
r.p = (r.p+pr.p)/2.0f;
switch(control_mode)
{
case 1:
case 2:
/* Wiimote is handled horizontally.
* '2' button under left thumb
* directionnal cross under right thumb
*/
/* 0 -> buttons facing sky */
if ((SWITCHING(CWIID_BTN_1)||SWITCHING(CWIID_BTN_2)||SWITCHING(CWIID_BTN_B))){ rref=r; }
if (PRESSED(CWIID_BTN_1)||PRESSED(CWIID_BTN_2)||PRESSED(CWIID_BTN_B))
{
/* If wiimote facing ground */
if (a.z<0 && a.x>0)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = (r.t-rref.t)*1.0f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = -(r.p-rref.p)*0.75f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
pitch=roll=0;
rumble(0);
leds(1<<(control_mode-1));
fly=0;
}
gaz = (PRESSED(CWIID_BTN_LEFT))? 1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? -1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? 1.0f : (control_mode==2) ? (-pitch*roll) : 0.0f;
break;
case 3:
case 4:
if (PRESSED(CWIID_BTN_B))
{
if (a.z<-0.5f)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = -(r.p-rref.p)*1.5f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = (r.t-rref.t)*1.5f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
rumble(0);
leds(1<<(control_mode-1));
fly=0;
pitch=roll=0;
}
if (SWITCHING(CWIID_BTN_B)) { rref = r; }
gaz = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? +1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_LEFT))? -1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? 1.0f : (control_mode==4) ? (-pitch*roll) : 0.0f;
}
/* Buttons common to all modes */
if (SWITCHING(CWIID_BTN_A)) { start^=1; ardrone_tool_set_ui_pad_start(start); }
if (SWITCHING(CWIID_BTN_HOME)) {
select^=1;
ardrone_tool_set_ui_pad_select(select);
ardrone_tool_set_ui_pad_start(0);
}
//
//printf("Wiimote mode 2 [ax %f][ay %f][az %f]\n\033[1A", a.x,a.y,a.z);
printf("Wiimode %i [rr %3.2f][rp %3.2f][rt %3.2f]\n", control_mode,r.r,r.p,r.t);
printf("[Fly %i][Ptc %3.2f][Rl %3.2f][Yw %3.2f][Gz %3.2f][Strt. %i][Sel. %i]\n\033[2A", fly,pitch,roll,yaw,gaz,start,select);
ardrone_at_set_progress_cmd( fly,/*roll*/roll,/*pitch*/pitch,/*gaz*/gaz,/*yaw*/yaw);
/*api_set_iphone_acceleros(
(state.buttons&CWIID_BTN_2 ? 1 : 0)|(state.buttons&CWIID_BTN_A ? 2 : 0),
a_x, a_y, a_z);*/
previous_state = state;
pr=r;
return C_OK;
}
void checkErrors(void)
{
input_state_t* input_state = ardrone_tool_get_input_state();
strcpy(ctrldata.error_msg, "");
if(!ctrldata.wifiReachabled)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_WIFI_NOT_REACHABLED]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else
{
if(ctrldata.needAnimation >= 0)
{
ardrone_at_set_anim(ctrldata.needAnimation, MAYDAY_TIMEOUT[ctrldata.needAnimation]);
ctrldata.needAnimation = -1;
}
if(ctrldata.needVideoSwitch >= 0)
{
ardrone_at_zap(ctrldata.needVideoSwitch);
ctrldata.needVideoSwitch = -1;
}
if(ctrldata.needChangeCameraDetection >= 0)
{
ardrone_at_cad(ctrldata.needChangeCameraDetection, ctrldata.tag_size);
ctrldata.needChangeCameraDetection = -1;
}
if(ctrldata.needLedAnimation >= 0)
{
ardrone_at_set_led_animation(ctrldata.needLedAnimation, ctrldata.ledAnimFreq, ctrldata.ledAnimDuration);
ctrldata.needLedAnimation = -1;
}
if(ctrldata.needSetEmergency)
{
ctrldata.isInEmergency = ctrlnavdata.emergencyLanding;
ardrone_tool_set_ui_pad_select(1);
ctrldata.needSetEmergency = FALSE;
}
if( ctrldata.needSetNavdataDemo)
{
if( control_ack_configure_navdata_demo(ctrldata.navdata_demo))
ctrldata.needSetNavdataDemo = FALSE;
}
else if( ctrldata.needSetPairing )
{
if(control_ack_configure_mac_address((const char*)&iphone_mac_address[0]))
ctrldata.needSetPairing = FALSE;
}
else if(ctrldata.needSetFrequency)
{
if(control_ack_configure_ultrasound_frequency(!ctrldata.is_client))
ctrldata.needSetFrequency = FALSE;
}
else if(ctrldata.needSetManualTrim)
{
if(control_ack_configure_manual_trim(ctrldata.manual_trim))
{
ctrldata.needSetManualTrim = FALSE;
ctrldata.manual_trim_enabled = ctrldata.manual_trim;
}
}
else if(ctrldata.needGetConfiguration)
{
//PRINT("Request configuration file\n");
if(control_get_configuration())
ctrldata.needGetConfiguration = FALSE;
}
if((ctrldata.framecounter % (kFPS / 2)) == 0)
{
if(ctrlnavdata.bootstrap)
{
setNavdataDemo(TRUE);
ctrldata.needSetFrequency = TRUE;
ctrldata.needGetConfiguration = TRUE;
ctrldata.needSetPairing = TRUE;
}
}
if(ardrone_navdata_client_get_num_retries() >= NAVDATA_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_CANT_CONNECT_TO_TOY]);
strcat(ctrldata.error_msg, "\n");
resetControlData();
}
else if(ctrlnavdata.emergencyLanding)
{
if(!ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
strcpy(ctrldata.emergency_msg, "reset");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PRESS_RESET]);
strcat(ctrldata.error_msg, "\n");
if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cutoutProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CUT_OUT]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.motorsProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_MOTORS]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.cameraProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_CAMERA]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_WATCHDOG]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.adcVersionProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_PIC_VERSION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.anglesProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_TOO_MUCH_ANGLE]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.userEmergency)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_USER]);
strcat(ctrldata.error_msg, "\n");
}
else
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_EMERGENCY_UNKNOWN]);
strcat(ctrldata.error_msg, "\n");
}
resetControlData();
ardrone_tool_start_reset();
if(ctrlnavdata.startPressed)
switchTakeOff();
}
else if(!ctrlnavdata.emergencyLanding)
{
if(ctrldata.isInEmergency && input_state->select)
ardrone_tool_set_ui_pad_select(0);
if(video_stage_get_num_retries() > VIDEO_MAX_RETRIES)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_NO_VIDEO_CONNECTION]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.vbatLowProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_BATTERY_LOW]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.ultrasoundProblem)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_ULTRASOUND]);
strcat(ctrldata.error_msg, "\n");
}
else if(ctrlnavdata.visionProblem && ctrlnavdata.flyingState)
{
strcat(ctrldata.error_msg, messages[MESSAGEBOX_ALERT_VISION]);
strcat(ctrldata.error_msg, "\n");
}
if(!ctrlnavdata.timerElapsed)
strcpy(ctrldata.emergency_msg, "emergency");
if(input_state->start)
{
if(ctrlnavdata.startPressed)
{
strcpy(ctrldata.takeoff_msg, "take_land");
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
strcat(ctrldata.error_msg, messages[MESSAGEBOX_START_NOT_RECEIVED]);
strcat(ctrldata.error_msg, "\n");
}
}
else
{
strcpy(ctrldata.takeoff_msg, "take_off");
}
}
}
}
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
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 start;
static float phi = 0, theta = 0, gaz = 0, yaw = 0;
phi = theta = gaz = yaw = 0.0;
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 = TRUE;
bool_t enable = FALSE;
int32_t idx = 0;
printf("Num events: %d\n", sizeof(struct js_event));
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 (!value) {
break;
}
else if (type & JS_EVENT_BUTTON ) {
printf("BUTTON: %d\n", number);
refresh_values = TRUE;
switch(number ) {
case BUTTON_START :
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
g_autopilot = FALSE;
break;
case BUTTON_SELECT :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
//g_exit = TRUE;
return C_OK;
case BUTTON_ZAP:
agent_zap();
break;
case BUTTON_GAZ_UP:
enable = TRUE;
gaz = +1;
break;
case BUTTON_GAZ_DOWN:
enable = TRUE;
printf("GAZ DOWN: %f \n", -1);
gaz = -1;
break;
case BUTTON_YAW_R:
enable = TRUE;
yaw = +js_e_buffer[idx].value;
break;
case BUTTON_YAW_L:
enable = TRUE;
yaw = -js_e_buffer[idx].value;
break;
/*case BUTTON_AUTO:
if (g_autopilot) {
refresh_values = TRUE;
}
else {
g_autopilot = TRUE;
}
break;*/
}
}
else if (type & JS_EVENT_AXIS ) {
printf("AXIS: %d\n", number);
if (number != AXIS_IGNORE3 && number != AXIS_IGNORE4) {
refresh_values = TRUE;
enable = TRUE;
float angle = value / (float)SHRT_MAX;
printf("ANGLE: %f", angle);
switch (number) {
case AXIS_PHI:
phi = angle;
break;
case AXIS_THETA:
theta = angle;
break;
//case AXIS_GAZ:
// gaz = angle;
// break;
//case AXIS_YAW:
// yaw = angle;
// break;
}
}
}
} // loop over events
if (refresh_values) {
g_autopilot = FALSE;
printf("\nSENDING: %f, %f, %f, %f\n", phi, theta, gaz, yaw);
ardrone_at_set_progress_cmd(enable, phi, theta, gaz, yaw);
}
return C_OK;
}
void recover_emergency() {
ardrone_tool_set_ui_pad_select(1);
}
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;
*/
finalcmd_vel.linear.x = teleopcmd_vel.twist.linear.x+posecmd_vel.linear.x;
finalcmd_vel.linear.y = teleopcmd_vel.twist.linear.y+posecmd_vel.linear.y;
finalcmd_vel.linear.z = teleopcmd_vel.twist.linear.z+posecmd_vel.linear.z;
finalcmd_vel.angular.z = teleopcmd_vel.twist.angular.z+yawcmd_vel.angular.z;
float left_right = finalcmd_vel.linear.y;
float front_back = finalcmd_vel.linear.x;
float up_down = finalcmd_vel.linear.z;
float turn = finalcmd_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;
int32_t hover = (int32_t)
!(
(fabs(left_right) < _EPS) &&
(fabs(front_back) < _EPS) &&
(fabs(up_down) < _EPS) &&
(fabs(turn) < _EPS)
);
control_flag |= (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 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;
}
C_RESULT update_gamepad(void) {
static int32_t start;
static float phi, theta, gaz, yaw;
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 (!value) {
break;
}
else if (type & JS_EVENT_BUTTON ) {
switch(number ) {
case BUTTON_START :
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
g_autopilot = FALSE;
break;
case BUTTON_SELECT :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
return C_OK;
case BUTTON_ZAP:
zap();
break;
case BUTTON_AUTO:
if (g_autopilot) {
refresh_values = TRUE;
}
else {
g_autopilot = TRUE;
}
break;
}
}
else if (type & JS_EVENT_AXIS ) {
if (number != AXIS_IGNORE3 && number != AXIS_IGNORE4) {
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;
break;
case AXIS_YAW:
yaw = angle;
break;
}
}
}
} // loop over events
if (refresh_values) {
g_autopilot = FALSE;
set(phi, theta, gaz, yaw);
}
return C_OK;
}
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 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;
}
