Browse Source

AC_AttitudeControl: return roll, pitch and yaw controller error separately

as discussed with Leonard
mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
b4bdfa2451
  1. 6
      libraries/AC_AttitudeControl/AC_AttitudeControl.h
  2. 23
      libraries/AC_AttitudeControl/ControlMonitor.cpp

6
libraries/AC_AttitudeControl/AC_AttitudeControl.h

@ -372,8 +372,10 @@ public: @@ -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), \

23
libraries/AC_AttitudeControl/ControlMonitor.cpp

@ -45,10 +45,25 @@ void AC_AttitudeControl::control_monitor_log(void) @@ -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);
}

Loading…
Cancel
Save