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