Browse Source

AC_AttitudeControl: added get_rpy_srate()

and remove dmod binding, as slew rate turned out to be more useful
apm_2208
Andrew Tridgell 3 years ago
parent
commit
99a6cb69f8
  1. 12
      libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
  2. 6
      libraries/AC_AttitudeControl/AC_AttitudeControl.h

12
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@ -1133,12 +1133,12 @@ bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix, @@ -1133,12 +1133,12 @@ bool AC_AttitudeControl::pre_arm_checks(const char *param_prefix,
}
/*
get the PDmod value for roll, pitch and yaw, for oscillation
detection in lua scripts
get the slew rate for roll, pitch and yaw, for oscillation
detection in lua scripts
*/
void AC_AttitudeControl::get_rpy_PDmod(float &roll_dmod, float &pitch_dmod, float &yaw_dmod)
void AC_AttitudeControl::get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate)
{
roll_dmod = get_rate_roll_pid().get_pid_info().Dmod;
pitch_dmod = get_rate_pitch_pid().get_pid_info().Dmod;
yaw_dmod = get_rate_yaw_pid().get_pid_info().Dmod;
roll_srate = get_rate_roll_pid().get_pid_info().slew_rate;
pitch_srate = get_rate_pitch_pid().get_pid_info().slew_rate;
yaw_srate = get_rate_yaw_pid().get_pid_info().slew_rate;
}

6
libraries/AC_AttitudeControl/AC_AttitudeControl.h

@ -368,9 +368,9 @@ public: @@ -368,9 +368,9 @@ public:
// enable inverted flight on backends that support it
virtual void set_inverted_flight(bool inverted) {}
// get the PDmod value for roll, pitch and yaw, for oscillation detection in lua scripts
void get_rpy_PDmod(float &roll_dmod, float &pitch_dmod, float &yaw_dmod);
// get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];

Loading…
Cancel
Save