Skip to content

Commit

Permalink
AC_AttitudeControl: change to const Vector3f &ang_vel_body
Browse files Browse the repository at this point in the history
  • Loading branch information
lthall committed Jun 20, 2024
1 parent 5b707f6 commit c908522
Show file tree
Hide file tree
Showing 4 changed files with 5 additions and 5 deletions.
2 changes: 1 addition & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -228,7 +228,7 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()

// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step by the integral of the body frame angular velocity
void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body)
void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, const Vector3f &ang_vel_body)
{
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
Expand Down
2 changes: 1 addition & 1 deletion libraries/AC_AttitudeControl/AC_AttitudeControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ class AC_AttitudeControl {

// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity
virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body);
virtual void input_quaternion(Quaternion& attitude_desired_quat, const Vector3f &ang_vel_body);

// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol

// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) {
void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, const Vector3f &ang_vel_body) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
#endif
Expand Down
4 changes: 2 additions & 2 deletions libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ class AC_AttitudeControl_Multi_6DoF : public AC_AttitudeControl_Multi {
}

// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) override;
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity
void input_quaternion(Quaternion& attitude_desired_quat, const Vector3f &ang_vel_body) override;
/*
override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles
*/
Expand Down

0 comments on commit c908522

Please sign in to comment.