diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 2c4812936a..bbb065392f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -240,8 +240,8 @@ void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat) _attitude_target_quat = attitude_desired_quat; // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); } // Call quaternion attitude controller @@ -264,7 +264,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss())); + const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration @@ -291,8 +291,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); } // Call quaternion attitude controller @@ -316,7 +316,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float e if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss())); + const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration @@ -352,8 +352,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw_euler_rate_yaw(float e _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); } // Call quaternion attitude controller @@ -376,7 +376,7 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss())); + const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration @@ -410,8 +410,8 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); } // Call quaternion attitude controller @@ -431,7 +431,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c if (_rate_bf_ff_enabled) { // translate the roll pitch and yaw acceleration limits to the euler axis - const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss())); + const Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); // When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing // the output rate towards the input rate. @@ -449,8 +449,8 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c _attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + euler_yaw_rate * _dt); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); // Compute quaternion target attitude _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); @@ -489,8 +489,8 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl _attitude_target_quat.normalize(); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); } // Call quaternion attitude controller @@ -592,8 +592,8 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); // Set rate feedforward requests to zero - _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f); - _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f); + _attitude_target_euler_rate.zero(); + _attitude_target_ang_vel.zero(); // Call quaternion attitude controller attitude_controller_run_quat();