Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: added monitoring of controller error

Browse Source 
use RMS P+I+FF output. Thanks to Leonard for the suggestion
This commit is contained in:
Andrew Tridgell 2016-05-27 14:13:02 +10:00
parent fdac1d26cc
commit 6330060e49
3 changed files with 78 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

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

23
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -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), \

54
libraries/AC_AttitudeControl/ControlMonitor.cpp Normal file
View File

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