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