Exemplos de attitude_error_estimator_set_quat_ref em C++ (Cpp)

Exemplo n.º 1

Arquivo: attitude_controller_p2.c Projeto: SyrianSpock/tp-mobile-robots

void attitude_controller_p2_update(attitude_controller_p2_t* controller)
{
	float errors[3];
	float rates[3];

	// Get attitude command
	switch ( controller->attitude_command->mode )
	{
		case ATTITUDE_COMMAND_MODE_QUATERNION:
			attitude_error_estimator_set_quat_ref(	&controller->attitude_error_estimator,
													controller->attitude_command->quat );
			break;

		case ATTITUDE_COMMAND_MODE_RPY:
			attitude_error_estimator_set_quat_ref_from_rpy( &controller->attitude_error_estimator,
															controller->attitude_command->rpy );
			break;
	}

	// Get local angular errors
	attitude_error_estimator_update( &controller->attitude_error_estimator );
	errors[0] = controller->attitude_error_estimator.rpy_errors[0];
	errors[1] = controller->attitude_error_estimator.rpy_errors[1];
	errors[2] = controller->attitude_error_estimator.rpy_errors[2];

	// Get gyro rates
	rates[0] = controller->ahrs->angular_speed[0];
	rates[1] = controller->ahrs->angular_speed[1];
	rates[2] = controller->ahrs->angular_speed[2];

	// Compute outputs
	controller->torque_command->xyz[0] = controller->p_gain_angle[0] * errors[0] - controller->p_gain_rate[0] * rates[0];
	controller->torque_command->xyz[1] = controller->p_gain_angle[1] * errors[1] - controller->p_gain_rate[1] * rates[1];
	controller->torque_command->xyz[2] = controller->p_gain_angle[2] * errors[2] - controller->p_gain_rate[2] * rates[2];
}

Exemplo n.º 2

Arquivo: attitude_controller.cpp Projeto: lis-epfl/MAVRIC_Library

bool Attitude_controller::update(void)
{
    float now      = time_keeper_get_s();
    dt_s_          = now - last_update_s_;
    last_update_s_ = now;

    // Get attitude command
    attitude_error_estimator_set_quat_ref(&attitude_error_estimator_,
                                          attitude_command_);

    // Get local angular errors
    attitude_error_estimator_update(&attitude_error_estimator_);
    float errors[3];
    errors[ROLL]    = attitude_error_estimator_.rpy_errors[ROLL];
    errors[PITCH]   = attitude_error_estimator_.rpy_errors[PITCH];
    errors[YAW]     = attitude_error_estimator_.rpy_errors[YAW];

    // Update PIDs
    rate_command_.xyz[ROLL]  = pid_controller_update_dt(&pid_[ROLL],  errors[ROLL],  dt_s_);
    rate_command_.xyz[PITCH] = pid_controller_update_dt(&pid_[PITCH], errors[PITCH], dt_s_);
    rate_command_.xyz[YAW]   = pid_controller_update_dt(&pid_[YAW],   errors[YAW],   dt_s_);

    return true;
}