AC_PosControl: add get_horizontal_error

This commit is contained in:
Jonathan Challinger 2016-07-07 17:36:01 -07:00 committed by Randy Mackay
parent 8fe0c1b05b
commit 630e5378da
2 changed files with 8 additions and 0 deletions

View File

@ -744,6 +744,11 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
update_z_controller();
}
float AC_PosControl::get_horizontal_error() const
{
return norm(_pos_error.x, _pos_error.y);
}
///
/// private methods
///

View File

@ -147,6 +147,9 @@ public:
/// get_alt_error - returns altitude error in cm
float get_alt_error() const;
// returns horizontal error in cm
float get_horizontal_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();