Browse Source

AC_PosControl: remove alt_max

AC_Avoidance enforces the altitude limit
master
Randy Mackay 8 years ago
parent
commit
ff042528fe
  1. 15
      libraries/AC_AttitudeControl/AC_PosControl.cpp
  2. 6
      libraries/AC_AttitudeControl/AC_PosControl.h

15
libraries/AC_AttitudeControl/AC_PosControl.cpp

@ -52,7 +52,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, @@ -52,7 +52,6 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
_leash_up_z(POSCONTROL_LEASH_LENGTH_MIN),
_roll_target(0.0f),
_pitch_target(0.0f),
_alt_max(0.0f),
_distance_to_target(0.0f),
_accel_target_jerk_limited(0.0f,0.0f),
_accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ)
@ -166,12 +165,6 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d @@ -166,12 +165,6 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
_pos_target.z += climb_rate_cms * dt;
}
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// do not use z-axis desired velocity feed forward
// vel_desired set to desired climb rate for reporting and land-detector
_flags.use_desvel_ff_z = false;
@ -212,14 +205,6 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa @@ -212,14 +205,6 @@ void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, floa
if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
_pos_target.z += _vel_desired.z * dt;
}
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
// decelerate feed forward to zero
_vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
}
}
/// add_takeoff_climb_rate - adjusts alt target up or down using a climb rate in cm/s

6
libraries/AC_AttitudeControl/AC_PosControl.h

@ -76,11 +76,6 @@ public: @@ -76,11 +76,6 @@ public:
/// z position controller
///
/// set_alt_max - sets maximum altitude above the ekf origin in cm
/// only enforced when set_alt_target_from_climb_rate is used
/// set to zero to disable limit
void set_alt_max(float alt) { _alt_max = alt; }
/// set_speed_z - sets maximum climb and descent rates
/// speed_down can be positive or negative but will always be interpreted as a descent speed
/// leash length will be recalculated the next time update_z_controller() is called
@ -417,7 +412,6 @@ private: @@ -417,7 +412,6 @@ private:
Vector3f _accel_error; // desired acceleration in cm/s/s // To-Do: are xy actually required?
Vector3f _accel_feedforward; // feedforward acceleration in cm/s/s
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
float _distance_to_target; // distance to position target - for reporting only
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error

Loading…
Cancel
Save