diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 731429a7f9..0de69719f1 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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 integrator_delta = min(integrator_delta , 0); } - _integrator += integrator_delta; + _pid_info.I += integrator_delta; } } else { - _integrator = 0; + _pid_info.I = 0; } // Scale the integration limit float intLimScaled = gains.imax * 0.01f; // 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 // 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 // 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. - _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) { // 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); } - _last_out += _integrator; + _last_out += _pid_info.I; // Convert to centi-degrees and constrain 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() { - _integrator = 0; + _pid_info.I = 0; } diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index f76d31a0b8..99784dab1f 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -28,6 +28,8 @@ public: void autotune_start(void) { autotune.start(); } 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[]; private: @@ -39,7 +41,7 @@ private: uint32_t _last_t; 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); float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index df34a7a580..9993aefbb2 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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 integrator_delta = min(integrator_delta, 0); } - _integrator += integrator_delta; + _pid_info.I += integrator_delta; } } else { - _integrator = 0; + _pid_info.I = 0; } // Scale the integration limit float intLimScaled = gains.imax * 0.01f; // 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 // 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. // 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) { // 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); } - _last_out += _integrator; + _last_out += _pid_info.I; // Convert to centi-degrees and constrain 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() { - _integrator = 0; + _pid_info.I = 0; } diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 610bc21811..8714e027d6 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -9,6 +9,7 @@ #include #include #include +#include class AP_RollController { public: @@ -28,6 +29,8 @@ public: void autotune_start(void) { autotune.start(); } 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[]; private: @@ -37,7 +40,7 @@ private: uint32_t _last_t; float _last_out; - float _integrator; + DataFlash_Class::PID_Info _pid_info; int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);