aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: Add PID logging to yaw controller

Browse Source
This commit is contained in:
Michael du Breuil 2015-06-21 20:28:08 -07:00 committed by Andrew Tridgell
parent 1a121f543b
commit 6cd81ae1fb
2 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/APM_Control/AP_YawController.cpp
View File

@ -166,7 +166,9 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
// Save to last value before application of limiter so that integrator limiting
// can detect exceedance next frame
// Scale using inverse dynamic pressure (1/V^2)
_last_out = _K_D * (_integrator - rate_hp_out) * scaler * scaler;
_pid_info.I = _K_D * _integrator * scaler * scaler;
_pid_info.D = _K_D * (-rate_hp_out) * scaler * scaler;
_last_out = _pid_info.I + _pid_info.D;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);

8
libraries/APM_Control/AP_YawController.h
View File

@ -6,6 +6,7 @@
#include <AP_AHRS.h>
#include <AP_Common.h>
#include <AP_Vehicle.h>
#include <DataFlash.h>
#include <math.h>
class AP_YawController {
@ -15,12 +16,17 @@ public:
_ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
_pid_info.desired = 0;
_pid_info.FF = 0;
_pid_info.P = 0;
}
int32_t get_servo_out(float scaler, bool disable_integrator);
void reset_I();
const DataFlash_Class::PID_Info& get_pid_info(void) const {return _pid_info; }
static const struct AP_Param::GroupInfo var_info[];
private:
@ -39,6 +45,8 @@ private:
float _integrator;
DataFlash_Class::PID_Info _pid_info;
AP_AHRS &_ahrs;
};