AC_AttitudeControl: : Add units to the accessors.

This commit is contained in:
Leonard Hall 2021-09-06 19:15:31 +09:30 committed by Randy Mackay
parent 7852e9d1b0
commit ad278779e3
4 changed files with 16 additions and 16 deletions

View File

@ -1007,7 +1007,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
}
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
float AC_AttitudeControl::get_althold_lean_angle_max() const
float AC_AttitudeControl::get_althold_lean_angle_max_cd() const
{
// convert to centi-degrees for public interface
return MAX(ToDeg(_althold_lean_angle_max), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN) * 100.0f;

View File

@ -76,34 +76,34 @@ public:
virtual AC_PID& get_rate_yaw_pid() = 0;
// get the roll acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_roll_max() const { return _accel_roll_max; }
float get_accel_roll_max_cdss() const { return _accel_roll_max; }
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(float accel_roll_max) { _accel_roll_max = accel_roll_max; }
void set_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max = accel_roll_max; }
// Sets and saves the roll acceleration limit in centidegrees/s/s
void save_accel_roll_max(float accel_roll_max) { _accel_roll_max.set_and_save(accel_roll_max); }
void save_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max.set_and_save(accel_roll_max); }
// get the pitch acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_pitch_max() const { return _accel_pitch_max; }
float get_accel_pitch_max_cdss() const { return _accel_pitch_max; }
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(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; }
void set_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; }
// Sets and saves the pitch acceleration limit in centidegrees/s/s
void save_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max.set_and_save(accel_pitch_max); }
void save_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max.set_and_save(accel_pitch_max); }
// get the yaw acceleration limit in centidegrees/s/s or radians/s/s
float get_accel_yaw_max() const { return _accel_yaw_max; }
float get_accel_yaw_max_cdss() const { return _accel_yaw_max; }
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(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; }
void set_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; }
// Sets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); }
void save_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); }
// get the roll angular velocity limit in radians/s
float get_ang_vel_roll_max_rads() const { return _ang_vel_roll_max; }
@ -279,13 +279,13 @@ public:
float angle_boost() const { return _angle_boost; }
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
virtual float get_althold_lean_angle_max() const;
virtual float get_althold_lean_angle_max_cd() const;
// Return configured tilt angle limit in centidegrees
float lean_angle_max() const { return _aparm.angle_max; }
float lean_angle_max_cd() const { return _aparm.angle_max; }
// Return tilt angle in degrees
float lean_angle() const { return degrees(_thrust_angle); }
float lean_angle_deg() const { return degrees(_thrust_angle); }
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain

View File

@ -62,7 +62,7 @@ public:
void rate_controller_run() override;
// limiting lean angle based on throttle makes no sense for 6DoF, always allow 90 deg, return in centi-degrees
float get_althold_lean_angle_max() const override { return 9000.0f; }
float get_althold_lean_angle_max_cd() const override { return 9000.0f; }
// set the attitude that will be used in 6DoF flight
void set_offset_roll_pitch(float roll_deg, float pitch_deg) {

View File

@ -654,7 +654,7 @@ void AC_PosControl::update_xy_controller()
// limit acceleration using maximum lean angles
_limit_vector.xy().zero();
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
if (_accel_target.limit_length_xy(accel_max)) {
_limit_vector.xy() = _accel_target.xy();
@ -1033,7 +1033,7 @@ void AC_PosControl::update_z_controller()
float AC_PosControl::get_lean_angle_max_cd() const
{
if (is_zero(_lean_angle_max)) {
return _attitude_control.lean_angle_max();
return _attitude_control.lean_angle_max_cd();
}
return _lean_angle_max * 100.0f;
}