C_RESULT small_move( C_RESULT (*function)(void*) ){
C_RESULT ret = C_FAIL;
int32_t usec = 0;
/*default struct, hardcoded values*/
mov def_mov = {10, 0, 350};
/*for up and dow, the movement is a bit different*/
if (function == up || function == down){
def_mov.time = 400;
}
usec = 1000 * def_mov.time;
null_state(&drone_state);
ret = (*function)(&def_mov);
/* Send the command to the drone */
if ( ret == C_OK ){
ardrone_tool_set_progressive_cmd( drone_state.flag,
drone_state.phi,
drone_state.theta,
drone_state.gaz,
drone_state.yaw,
drone_state.psi,
drone_state.psi_accuracy);
if (usec != 0){
if (usec > 0){
usleep(usec);
} else {
//if the time is not valid (negative value) stop the drone!
printf("usec was < 0, stop the drone immediatly\n");
}
//stop the drone
ardrone_tool_set_progressive_cmd(0,0,0,0,0,0,0);
}
ret = C_OK;
} else {
printf("Fail send order\n");
}
return ret;
}
C_RESULT send_order( C_RESULT (*function)(void*), void *arg) {
C_RESULT ret = C_FAIL;
int32_t usec = 0;
if (arg != NULL){
mov *mv = (mov*)arg;
//fprintf(stderr, "argument passé à l'api power = %f\n", mv->power);
usec = mv->time * 1000;//d2time(mv->power, mv->distance);
}
null_state(&drone_state);
ret = (*function)(arg);
/* Send the command to the drone */
if ( ret == C_OK ){
ardrone_tool_set_progressive_cmd( drone_state.flag,
drone_state.phi,
drone_state.theta,
drone_state.gaz,
drone_state.yaw,
drone_state.psi,
drone_state.psi_accuracy);
if (usec != 0){
printf("passage dans usleep\n");
if (usec > 0){
usleep(usec);
} else {
//if the time is not valid (negative value) stop the drone!
printf("usec was < 0, stop the drone immediatly\n");
}
//stop the drone
ardrone_tool_set_progressive_cmd(0,0,0,0,0,0,0);
}
ret = C_OK;
} else {
printf("Fail send order\n");
}
return ret;
}
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 send_fast_order( C_RESULT (*function)(void*), void *arg) {
C_RESULT ret = C_FAIL;
int32_t usec = 0;
null_state(&drone_state);
ret = (*function)(arg);
/* Send the command to the drone */
if ( ret == C_OK ){
ardrone_tool_set_progressive_cmd( drone_state.flag,
drone_state.phi,
drone_state.theta,
drone_state.gaz,
drone_state.yaw,
drone_state.psi,
drone_state.psi_accuracy);
ret = C_OK;
} else {
printf("Fail send order\n");
}
return ret;
}
C_RESULT update_control_device(void) {
static int32_t stick1LR = 0, stick1UD = 0 , stick2LR = 0 , stick2UD = 0;
//static int32_t x = 0, y = 0;
//static bool_t refresh_values = FALSE;
ssize_t res;
static struct js_event js_e_buffer[64];
//input_state_t* input_state;
static int center_x1=0;
static int center_y1=0;
static int center_x2=0;
static int center_y2=0;
/* static int32_t theta_trim = 0;
static int32_t phi_trim = 0;
static int32_t yaw_trim = 0;*/
if (default_control->config)
return C_OK;
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_input_get_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
if ((js_e_buffer[idx].number == default_control->commands[ROLL_LEFT].value) && (default_control->commands[ROLL_LEFT].type == BUTTON)) {
//printf("buttons.control_ag : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick1LR = -32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[SPEED_DOWN].value) && (default_control->commands[SPEED_DOWN].type == BUTTON)) {
//printf("buttons.control_ab : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick2UD = 32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[ROLL_RIGHT].value) && (default_control->commands[ROLL_RIGHT].type == BUTTON)) {
//printf("buttons.control_ad : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick1LR = 32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[SPEED_UP].value) && (default_control->commands[SPEED_UP].type == BUTTON)) {
//printf("buttons.control_ah : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick2UD = -32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[YAW_LEFT].value) && (default_control->commands[YAW_LEFT].type == BUTTON)) {
//printf("buttons.control_l1 : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick2LR = -32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[YAW_RIGHT].value) && (default_control->commands[YAW_RIGHT].type == BUTTON)) {
//printf("buttons.control_r1 : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick2LR = 32767 * js_e_buffer[idx].value;
} else if ((js_e_buffer[idx].number == default_control->commands[EMERGENCY].value) && (default_control->commands[EMERGENCY].type == BUTTON)) {
//printf("buttons.control_select : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
ardrone_academy_navdata_emergency();
} else if ((js_e_buffer[idx].number == default_control->commands[START].value) && (default_control->commands[START].type == BUTTON)) {
//printf("buttons.control_start : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
if( js_e_buffer[idx].value ) {
ardrone_academy_navdata_takeoff();
}
} else if ((js_e_buffer[idx].number == default_control->commands[PITCH_FRONT].value) && (default_control->commands[PITCH_FRONT].type == BUTTON)) {
//printf("buttons.pitch_front : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick1UD = -32767;
} else if ((js_e_buffer[idx].number == default_control->commands[PITCH_BACK].value) && (default_control->commands[PITCH_BACK].type == BUTTON)) {
//printf("buttons.pitch_back : %d => %d\n", js_e_buffer[idx].number, js_e_buffer[idx].value);
stick1UD = 32767;
}
} else if(js_e_buffer[idx].type & JS_EVENT_AXIS ) { // Event Axis detected
//refresh_values = TRUE;
int axis_value = (js_e_buffer[idx].number + 1) * (js_e_buffer[idx].value > 0 ? 1 : -1);
if ((axis_value == default_control->commands[PITCH_FRONT].value) && (default_control->commands[PITCH_FRONT].type == AXIS)) {
//printf("axis.pitch_front : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick1UD = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[PITCH_BACK].value) && (default_control->commands[PITCH_BACK].type == AXIS)) {
//printf("axis.pitch_back : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick1UD = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[ROLL_LEFT].value) && (default_control->commands[ROLL_LEFT].type == AXIS)) {
//printf("axis.roll_left : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick1LR = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[ROLL_RIGHT].value) && (default_control->commands[ROLL_RIGHT].type == AXIS)) {
//printf("axis.roll_right : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick1LR = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[SPEED_UP].value) && (default_control->commands[SPEED_UP].type == AXIS)) {
//printf("axis.speed_up : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick2UD = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[SPEED_DOWN].value) && (default_control->commands[SPEED_DOWN].type == AXIS)) {
//printf("axis.speed_down : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick2UD = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[YAW_LEFT].value) && (default_control->commands[YAW_LEFT].type == AXIS)) {
//printf("axis.yaw_left : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick2LR = ( js_e_buffer[idx].value );
} else if ((axis_value == default_control->commands[YAW_RIGHT].value) && (default_control->commands[YAW_RIGHT].type == AXIS)) {
//printf("axis.yaw_right : %d => %d (%d)\n", js_e_buffer[idx].number, js_e_buffer[idx].value, ( js_e_buffer[idx].value + 1 ) >> 15);
stick2LR = ( js_e_buffer[idx].value );
}
} else {
// TODO: default: ERROR (non-supported)
}
}
//if(refresh_values)// Axis values to refresh
{
//printf("roll : %f, pitch : %f, gaz : %f, yaw : %f\n",
// /*roll*/(float)(stick1LR-center_x1)/32767.0f,
// /*pitch*/(float)(stick1UD-center_y1)/32767.0f,
// /*gaz*/-(float)(stick2UD-center_x2)/32767.0f,
// /*yaw*/(float)(stick2LR-center_y2)/32767.0f);
static int minActive = 32768 * (JOYSTICK_DEAD_ZONE_PERCENT) / 100;
static int negMinActive = -32768 * (JOYSTICK_DEAD_ZONE_PERCENT) / 100;
#define SATURATE_JOYSTICK(STICK, POS, NEG) \
do \
{ \
if (POS > STICK && NEG < STICK) \
{ \
STICK = 0; \
} \
} while (0)
int enable = 1;
SATURATE_JOYSTICK (stick1LR, minActive, negMinActive);
SATURATE_JOYSTICK (stick1UD, minActive, negMinActive);
SATURATE_JOYSTICK (stick2LR, minActive, negMinActive);
SATURATE_JOYSTICK (stick2UD, minActive, negMinActive);
if (0 == stick1LR && 0 == stick1UD)
{
enable = 0;
}
ardrone_tool_set_progressive_cmd( enable,
/*roll*/(float)(stick1LR-center_x1)/32767.0f,
/*pitch*/(float)(stick1UD-center_y1)/32767.0f,
/*gaz*/-(float)(stick2UD-center_x2)/32767.0f,
/*yaw*/(float)(stick2LR-center_y2)/32767.0f,
/*psi*/0.0,
/*psi_accuracy*/0.0);
}
return C_OK;
}
void sendControls(void)
{
ardrone_tool_set_progressive_cmd(ctrldata.command_flag, ctrldata.iphone_phi, ctrldata.iphone_theta, ctrldata.gaz, ctrldata.yaw, ctrldata.iphone_psi, ctrldata.iphone_psi_accuracy);
}
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;
}
void set(float phi, float theta, float gaz, float yaw)
{
// psi= 0, psi_accuracy = 0
ardrone_tool_set_progressive_cmd(1, phi, theta, gaz, yaw, 0, 0);
}
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_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 video_transform (video_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
// Realloc frameBuffer if needed
if (in->size != cfg->fbSize)
{
cfg->frameBuffer = vp_os_realloc (cfg->frameBuffer, in->size);
cfg->fbSize = in->size;
}
// Copy last frame to frameBuffer
vp_os_memcpy (cfg->frameBuffer, in->buffers[in->indexBuffer], cfg->fbSize);
// Convert BRG to RGB
int k;
for (k=0; k<cfg->fbSize; k+=3) {
uint8_t tmp = cfg->frameBuffer[k];
cfg->frameBuffer[k] = cfg->frameBuffer[k+2];
cfg->frameBuffer[k+2] = tmp;
}
// Grab image width from data structure
int img_width = cfg->width;
int img_height = cfg->height;
// If belly camera indicated on AR.Drone 1.0, move camera data to beginning
// of buffer
if (!IS_ARDRONE2 && g_is_bellycam) {
int j;
for (j=0; j<QCIF_HEIGHT; ++j) {
memcpy(&cfg->frameBuffer[j*QCIF_WIDTH*3],
&cfg->frameBuffer[j*QVGA_WIDTH*3],
QCIF_WIDTH*3);
}
img_width = QCIF_WIDTH;
img_height = QCIF_HEIGHT;
}
// normalize navdata angles to (-1,+1)
g_navdata.navdata_demo.phi /= 100000;
g_navdata.navdata_demo.theta /= 100000;
g_navdata.navdata_demo.psi /= 180000;
// Init new commands
commands_t commands;
// Call the Python (or Matlab or C) agent's action routine
agent_act(cfg->frameBuffer, img_width, img_height, g_is_bellycam, g_pass_button, &g_navdata, &commands);
g_pass_button = 0; // the agent has been notified of the button press
if (g_autopilot) {
if( commands.phi == 0 && commands.theta == 0 && commands.gaz == 0 && commands.yaw == 0 ) {
ardrone_tool_set_progressive_cmd(0,0,0,0,0,0,0);
} else {
ardrone_tool_set_progressive_cmd(1,commands.phi,commands.theta,commands.gaz,commands.yaw,0,0);
//set(commands.phi, commands.theta, commands.gaz, commands.yaw);
}
if (commands.zap) {
zap();
}
}
// Tell the pipeline that we don't have any output
out->size = 0;
return C_OK;
}
C_RESULT display_stage_transform (display_stage_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
if(count > 1)
{
temp_x = cur_x;
temp_y = cur_y;
}
int i = 0;
const char *cascade_name = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
CvHaarClassifierCascade *cascade = 0;
CvMemStorage *storage = 0;
CvSeq *faces;
cascade = (CvHaarClassifierCascade *) cvLoad (cascade_name, 0, 0, 0);
if(!cascade)
{
printf("Could not load cascade.\n");
return -1;
}
uint32_t width = 0, height = 0;
getPicSizeFromBufferSize (in->size, &width, &height);
//Obtain image from ARDrone and convert it to OpenCV format.
IplImage *img = ipl_image_from_data((uint8_t*)in->buffers[0], 1, 640, 360);
storage = cvCreateMemStorage (0);
cvClearMemStorage (storage);
//DetectFaces
faces = cvHaarDetectObjects (img, cascade, storage, 1.11, 4, 0, cvSize(30, 30), cvSize (0, 0));
for (i = 0; i < (faces ? faces->total : 0); i++)
{
CvRect *r = (CvRect *) cvGetSeqElem (faces, i);
doMosaic(img, r->x, r->y, r->width, r->height, 10);
cur_x = r->x;
cur_y = r->y;
//printf("In the loop.\n");
count++; //increases count when detect faces
}
if(count > 2)
{
printf("prev_x = %d, prev_y = %d, cur_x = %d, cur_y = %d\n", temp_x, temp_y, cur_x, cur_y);
if(cur_x - temp_x > 3)
{
//printf("Move Right.\n");
ardrone_at_set_progress_cmd( 1, 1.0, 0.0, 0.0, 0.0 );
}
if(cur_x - temp_x < -3)
{
//printf("Move Left.\n");
ardrone_at_set_progress_cmd( 1, -1.0, 0.0, 0.0, 0.0 );
}
if(cur_y - temp_y > 3)
{
//printf("Move Down.\n");
ardrone_at_set_progress_cmd( 1, 0.0, 0.0, -1.0, 0.0 );
}
if(cur_y - temp_y < -5)
{
//printf("Move Up.\n");
//ardrone_at_reset_com_watchdog();
ardrone_tool_set_progressive_cmd( 1, 0.0, 0.0, 0.3, 0.0 );
//vp_os_delay(1000);
}
}
//emergency++;p
if(count > 50)
{
printf("Exit\n");
ardrone_tool_set_ui_pad_start(0);
return -1;
}
cvNamedWindow("FaceDetect", CV_WINDOW_AUTOSIZE);
cvShowImage("FaceDetect", img);
cvWaitKey(1);
cvReleaseImage(&img);
return C_OK;
}