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 velocity_controller_copter_init(velocity_controller_copter_t* controller, const velocity_controller_copter_conf_t* config, const ahrs_t* ahrs, const position_estimation_t* pos_est, const velocity_command_t* velocity_command, attitude_command_t* attitude_command, thrust_command_t* thrust_command) { // Init dependencies controller->velocity_command = velocity_command; controller->attitude_command = attitude_command; controller->thrust_command = thrust_command; controller->ahrs = ahrs; controller->pos_est = pos_est; // Init hover point controller->thrust_hover_point = config->thrust_hover_point; // Init PID gains pid_controller_init(&controller->pid[X], &config->pid_config[X]); pid_controller_init(&controller->pid[Y], &config->pid_config[Y]); pid_controller_init(&controller->pid[Z], &config->pid_config[Z]); }
Altitude_controller::Altitude_controller(const position_command_t& position_command, const altitude_t& altitude, thrust_command_t& thrust_command, altitude_controller_conf_t config): position_command_(position_command), altitude_(altitude), thrust_command_(thrust_command) { hover_point_ = config.hover_point; pid_controller_init(&pid_, &config.pid_config); }