Attitude_controller::Attitude_controller(const args_t& args, const conf_t& config) :
ahrs_(args.ahrs),
attitude_command_(args.attitude_command),
rate_command_(args.rate_command),
dt_s_(0.0f),
last_update_s_(0.0f)
{
// set initial attitude command
attitude_command_.s = 1.0f;
attitude_command_.v[X] = 0.0f;
attitude_command_.v[Y] = 0.0f;
attitude_command_.v[Z] = 0.0f;
// set initial rate command
rate_command_.xyz[X] = 0.0f;
rate_command_.xyz[Y] = 0.0f;
rate_command_.xyz[Z] = 0.0f;
// Init rate gains
pid_controller_init(&pid_[ROLL], &config.pid_config[ROLL]);
pid_controller_init(&pid_[PITCH], &config.pid_config[PITCH]);
pid_controller_init(&pid_[YAW], &config.pid_config[YAW]);
// Init attitude error estimator
attitude_error_estimator_init(&attitude_error_estimator_, &ahrs_);
}
void attitude_controller_p2_init(attitude_controller_p2_t* controller, const attitude_controller_p2_conf_t* config, const attitude_command_t* attitude_command, torque_command_t* torque_command, const ahrs_t* ahrs)
{
// Init dependencies
controller->attitude_command = attitude_command;
controller->torque_command = torque_command;
controller->ahrs = ahrs;
// Init attitude error estimator
attitude_error_estimator_init(&controller->attitude_error_estimator, ahrs);
// Init gains
controller->p_gain_angle[0] = config->p_gain_angle[0];
controller->p_gain_angle[1] = config->p_gain_angle[1];
controller->p_gain_angle[2] = config->p_gain_angle[2];
controller->p_gain_rate[0] = config->p_gain_rate[0];
controller->p_gain_rate[1] = config->p_gain_rate[1];
controller->p_gain_rate[2] = config->p_gain_rate[2];
}
