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