C++ (Cpp) servos_set_value_failsafe 예제들

예제 #1

파일: pwm_servos.c 프로젝트: gburri/MAVRIC_Library

void pwm_servos_calibrate_esc(const servos_t* servos)
{
	int16_t i;
	
	servos_t speed_controller = *servos;
	
	for(i = 0;i < 4; ++i)
	{
		speed_controller.servo[i].value = servos->servo[i].max;
	}
	
	pwm_servos_write_to_hardware(&speed_controller);
	delay_ms(2000);
	servos_set_value_failsafe(&speed_controller);
	pwm_servos_write_to_hardware(&speed_controller);
}

예제 #2

파일: tasks.c 프로젝트: SyrianSpock/tp-mobile-robots

task_return_t tasks_run_stabilisation(void* arg) 
{
	tasks_run_imu_update(0);
	
	mav_mode_t mode = central_data->state.mav_mode;

	if( mode.ARMED == ARMED_ON )
	{
		if ( mode.AUTO == AUTO_ON )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			// if no waypoints are set, we do position hold therefore the yaw mode is absolute
			if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			{
				central_data->controls.yaw_mode = YAW_COORDINATED;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}
		}
		else if ( mode.GUIDED == GUIDED_ON )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			if ((central_data->state.mav_state == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == FLY_TO_HOME_WP))
			{
				central_data->controls.yaw_mode = YAW_COORDINATED;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}
		}
		else if ( mode.STABILISE == STABILISE_ON )
		{
			if (central_data->state.remote_active == 1)
			{
				remote_get_velocity_vector_from_remote(&central_data->remote, &central_data->controls);
			}
			else
			{
				joystick_parsing_get_velocity_vector_from_joystick(&central_data->joystick_parsing,&central_data->controls);
			}
			
			
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;
			central_data->controls.yaw_mode = YAW_RELATIVE;
			
			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			}		
		}
		else if ( mode.MANUAL == MANUAL_ON )
		{
			if (central_data->state.remote_active == 1)
			{
				remote_get_command_from_remote(&central_data->remote, &central_data->controls);
			}
			else
			{
				joystick_parsing_get_attitude_command_from_joystick(&central_data->joystick_parsing,&central_data->controls);
			}
			
			central_data->controls.control_mode = ATTITUDE_COMMAND_MODE;
			central_data->controls.yaw_mode=YAW_RELATIVE;
		
			stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);		
		}
		else
		{
			servos_set_value_failsafe( &central_data->servos );
		}
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

		
	// !!! -- for safety, this should remain the only place where values are written to the servo outputs! --- !!!
	if ( mode.HIL == HIL_OFF )
	{
		pwm_servos_write_to_hardware( &central_data->servos );
	}
	
	return TASK_RUN_SUCCESS;
}

예제 #3

파일: tasks.c 프로젝트: SyrianSpock/tp-mobile-robots

task_return_t tasks_run_stabilisation_quaternion(void* arg)
{
	tasks_run_imu_update(0);
	
	mav_mode_t mode = central_data->state.mav_mode;

	if( mode.ARMED == ARMED_OFF )
	{
		// Set command to current heading
		central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
		servos_set_value_failsafe( &central_data->servos );
	}
	else if( mode.AUTO == AUTO_ON )
	{
		vector_field_waypoint_update( &central_data->vector_field_waypoint );
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mode.MANUAL == MANUAL_ON && mode.GUIDED == GUIDED_ON )
	{
		remote_get_velocity_vector_from_remote(&central_data->remote, &central_data->controls);

		central_data->command.velocity.xyz[X] = central_data->controls.tvel[X];
		central_data->command.velocity.xyz[Y] = central_data->controls.tvel[Y];
		central_data->command.velocity.xyz[Z] = central_data->controls.tvel[Z];
		central_data->command.velocity.mode   = VELOCITY_COMMAND_MODE_GLOBAL;

		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mode.MANUAL == MANUAL_ON && mode.STABILISE == STABILISE_ON )
	{	
		remote_get_command_from_remote(&central_data->remote, &central_data->controls);
		
		central_data->command.attitude.rpy[0] 	= central_data->controls.rpy[0];
		central_data->command.attitude.rpy[1] 	= central_data->controls.rpy[1];
		central_data->command.attitude.rpy[2] 	+= 0.02 * central_data->controls.rpy[2];
		central_data->command.attitude.mode 	= ATTITUDE_COMMAND_MODE_RPY;
		central_data->command.thrust.thrust 	= central_data->controls.thrust;
	
		attitude_controller_update( &central_data->attitude_controller );

		// altitude
		// if( mode.GUIDED == GUIDED_ON )
		// {
		// 	float delta_h = 1.0 - central_data->sonar_i2cxl.data.current_distance;
		// 	float k_h = 0.2f;
		// 	central_data->command.thrust.thrust += k_h * delta_h;
		// }
			
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

	// !!! -- for safety, this should remain the only place where values are written to the servo outputs! --- !!!
	if ( mode.ARMED == ARMED_ON && mode.HIL == HIL_OFF )
	{
		pwm_servos_write_to_hardware( &central_data->servos );
	}
	
	return TASK_RUN_SUCCESS;
}