diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index 217841cb39..73b311e89b 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/AP_Common.cpp @@ -36,3 +36,6 @@ 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 7bf9f409e5..47b97d5195 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -129,27 +129,17 @@ template struct assert_storage_size { //@{ -struct PACKED Location_Option_Flags { +struct Location { uint8_t relative_alt : 1; // 1 if altitude is relative to home - uint8_t unused1 : 1; // unused flag (defined so that loiter_ccw uses the correct bit) 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 -}; -struct PACKED Location { - union { - Location_Option_Flags flags; ///< options bitmask (1<<0 = relative altitude) - uint8_t options; /// allows writing all flags to eeprom as one byte - }; - // by making alt 24 bit we can make p1 in a command 16 bit, - // allowing an accurate angle in centi-degrees. This keeps the - // storage cost per mission item at 15 bytes, and allows mission - // altitudes of up to +/- 83km - int32_t alt:24; ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M - int32_t lat; ///< param 3 - Latitude * 10**7 - int32_t lng; ///< param 4 - Longitude * 10**7 + // note that mission storage only stores 24 bits of altitude (~ +/- 83km) + int32_t alt; + int32_t lat; + int32_t lng; }; //@} diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 1238e3796e..a93165905b 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -17,11 +17,22 @@ Location_Class::Location_Class() zero(); } +const Location_Class definitely_zero{}; +bool Location_Class::is_zero(void) const +{ + return !memcmp(this, &definitely_zero, sizeof(*this)); +} + +void Location_Class::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) { + zero(); lat = latitude; lng = longitude; - options = 0; set_alt_cm(alt_in_cm, frame); } @@ -30,7 +41,11 @@ Location_Class::Location_Class(const Location& loc) lat = loc.lat; lng = loc.lng; alt = loc.alt; - options = loc.options; + 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) @@ -52,31 +67,35 @@ Location_Class& Location_Class::operator=(const struct Location &loc) lat = loc.lat; lng = loc.lng; alt = loc.alt; - options = loc.options; + 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) { alt = alt_cm; - flags.relative_alt = false; - flags.terrain_alt = false; - flags.origin_alt = false; + relative_alt = false; + terrain_alt = false; + origin_alt = false; switch (frame) { case ALT_FRAME_ABSOLUTE: // do nothing break; case ALT_FRAME_ABOVE_HOME: - flags.relative_alt = true; + relative_alt = true; break; case ALT_FRAME_ABOVE_ORIGIN: - flags.origin_alt = true; + origin_alt = true; break; case ALT_FRAME_ABOVE_TERRAIN: // we mark it as a relative altitude, as it doesn't have // home alt added - flags.relative_alt = true; - flags.terrain_alt = true; + relative_alt = true; + terrain_alt = true; break; } } @@ -95,13 +114,13 @@ bool Location_Class::change_alt_frame(ALT_FRAME desired_frame) // get altitude frame Location_Class::ALT_FRAME Location_Class::get_alt_frame() const { - if (flags.terrain_alt) { + if (terrain_alt) { return ALT_FRAME_ABOVE_TERRAIN; } - if (flags.origin_alt) { + if (origin_alt) { return ALT_FRAME_ABOVE_ORIGIN; } - if (flags.relative_alt) { + if (relative_alt) { return ALT_FRAME_ABOVE_HOME; } return ALT_FRAME_ABSOLUTE; diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 36fa9367dd..28e4616b3c 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -66,9 +66,9 @@ public: // extrapolate latitude/longitude given distances (in meters) north and east void offset(float ofs_north, float ofs_east); - bool is_zero(void) { return (lat == 0 && lng == 0 && alt == 0 && options == 0); } + bool is_zero(void) const; - void zero(void) { lat = lng = alt = 0; options = 0; } + void zero(void); private: static AP_Terrain *_terrain;