InertialNav: fix for get_velocity_xy

forgot to sq
This commit is contained in:
Jason Short 2013-11-02 15:57:08 -07:00 committed by Randy Mackay
parent 9951c50e48
commit 1c7d9f43c1

View File

@ -279,7 +279,7 @@ void AP_InertialNav::set_velocity_xy(float x, float y)
// set_velocity_xy - set velocity in latitude & longitude directions (in cm/s) // set_velocity_xy - set velocity in latitude & longitude directions (in cm/s)
float AP_InertialNav::get_velocity_xy() float AP_InertialNav::get_velocity_xy()
{ {
return safe_sqrt(_velocity.x + _velocity.y); return safe_sqrt(_velocity.x * _velocity.x + _velocity.y * _velocity.y);
} }
// //