From 57804e31187fc50657f469e20cd20f002c97bdae Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 2 Jan 2019 13:53:47 +1100 Subject: [PATCH] AP_Common: unify Location_Class and Location --- libraries/AP_Common/AP_Common.cpp | 3 -- libraries/AP_Common/AP_Common.h | 30 ---------------- libraries/AP_Common/Location.cpp | 60 ++++++++++--------------------- libraries/AP_Common/Location.h | 23 +++++++----- 4 files changed, 34 insertions(+), 82 deletions(-) diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index 73b311e89b..217841cb39 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/AP_Common.cpp @@ -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 _assert_storage_size_Location; diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index 47b97d5195..d5176c179d 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -114,36 +114,6 @@ template struct assert_storage_size { _assert_storage_size _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 /// diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index a93165905b..66fe09cd43 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -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 _assert_storage_size_Location; diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 28e4616b3c..103893e865 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -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);