Browse Source

AP_Common: remove packed nature of Location, move flags out of union

This saves ~2.5kB on fmuv3
master
Peter Barker 6 years ago committed by Peter Barker
parent
commit
a3a012b77e
  1. 3
      libraries/AP_Common/AP_Common.cpp
  2. 20
      libraries/AP_Common/AP_Common.h
  3. 45
      libraries/AP_Common/Location.cpp
  4. 4
      libraries/AP_Common/Location.h

3
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; return false;
} }
// make sure we know what size the Location object is:
assert_storage_size<Location, 16> _assert_storage_size_Location;

20
libraries/AP_Common/AP_Common.h

@ -129,27 +129,17 @@ template<typename s, int t> 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 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 loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise
uint8_t terrain_alt : 1; // this altitude is above terrain uint8_t terrain_alt : 1; // this altitude is above terrain
uint8_t origin_alt : 1; // this altitude is above ekf origin 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 uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
};
struct PACKED Location { // note that mission storage only stores 24 bits of altitude (~ +/- 83km)
union { int32_t alt;
Location_Option_Flags flags; ///< options bitmask (1<<0 = relative altitude) int32_t lat;
uint8_t options; /// allows writing all flags to eeprom as one byte int32_t lng;
};
// 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
}; };
//@} //@}

45
libraries/AP_Common/Location.cpp

@ -17,11 +17,22 @@ Location_Class::Location_Class()
zero(); 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) Location_Class::Location_Class(int32_t latitude, int32_t longitude, int32_t alt_in_cm, ALT_FRAME frame)
{ {
zero();
lat = latitude; lat = latitude;
lng = longitude; lng = longitude;
options = 0;
set_alt_cm(alt_in_cm, frame); set_alt_cm(alt_in_cm, frame);
} }
@ -30,7 +41,11 @@ Location_Class::Location_Class(const Location& loc)
lat = loc.lat; lat = loc.lat;
lng = loc.lng; lng = loc.lng;
alt = loc.alt; 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) 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; lat = loc.lat;
lng = loc.lng; lng = loc.lng;
alt = loc.alt; 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; return *this;
} }
void Location_Class::set_alt_cm(int32_t alt_cm, ALT_FRAME frame) void Location_Class::set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
{ {
alt = alt_cm; alt = alt_cm;
flags.relative_alt = false; relative_alt = false;
flags.terrain_alt = false; terrain_alt = false;
flags.origin_alt = false; origin_alt = false;
switch (frame) { switch (frame) {
case ALT_FRAME_ABSOLUTE: case ALT_FRAME_ABSOLUTE:
// do nothing // do nothing
break; break;
case ALT_FRAME_ABOVE_HOME: case ALT_FRAME_ABOVE_HOME:
flags.relative_alt = true; relative_alt = true;
break; break;
case ALT_FRAME_ABOVE_ORIGIN: case ALT_FRAME_ABOVE_ORIGIN:
flags.origin_alt = true; origin_alt = true;
break; break;
case ALT_FRAME_ABOVE_TERRAIN: case ALT_FRAME_ABOVE_TERRAIN:
// we mark it as a relative altitude, as it doesn't have // we mark it as a relative altitude, as it doesn't have
// home alt added // home alt added
flags.relative_alt = true; relative_alt = true;
flags.terrain_alt = true; terrain_alt = true;
break; break;
} }
} }
@ -95,13 +114,13 @@ bool Location_Class::change_alt_frame(ALT_FRAME desired_frame)
// get altitude frame // get altitude frame
Location_Class::ALT_FRAME Location_Class::get_alt_frame() const Location_Class::ALT_FRAME Location_Class::get_alt_frame() const
{ {
if (flags.terrain_alt) { if (terrain_alt) {
return ALT_FRAME_ABOVE_TERRAIN; return ALT_FRAME_ABOVE_TERRAIN;
} }
if (flags.origin_alt) { if (origin_alt) {
return ALT_FRAME_ABOVE_ORIGIN; return ALT_FRAME_ABOVE_ORIGIN;
} }
if (flags.relative_alt) { if (relative_alt) {
return ALT_FRAME_ABOVE_HOME; return ALT_FRAME_ABOVE_HOME;
} }
return ALT_FRAME_ABSOLUTE; return ALT_FRAME_ABSOLUTE;

4
libraries/AP_Common/Location.h

@ -66,9 +66,9 @@ public:
// extrapolate latitude/longitude given distances (in meters) north and east // extrapolate latitude/longitude given distances (in meters) north and east
void offset(float ofs_north, float ofs_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: private:
static AP_Terrain *_terrain; static AP_Terrain *_terrain;

Loading…
Cancel
Save