Copter: SuperSimple mode bug

SuperSimple bearing was updated without checking for GPS 3D Fix availability.
This commit is contained in:
Olivier-ADLER 2013-06-15 23:48:01 +02:00 committed by Randy Mackay
parent 1cbedc06b3
commit 40b7000cfd

View File

@ -68,7 +68,7 @@ static void calc_distance_and_bearing()
}
// calculate home distance and bearing
if( ap.home_is_set ) {
if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));