Пример #1
0
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;
}
Пример #2
0
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;
}
Пример #3
0
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;
}
Пример #4
0
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;
}
Пример #5
0
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;
}
Пример #6
0
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;
}
Пример #12
0
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;
}