Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Common: improved accuracy of get_bearing() 

make base function ftype, then convert to int32_t for get_bearing_to()
This commit is contained in:
Andrew Tridgell 2022-03-08 08:35:25 +11:00
parent f7e10208cd
commit 96a55878a7
3 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Common/Location.cpp
View File

@ -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;
}

11
libraries/AP_Common/Location.h
View File

@ -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;

2
libraries/AP_Common/tests/test_location.cpp
View File

@ -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);
}