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