void stabilisation_run(stabiliser_t *stabiliser, float dt, float errors[])
{
for (int32_t i = 0; i < 3; i++)
{
stabiliser->output.rpy[i] = pid_controller_update_dt(&(stabiliser->rpy_controller[i]), errors[i], dt);
}
stabiliser->output.thrust = pid_controller_update_dt(&(stabiliser->thrust_controller), errors[3], dt);
}
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;
}
void velocity_controller_copter_update(velocity_controller_copter_t* controller)
{
float velocity_command_global[3];
float errors[3];
float thrust_vector[3];
float thrust_norm;
float thrust_dir[3];
// Get the command velocity in global frame
switch( controller->velocity_command->mode )
{
case VELOCITY_COMMAND_MODE_LOCAL:
get_velocity_command_from_local_to_global( controller,
velocity_command_global );
break;
case VELOCITY_COMMAND_MODE_SEMI_LOCAL:
get_velocity_command_from_semilocal_to_global( controller,
velocity_command_global );
break;
case VELOCITY_COMMAND_MODE_GLOBAL:
velocity_command_global[X] = controller->velocity_command->xyz[X];
velocity_command_global[Y] = controller->velocity_command->xyz[Y];
velocity_command_global[Z] = controller->velocity_command->xyz[Z];
break;
default:
velocity_command_global[X] = 0.0f;
velocity_command_global[Y] = 0.0f;
velocity_command_global[Z] = 0.0f;
break;
}
// Compute errors
errors[X] = velocity_command_global[X] - controller->pos_est->vel[X];
errors[Y] = velocity_command_global[Y] - controller->pos_est->vel[Y];
errors[Z] = velocity_command_global[Z] - controller->pos_est->vel[Z]; // WARNING: it was multiplied by (-1) in stabilisation_copter.c
// Update PID
thrust_vector[X] = pid_controller_update_dt( &controller->pid[X], errors[X], controller->ahrs->dt ); // should be multiplied by mass
thrust_vector[Y] = pid_controller_update_dt( &controller->pid[Y], errors[Y], controller->ahrs->dt ); // should be multiplied by mass
// thrust_vector[Z] = - GRAVITY + pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt ); // should be multiplied by mass
thrust_vector[Z] = pid_controller_update_dt( &controller->pid[Z], errors[Z], controller->ahrs->dt ); // should be multiplied by mass
// Compute the norm of the thrust that should be applied
thrust_norm = vectors_norm(thrust_vector);
// Compute the direction in which thrust should be apply
thrust_dir[X] = thrust_vector[X] / thrust_norm;
thrust_dir[Y] = thrust_vector[Y] / thrust_norm;
thrust_dir[Z] = thrust_vector[Z] / thrust_norm;
// Map thrust dir to attitude
controller->attitude_command->mode = ATTITUDE_COMMAND_MODE_RPY;
controller->attitude_command->rpy[ROLL] = maths_clip(thrust_vector[Y], 1);
controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
controller->attitude_command->rpy[YAW] = 0.0f;
// Map PID output to thrust
// float max_thrust = 30.0f; // 10 Newton max thrust
// controller->thrust_command->thrust = maths_clip(thrust_norm/max_thrust, 1.0f) * 2.0f - 1;
// controller->thrust_command->thrust = maths_clip(thrust_norm, 1.0f) * 2.0f - 1;
// controller->thrust_command->thrust = - 1.0 + 2 * thrust_norm/max_thrust;
// // Map PID output to attitude
// controller->attitude_command->mode = ATTITUDE_COMMAND_MODE_RPY;
// controller->attitude_command->rpy[ROLL] = maths_clip(thrust_vector[Y], 1);
// controller->attitude_command->rpy[PITCH] = - maths_clip(thrust_vector[X], 1);
// controller->attitude_command->rpy[YAW] = 0.0f;
// Map PID output to thrust
controller->thrust_command->thrust = controller->thrust_hover_point - thrust_vector[Z];
}
