Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: return roll, pitch and yaw controller error separately 

as discussed with Leonard
This commit is contained in:
Andrew Tridgell 2016-06-01 17:18:58 +10:00
parent d56e2b6a39
commit b4bdfa2451
2 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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
View File

@ -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);
}