AC_AttitudeControl: added monitoring of controller error

use RMS P+I+FF output. Thanks to Leonard for the suggestion

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -1,4 +1,4 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include "AC_AttitudeControl.h"
 #include <AP_HAL/AP_HAL.h>
@@ -398,6 +398,7 @@ void AC_AttitudeControl::rate_controller_run()
     _motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
     _motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
     _motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
+    control_monitor_update();
 }
 
 void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -1,4 +1,4 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 #pragma once
 
 /// @file    AC_AttitudeControl.h
@@ -353,6 +353,27 @@ protected:
     const AP_AHRS&      _ahrs;
     const AP_Vehicle::MultiCopter &_aparm;
     AP_Motors&          _motors;
+
+private:
+    /*
+      state of control monitoring
+    */
+    struct {
+        float rms_roll;
+        float rms_pitch;
+        float rms_yaw;
+    } _control_monitor;
+
+    // update state in ControlMonitor
+    void control_monitor_filter_pid(const DataFlash_Class::PID_Info &pid_info, float &rms);
+    void control_monitor_update(void);
+
+public:
+    // log a CTRL message
+    void control_monitor_log(void);
+
+    // return current worst RMS controller filter
+    float control_monitor_rms_output(void) const;
 };
 
 #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude),	\

libraries/AC_AttitudeControl/ControlMonitor.cpp

@@ -0,0 +1,54 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include "AC_AttitudeControl.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+/*
+  code to monitor and report on the rate controllers, allowing for
+  notification of controller oscillation
+ */
+
+
+/*
+  update a RMS estimate of controller state
+ */
+void AC_AttitudeControl::control_monitor_filter_pid(const DataFlash_Class::PID_Info &pid_info, float &rms)
+{
+    float value = sq(pid_info.P + pid_info.D + pid_info.FF);
+    const float filter_constant = 0.99f;
+    // we don't do the sqrt() here as it is quite expensive. That is
+    // done when reporting a result
+    rms = filter_constant * rms + (1.0f - filter_constant) * value;
+}
+
+/*
+  update state in _control_monitor
+ */
+void AC_AttitudeControl::control_monitor_update(void)
+{
+    control_monitor_filter_pid(get_rate_roll_pid().get_pid_info(),  _control_monitor.rms_roll);
+    control_monitor_filter_pid(get_rate_pitch_pid().get_pid_info(), _control_monitor.rms_pitch);
+    control_monitor_filter_pid(get_rate_yaw_pid().get_pid_info(),   _control_monitor.rms_yaw);
+}
+
+/*
+  log a CRTL message
+ */
+void AC_AttitudeControl::control_monitor_log(void)
+{
+    DataFlash_Class::instance()->Log_Write("CTRL", "TimeUS,RMSRoll,RMSPitch,RMSYaw", "Qfff",
+                                           AP_HAL::micros64(),
+                                           sqrtf(_control_monitor.rms_roll),
+                                           sqrtf(_control_monitor.rms_pitch),
+                                           sqrtf(_control_monitor.rms_yaw));
+}
+
+/*
+  return current maximum controller RMS filter value
+ */
+float AC_AttitudeControl::control_monitor_rms_output(void) const
+{
+    float v = MAX(MAX(_control_monitor.rms_roll, _control_monitor.rms_pitch), _control_monitor.rms_yaw);
+    return sqrtf(v);
+}