diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index b6f3fe607d..9143e85cbe 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -1025,9 +1025,9 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits) _accel_yaw_max.load(); } } else { - _accel_roll_max = 0.0f; - _accel_pitch_max = 0.0f; - _accel_yaw_max = 0.0f; + _accel_roll_max.set(0.0f); + _accel_pitch_max.set(0.0f); + _accel_yaw_max.set(0.0f); } } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 57becd16cf..a30974d545 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -87,7 +87,7 @@ public: float get_accel_roll_max_radss() const { return radians(_accel_roll_max * 0.01f); } // Sets the roll acceleration limit in centidegrees/s/s - void set_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max = accel_roll_max; } + void set_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max.set(accel_roll_max); } // Sets and saves the roll acceleration limit in centidegrees/s/s void save_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max.set_and_save(accel_roll_max); } @@ -97,7 +97,7 @@ public: float get_accel_pitch_max_radss() const { return radians(_accel_pitch_max * 0.01f); } // Sets the pitch acceleration limit in centidegrees/s/s - void set_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; } + void set_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max.set(accel_pitch_max); } // Sets and saves the pitch acceleration limit in centidegrees/s/s void save_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max.set_and_save(accel_pitch_max); } @@ -107,7 +107,7 @@ public: float get_accel_yaw_max_radss() const { return radians(_accel_yaw_max * 0.01f); } // Sets the yaw acceleration limit in centidegrees/s/s - void set_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; } + void set_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max.set(accel_yaw_max); } // Sets and saves the yaw acceleration limit in centidegrees/s/s void save_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); } @@ -128,7 +128,7 @@ public: float get_input_tc() const { return _input_tc; } // set the rate control input smoothing time constant - void set_input_tc(float input_tc) { _input_tc = constrain_float(input_tc, 0.0f, 1.0f); } + void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); } // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); @@ -269,7 +269,7 @@ public: Vector3f rate_bf_targets() const { return _ang_vel_body + _sysid_ang_vel_body; } // Enable or disable body-frame feed forward - void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; } + void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled.set(enable_or_disable); } // Enable or disable body-frame feed forward and save void bf_feedforward_save(bool enable_or_disable) { _rate_bf_ff_enabled.set_and_save(enable_or_disable); } diff --git a/libraries/AC_AttitudeControl/AC_CommandModel.h b/libraries/AC_AttitudeControl/AC_CommandModel.h index 89bb116001..dd99031474 100644 --- a/libraries/AC_AttitudeControl/AC_CommandModel.h +++ b/libraries/AC_AttitudeControl/AC_CommandModel.h @@ -19,7 +19,7 @@ public: float get_expo() const { return expo; } // Set the max rate - void set_rate(float input) { rate = input; } + void set_rate(float input) { rate.set(input); } // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[];