aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: add relax Alt Hold controllers

Browse Source
This commit is contained in:
Leonard Hall 2015-04-14 23:56:33 +09:30 committed by Randy Mackay
parent 24b8d32b0e
commit 0aca3c4be6
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -160,6 +160,20 @@ void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float d
}
}
/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
{
_pos_target.z = _inav.get_altitude();
_vel_desired.z = 0.0f;
_vel_target.z= _inav.get_velocity_z();
_vel_last.z = _inav.get_velocity_z();
_accel_feedforward.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;
_pid_accel_z.set_integrator(throttle_setting);
}
// get_alt_error - returns altitude error in cm
float AC_PosControl::get_alt_error() const
{

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -126,6 +126,9 @@ public:
/// set_alt_target_to_current_alt - set altitude target to current altitude
void set_alt_target_to_current_alt() { _pos_target.z = _inav.get_altitude(); }
/// relax_alt_hold_controllers - set all desired and targets to measured
void relax_alt_hold_controllers(float throttle_setting);
/// get_alt_target, get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
/// To-Do: remove one of the two functions below
float get_alt_target() const { return _pos_target.z; }