3 changed files with 78 additions and 2 deletions
@ -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); |
||||||
|
} |
Loading…
Reference in new issue