mirror of https://github.com/ArduPilot/ardupilot
AC_PosControl: add get_horizontal_error
This commit is contained in:
parent
8fe0c1b05b
commit
630e5378da
|
@ -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
|
||||
///
|
||||
|
|
|
@ -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();
|
||||
|
|
Loading…
Reference in New Issue