Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: added get_pid_info() interface

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-23 07:39:38 +10:00
parent 23adf2773c
commit 216a78a9f0
4 changed files with 26 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/APM_Control/AP_PitchController.cpp
View File

@ -141,17 +141,17 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// prevent the integrator from decreasing if surface defln demand is below the lower limit // prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = min(integrator_delta , 0); integrator_delta = min(integrator_delta , 0);
} }
_integrator += integrator_delta; _pid_info.I += integrator_delta;
} }
} else { } else {
_integrator = 0; _pid_info.I = 0;
} }
// Scale the integration limit // Scale the integration limit
float intLimScaled = gains.imax * 0.01f; float intLimScaled = gains.imax * 0.01f;
// Constrain the integrator state // Constrain the integrator state
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled); _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D // No conversion is required for K_D
@ -161,7 +161,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path. // path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed. // This is because acceleration scales with speed^2, but rate scales with speed.
_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler; _pid_info.FF = desired_rate * kp_ff * scaler;
_pid_info.D = rate_error * gains.D * scaler;
_last_out = _pid_info.D + _pid_info.FF;
_pid_info.desired = desired_rate;
if (autotune.running && aspeed > aparm.airspeed_min) { if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values // let autotune have a go at the values
@ -174,7 +177,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
_max_rate_neg.set_and_save_ifchanged(gains.rmax); _max_rate_neg.set_and_save_ifchanged(gains.rmax);
} }
_last_out += _integrator; _last_out += _pid_info.I;
// Convert to centi-degrees and constrain // Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500); return constrain_float(_last_out * 100, -4500, 4500);
@ -290,5 +293,5 @@ int32_t AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool
void AP_PitchController::reset_I() void AP_PitchController::reset_I()
{ {
_integrator = 0; _pid_info.I = 0;
} }

4
libraries/APM_Control/AP_PitchController.h
View File

@ -28,6 +28,8 @@ public:
void autotune_start(void) { autotune.start(); } void autotune_start(void) { autotune.start(); }
void autotune_restore(void) { autotune.stop(); } void autotune_restore(void) { autotune.stop(); }
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
@ -39,7 +41,7 @@ private:
uint32_t _last_t; uint32_t _last_t;
float _last_out; float _last_out;
float _integrator; DataFlash_Class::PID_Info _pid_info;
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed); int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;

16
libraries/APM_Control/AP_RollController.cpp
View File

@ -133,23 +133,27 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// prevent the integrator from decreasing if surface defln demand is below the lower limit // prevent the integrator from decreasing if surface defln demand is below the lower limit
integrator_delta = min(integrator_delta, 0); integrator_delta = min(integrator_delta, 0);
} }
_integrator += integrator_delta; _pid_info.I += integrator_delta;
} }
} else { } else {
_integrator = 0; _pid_info.I = 0;
} }
// Scale the integration limit // Scale the integration limit
float intLimScaled = gains.imax * 0.01f; float intLimScaled = gains.imax * 0.01f;
// Constrain the integrator state // Constrain the integrator state
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled); _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
// Calculate the demanded control surface deflection // Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path. // path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed. // This is because acceleration scales with speed^2, but rate scales with speed.
_last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler; _pid_info.D = rate_error * gains.D * scaler;
_pid_info.FF = desired_rate * kp_ff * scaler;
_pid_info.desired = desired_rate;
_last_out = _pid_info.FF + _pid_info.D;
if (autotune.running && aspeed > aparm.airspeed_min) { if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values // let autotune have a go at the values
@ -159,7 +163,7 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
autotune.update(desired_rate, achieved_rate, _last_out); autotune.update(desired_rate, achieved_rate, _last_out);
} }
_last_out += _integrator; _last_out += _pid_info.I;
// Convert to centi-degrees and constrain // Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500); return constrain_float(_last_out * 100, -4500, 4500);
@ -201,6 +205,6 @@ int32_t AP_RollController::get_servo_out(int32_t angle_err, float scaler, bool d
void AP_RollController::reset_I() void AP_RollController::reset_I()
{ {
_integrator = 0; _pid_info.I = 0;
} }

5
libraries/APM_Control/AP_RollController.h
View File

@ -9,6 +9,7 @@
#include <AP_AutoTune.h> #include <AP_AutoTune.h>
#include <DataFlash.h> #include <DataFlash.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <DataFlash.h>
class AP_RollController { class AP_RollController {
public: public:
@ -28,6 +29,8 @@ public:
void autotune_start(void) { autotune.start(); } void autotune_start(void) { autotune.start(); }
void autotune_restore(void) { autotune.stop(); } void autotune_restore(void) { autotune.stop(); }
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
@ -37,7 +40,7 @@ private:
uint32_t _last_t; uint32_t _last_t;
float _last_out; float _last_out;
float _integrator; DataFlash_Class::PID_Info _pid_info;
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator); int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);