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

AP_Common: unify Location_Class and Location

This commit is contained in:
Peter Barker 2019-01-02 13:53:47 +11:00 committed by Peter Barker
parent d92f34c3d0
commit 57804e3118
4 changed files with 34 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Common/AP_Common.cpp
View File

@ -36,6 +36,3 @@ bool is_bounded_int32(int32_t value, int32_t lower_bound, int32_t upper_bound)
return false;
}
// make sure we know what size the Location object is:
assert_storage_size<Location, 16> _assert_storage_size_Location;

30
libraries/AP_Common/AP_Common.h
View File

@ -114,36 +114,6 @@ template<typename s, int t> struct assert_storage_size {
_assert_storage_size<s, sizeof(s), t> _member;
};
////////////////////////////////////////////////////////////////////////////////
/// @name Types
///
/// Data structures and types used throughout the libraries and applications. 0 = default
/// bit 0: Altitude is stored 0: Absolute, 1: Relative
/// bit 1: Change Alt between WP 0: Gradually, 1: ASAP
/// bit 2: Direction of loiter command 0: Clockwise 1: Counter-Clockwise
/// bit 3: Req.to hit WP.alt to continue 0: No, 1: Yes
/// bit 4: Relative to Home 0: No, 1: Yes
/// bit 5: Loiter crosstrack reference 0: WP center 1: Tangent exit point
/// bit 6:
/// bit 7: Move to next Command 0: YES, 1: Loiter until commanded
//@{
struct Location {
uint8_t relative_alt : 1; // 1 if altitude is relative to home
uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise
uint8_t terrain_alt : 1; // this altitude is above terrain
uint8_t origin_alt : 1; // this altitude is above ekf origin
uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
// note that mission storage only stores 24 bits of altitude (~ +/- 83km)
int32_t alt;
int32_t lat;
int32_t lng;
};
//@}
////////////////////////////////////////////////////////////////////////////////
/// @name Conversions
///

60
libraries/AP_Common/Location.cpp
View File

@ -9,26 +9,26 @@
extern const AP_HAL::HAL& hal;
AP_Terrain *Location_Class::_terrain = nullptr;
AP_Terrain *Location::_terrain = nullptr;
/// constructors
Location_Class::Location_Class()
Location::Location()
{
zero();
}
const Location_Class definitely_zero{};
bool Location_Class::is_zero(void) const
const Location definitely_zero{};
bool Location::is_zero(void) const
{
return !memcmp(this, &definitely_zero, sizeof(*this));
}
void Location_Class::zero(void)
void Location::zero(void)
{
memset(this, 0, sizeof(*this));
}
Location_Class::Location_Class(int32_t latitude, int32_t longitude, int32_t alt_in_cm, ALT_FRAME frame)
Location::Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, ALT_FRAME frame)
{
zero();
lat = latitude;
@ -36,19 +36,7 @@ Location_Class::Location_Class(int32_t latitude, int32_t longitude, int32_t alt_
set_alt_cm(alt_in_cm, frame);
}
Location_Class::Location_Class(const Location& loc)
{
lat = loc.lat;
lng = loc.lng;
alt = loc.alt;
relative_alt = loc.relative_alt;
loiter_ccw = loc.loiter_ccw;
terrain_alt = loc.terrain_alt;
origin_alt = loc.origin_alt;
loiter_xtrack = loc.loiter_xtrack;
}
Location_Class::Location_Class(const Vector3f &ekf_offset_neu)
Location::Location(const Vector3f &ekf_offset_neu)
{
// store alt and alt frame
set_alt_cm(ekf_offset_neu.z, ALT_FRAME_ABOVE_ORIGIN);
@ -62,20 +50,7 @@ Location_Class::Location_Class(const Vector3f &ekf_offset_neu)
}
}
Location_Class& Location_Class::operator=(const struct Location &loc)
{
lat = loc.lat;
lng = loc.lng;
alt = loc.alt;
relative_alt = loc.relative_alt;
loiter_ccw = loc.loiter_ccw;
terrain_alt = loc.terrain_alt;
origin_alt = loc.origin_alt;
loiter_xtrack = loc.loiter_xtrack;
return *this;
}
void Location_Class::set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
void Location::set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
{
alt = alt_cm;
relative_alt = false;
@ -101,7 +76,7 @@ void Location_Class::set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
}
// converts altitude to new frame
bool Location_Class::change_alt_frame(ALT_FRAME desired_frame)
bool Location::change_alt_frame(ALT_FRAME desired_frame)
{
int32_t new_alt_cm;
if (!get_alt_cm(desired_frame, new_alt_cm)) {
@ -112,7 +87,7 @@ bool Location_Class::change_alt_frame(ALT_FRAME desired_frame)
}
// get altitude frame
Location_Class::ALT_FRAME Location_Class::get_alt_frame() const
Location::ALT_FRAME Location::get_alt_frame() const
{
if (terrain_alt) {
return ALT_FRAME_ABOVE_TERRAIN;
@ -127,9 +102,9 @@ Location_Class::ALT_FRAME Location_Class::get_alt_frame() const
}
/// get altitude in desired frame
bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) const
bool Location::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) const
{
Location_Class::ALT_FRAME frame = get_alt_frame();
Location::ALT_FRAME frame = get_alt_frame();
// shortcut if desired and underlying frame are the same
if (desired_frame == frame) {
@ -211,7 +186,7 @@ bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) co
}
}
bool Location_Class::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{
Location ekf_origin;
if (!AP::ahrs().get_origin(ekf_origin)) {
@ -222,7 +197,7 @@ bool Location_Class::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
return true;
}
bool Location_Class::get_vector_from_origin_NEU(Vector3f &vec_neu) const
bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
{
// convert lat, lon
Vector2f vec_ne;
@ -243,7 +218,7 @@ bool Location_Class::get_vector_from_origin_NEU(Vector3f &vec_neu) const
}
// return distance in meters between two locations
float Location_Class::get_distance(const struct Location &loc2) const
float Location::get_distance(const struct Location &loc2) const
{
float dlat = (float)(loc2.lat - lat);
float dlng = ((float)(loc2.lng - lng)) * longitude_scale(loc2);
@ -251,7 +226,7 @@ float Location_Class::get_distance(const struct Location &loc2) const
}
// extrapolate latitude/longitude given distances (in meters) north and east
void Location_Class::offset(float ofs_north, float ofs_east)
void Location::offset(float ofs_north, float ofs_east)
{
// use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class
if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) {
@ -261,3 +236,6 @@ void Location_Class::offset(float ofs_north, float ofs_east)
lng += dlng;
}
}
// make sure we know what size the Location object is:
assert_storage_size<Location, 16> _assert_storage_size_Location;

23
libraries/AP_Common/Location.h
View File

@ -14,10 +14,21 @@
class AP_AHRS_NavEKF;
class AP_Terrain;
class Location_Class : public Location
class Location
{
public:
uint8_t relative_alt : 1; // 1 if altitude is relative to home
uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise
uint8_t terrain_alt : 1; // this altitude is above terrain
uint8_t origin_alt : 1; // this altitude is above ekf origin
uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
// note that mission storage only stores 24 bits of altitude (~ +/- 83km)
int32_t alt;
int32_t lat;
int32_t lng;
/// enumeration of possible altitude types
enum ALT_FRAME {
ALT_FRAME_ABSOLUTE = 0,
@ -27,16 +38,12 @@ public:
};
/// constructors
Location_Class();
Location_Class(int32_t latitude, int32_t longitude, int32_t alt_in_cm, ALT_FRAME frame);
Location_Class(const Location& loc);
Location_Class(const Vector3f &ekf_offset_neu);
Location();
Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, ALT_FRAME frame);
Location(const Vector3f &ekf_offset_neu);
static void set_terrain(AP_Terrain* terrain) { _terrain = terrain; }
// operators
Location_Class& operator=(const struct Location &loc);
// set altitude
void set_alt_cm(int32_t alt_cm, ALT_FRAME frame);