Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl_Sub: Add new relax_alt_hold_controllers

Browse Source 
When changing from manual to alt hold controller it's necessary to hold the I term
since this will help to fix the altitude with vehicles that are not neutral buoyancy

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2019-03-18 14:11:25 -03:00 committed by Jacob Walser
parent d2052cdd35
commit 6152f31c1e
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp
View File

@ -88,3 +88,17 @@ void AC_PosControl_Sub::set_alt_target_from_climb_rate_ff(float climb_rate_cms,
_vel_desired.z = constrain_float(0.0f, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);
}
}
/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl_Sub::relax_alt_hold_controllers()
{
_pos_target.z = _inav.get_altitude();
_vel_desired.z = 0.0f;
_flags.use_desvel_ff_z = false;
_vel_target.z = _inav.get_velocity_z();
_vel_last.z = _inav.get_velocity_z();
_accel_desired.z = 0.0f;
_accel_last_z_cms = 0.0f;
_accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
_flags.reset_accel_to_throttle = true;
}

9
libraries/AC_AttitudeControl/AC_PosControl_Sub.h
View File

@ -31,6 +31,15 @@ public:
/// set force_descend to true during landing to allow target to move low enough to slow the motors
void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend) override;
/// relax_alt_hold_controllers - set all desired and targets to measured
void relax_alt_hold_controllers(float throttle_setting) {
AC_PosControl::relax_alt_hold_controllers(throttle_setting);
}
/// relax_alt_hold_controllers - set all desired and targets to measured
/// integrator is not reset
void relax_alt_hold_controllers();
private:
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
float _alt_min; // min altitude - should be updated from the main code with altitude limit from fence