|
|
|
@ -17,11 +17,22 @@ Location_Class::Location_Class()
@@ -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)
@@ -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)
@@ -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)
@@ -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; |
|
|
|
|