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