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

AC_PosControl: add get_alt_error method

Browse Source
This commit is contained in:
Randy Mackay 2014-01-06 12:30:51 +09:00 committed by Andrew Tridgell
parent abb42bcb41
commit 3ce1c0a9d5
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -61,6 +61,12 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
/// z-axis position controller
///
// get_alt_error - returns altitude error in cm
float AC_PosControl::get_alt_error() const
{
return (_pos_target.z - _inav.get_position().z);
}
/// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home
void AC_PosControl::set_target_to_stopping_point_z()
{
@ -265,7 +271,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z)
// To-Do: where to get hover throttle?
// To-Do: we had a contraint here but it's now removed, is this ok? with the motors library handle it ok?
_attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);
// to-do add back in PID logging?
}
/*
@ -614,4 +620,4 @@ void AC_PosControl::reset_I()
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
*/
*/

8
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -67,9 +67,13 @@ public:
///
void set_alt_target(float alt_cm) { _pos_target.z = alt_cm; }
/// 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; }
/// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
float get_desired_alt() const { return _pos_target.z; }
// get_alt_error - returns altitude error in cm
float get_alt_error() const;
/// set_target_to_stopping_point_z - sets altitude target to reasonable stopping altitude in cm above home
void set_target_to_stopping_point_z();