AC_AttControl: add get_throttle_in accessor

Used for logging only
This commit is contained in:
Leonard Hall 2016-01-04 17:14:01 +09:00 committed by Randy Mackay
parent c0f209fa42
commit 2822b93cd4
2 changed files with 8 additions and 0 deletions

View File

@ -588,6 +588,7 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
void AC_AttitudeControl::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_stabilizing(true);
_motors.set_throttle_filter_cutoff(filter_cutoff);
@ -602,6 +603,7 @@ void AC_AttitudeControl::set_throttle_out(float throttle_in, bool apply_angle_bo
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
if (reset_attitude_control) {
relax_bf_rate_controller();

View File

@ -196,6 +196,9 @@ public:
// Set output throttle and disable stabilization
void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff);
// get throttle passed into attitude controller (i.e. throttle_in provided to set_throttle_out)
float get_throttle_in() const { return _throttle_in; }
// Return throttle increase applied for tilt compensation
float angle_boost() const { return _angle_boost; }
@ -327,6 +330,9 @@ protected:
// velocity controller, in radians per second. Formerly _rate_bf_target.
Vector3f _ang_vel_target_rads;
// throttle provided as input to attitude controller. This does not include angle boost.
// Used only for logging.
float _throttle_in = 0.0f;
// This represents the throttle increase applied for tilt compensation.
// Used only for logging.