C++ (Cpp) attitude_controller_update示例

示例#1

文件： 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;
} 

示例#2

文件： tasks.cpp 项目： froj/MAVRIC_Library

bool tasks_run_stabilisation_quaternion(Central_data* central_data)
{
    tasks_run_imu_update(central_data);

    const State& state = central_data->state;

    if (!state.is_armed())
    {
        // Set command to current heading
        central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
        central_data->servo_0.failsafe();
        central_data->servo_1.failsafe();
        central_data->servo_2.failsafe();
        central_data->servo_3.failsafe();
    }
    else if (state.is_auto())
    {
        // 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 (state.is_manual() && state.is_guided())
    {
        // manual_control_get_velocity_command(&central_data->manual_control, &central_data->command.velocity, 1.0f);
        // velocity_controller_copter_update(&central_data->velocity_controller);

        // get attitude command from remote
        central_data->manual_control.get_attitude_command(0.02f, &central_data->command.attitude, 1.0f);

        // Hardcode altitude command
        central_data->command.position.xyz[0] = 0.0f;
        central_data->command.position.xyz[1] = 0.0f;
        central_data->command.position.xyz[2] = -5.0f;
        central_data->command.position.mode   = POSITION_COMMAND_MODE_LOCAL;

        // Do control
        central_data->altitude_controller_.update();
        attitude_controller_update(&central_data->attitude_controller);

        servos_mix_quadcopter_diag_update(&central_data->servo_mix);
    }
    else if (state.is_manual() && state.is_stabilize())
    {
        // get command from remote
        central_data->manual_control.get_attitude_command(0.02f, &central_data->command.attitude, 1.0f);
        central_data->manual_control.get_thrust_command(&central_data->command.thrust);

        // Do control
        attitude_controller_update(&central_data->attitude_controller);

        servos_mix_quadcopter_diag_update(&central_data->servo_mix);
    }
    else
    {
        central_data->servo_0.failsafe();
        central_data->servo_1.failsafe();
        central_data->servo_2.failsafe();
        central_data->servo_3.failsafe();
    }

    return true;
}