diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index df92233bab..e1bd1c7d71 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -372,8 +372,10 @@ public: // log a CTRL message void control_monitor_log(void); - // return current worst RMS controller filter - float control_monitor_rms_output(void) const; + // return current RMS controller filter for each axis + float control_monitor_rms_output_roll(void) const; + float control_monitor_rms_output_pitch(void) const; + float control_monitor_rms_output_yaw(void) const; }; #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \ diff --git a/libraries/AC_AttitudeControl/ControlMonitor.cpp b/libraries/AC_AttitudeControl/ControlMonitor.cpp index 48ab6b2795..098815e7dd 100644 --- a/libraries/AC_AttitudeControl/ControlMonitor.cpp +++ b/libraries/AC_AttitudeControl/ControlMonitor.cpp @@ -45,10 +45,25 @@ void AC_AttitudeControl::control_monitor_log(void) } /* - return current maximum controller RMS filter value + return current controller RMS filter value for roll */ -float AC_AttitudeControl::control_monitor_rms_output(void) const +float AC_AttitudeControl::control_monitor_rms_output_roll(void) const { - float v = MAX(MAX(_control_monitor.rms_roll, _control_monitor.rms_pitch), _control_monitor.rms_yaw); - return sqrtf(v); + return sqrtf(_control_monitor.rms_roll); +} + +/* + return current controller RMS filter value for pitch + */ +float AC_AttitudeControl::control_monitor_rms_output_pitch(void) const +{ + return sqrtf(_control_monitor.rms_pitch); +} + +/* + return current controller RMS filter value for yaw + */ +float AC_AttitudeControl::control_monitor_rms_output_yaw(void) const +{ + return sqrtf(_control_monitor.rms_yaw); }