Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: log rms P and D separately

Browse Source 
as discussed with Leonard
This commit is contained in:
Andrew Tridgell 2016-06-09 16:00:57 +10:00
parent 1a08f4b13f
commit 1c463e3f3b
2 changed files with 25 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -359,13 +359,15 @@ private:
state of control monitoring
*/
struct {
float rms_roll;
float rms_pitch;
float rms_roll_P;
float rms_roll_D;
float rms_pitch_P;
float rms_pitch_D;
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_filter_pid(float value, float &rms_P);
void control_monitor_update(void);
public:

31
libraries/AC_AttitudeControl/ControlMonitor.cpp
View File

@ -13,13 +13,12 @@
/*
update a RMS estimate of controller state
*/
void AC_AttitudeControl::control_monitor_filter_pid(const DataFlash_Class::PID_Info &pid_info, float &rms)
void AC_AttitudeControl::control_monitor_filter_pid(float value, 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;
rms = filter_constant * rms + (1.0f - filter_constant) * sq(value);
}
/*
@ -27,9 +26,16 @@ void AC_AttitudeControl::control_monitor_filter_pid(const DataFlash_Class::PID_I
*/
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);
const DataFlash_Class::PID_Info &iroll = get_rate_roll_pid().get_pid_info();
control_monitor_filter_pid(iroll.P + iroll.FF, _control_monitor.rms_roll_P);
control_monitor_filter_pid(iroll.D, _control_monitor.rms_roll_D);
const DataFlash_Class::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info();
control_monitor_filter_pid(ipitch.P + iroll.FF, _control_monitor.rms_pitch_P);
control_monitor_filter_pid(ipitch.D, _control_monitor.rms_pitch_D);
const DataFlash_Class::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info();
control_monitor_filter_pid(iyaw.P + iyaw.D + iyaw.FF, _control_monitor.rms_yaw);
}
/*
@ -37,11 +43,14 @@ void AC_AttitudeControl::control_monitor_update(void)
*/
void AC_AttitudeControl::control_monitor_log(void)
{
DataFlash_Class::instance()->Log_Write("CTRL", "TimeUS,RMSRoll,RMSPitch,RMSYaw", "Qfff",
DataFlash_Class::instance()->Log_Write("CTRL", "TimeUS,RMSRollP,RMSRollD,RMSPitchP,RMSPitchD,RMSYaw", "Qfffff",
AP_HAL::micros64(),
(double)sqrtf(_control_monitor.rms_roll),
(double)sqrtf(_control_monitor.rms_pitch),
(double)sqrtf(_control_monitor.rms_roll_P),
(double)sqrtf(_control_monitor.rms_roll_D),
(double)sqrtf(_control_monitor.rms_pitch_P),
(double)sqrtf(_control_monitor.rms_pitch_D),
(double)sqrtf(_control_monitor.rms_yaw));
}
/*
@ -49,7 +58,7 @@ void AC_AttitudeControl::control_monitor_log(void)
*/
float AC_AttitudeControl::control_monitor_rms_output_roll(void) const
{
return sqrtf(_control_monitor.rms_roll);
return sqrtf(_control_monitor.rms_roll_P + _control_monitor.rms_roll_D);
}
/*
@ -57,7 +66,7 @@ float AC_AttitudeControl::control_monitor_rms_output_roll(void) const
*/
float AC_AttitudeControl::control_monitor_rms_output_pitch(void) const
{
return sqrtf(_control_monitor.rms_pitch);
return sqrtf(_control_monitor.rms_pitch_P + _control_monitor.rms_pitch_D);
}
/*