AP_Common: improved accuracy of get_bearing()

make base function ftype, then convert to int32_t for get_bearing_to()

libraries/AP_Common/Location.cpp

@@ -374,14 +374,14 @@ bool Location::sanitize(const Location &defaultLoc)
 assert_storage_size<Location, 16> _assert_storage_size_Location;
 
 
-// return bearing in centi-degrees from location to loc2
-int32_t Location::get_bearing_to(const struct Location &loc2) const
+// return bearing in radians from location to loc2, return is 0 to 2*Pi
+ftype Location::get_bearing(const struct Location &loc2) const
 {
     const int32_t off_x = diff_longitude(loc2.lng,lng);
     const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2);
-    int32_t bearing = 9000 + atan2F(-off_y, off_x) * DEGX100;
+    ftype bearing = (M_PI*0.5) + atan2F(-off_y, off_x);
     if (bearing < 0) {
-        bearing += 36000;
+        bearing += 2*M_PI;
     }
     return bearing;
 }

libraries/AP_Common/Location.h

@@ -92,10 +92,13 @@ public:
 
     void zero(void);
 
-    // return bearing in centi-degrees from location to loc2
-    int32_t get_bearing_to(const struct Location &loc2) const;
-    // return the bearing in radians
-    ftype get_bearing(const struct Location &loc2) const { return radians(get_bearing_to(loc2) * 0.01); } ;
+    // return the bearing in radians, from 0 to 2*Pi
+    ftype get_bearing(const struct Location &loc2) const;
+
+    // return bearing in centi-degrees from location to loc2, return is 0 to 35999
+    int32_t get_bearing_to(const struct Location &loc2) const {
+        return int32_t(get_bearing(loc2) * DEGX100 + 0.5);
+    }
 
     // check if lat and lng match. Ignore altitude and options
     bool same_latlon_as(const Location &loc2) const;

libraries/AP_Common/tests/test_location.cpp

@@ -311,7 +311,7 @@ TEST(Location, Distance)
     bearing = test_home.get_bearing_to(test_loc);
     EXPECT_EQ(31503, bearing);
     const float bearing_rad = test_home.get_bearing(test_loc);
-    EXPECT_FLOAT_EQ(radians(315.03), bearing_rad);
+    EXPECT_FLOAT_EQ(5.4982867, bearing_rad);
 
 }