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

AC_AttitudeControl: AC_PosControl: avoid calling norm()

Browse Source 
The result of this won't be used as often as this loop is called
This commit is contained in:
Peter Barker 2018-10-26 08:46:52 +11:00 committed by Randy Mackay
parent 6dd44a6d35
commit dcd930497a
2 changed files with 1 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -741,7 +741,7 @@ void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
/// get_distance_to_target - get horizontal distance to target position in cm /// get_distance_to_target - get horizontal distance to target position in cm
float AC_PosControl::get_distance_to_target() const float AC_PosControl::get_distance_to_target() const
{ {
return _distance_to_target; return norm(_pos_error.x, _pos_error.y);
} }
/// get_bearing_to_target - get bearing to target position in centi-degrees /// get_bearing_to_target - get bearing to target position in centi-degrees
@ -1016,7 +1016,6 @@ void AC_PosControl::run_xy_controller(float dt)
_pos_target.x = curr_pos.x + _pos_error.x; _pos_target.x = curr_pos.x + _pos_error.x;
_pos_target.y = curr_pos.y + _pos_error.y; _pos_target.y = curr_pos.y + _pos_error.y;
} }
_distance_to_target = norm(_pos_error.x, _pos_error.y);
_vel_target = sqrt_controller(_pos_error, kP, _accel_cms); _vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
} }

1
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -407,7 +407,6 @@ protected:
Vector3f _accel_target; // acceleration target in cm/s/s Vector3f _accel_target; // acceleration target in cm/s/s
Vector3f _accel_error; // acceleration error in cm/s/s Vector3f _accel_error; // acceleration error in cm/s/s
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
float _distance_to_target; // distance to position target - for reporting only
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
LowPassFilterVector2f _accel_target_filter; // acceleration target filter LowPassFilterVector2f _accel_target_filter; // acceleration target filter