Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Common: Location: use AP_AHRS singleton

Browse Source
This commit is contained in:
Peter Barker 2018-05-30 11:28:53 +10:00 committed by Francisco Ferreira
parent 9a345160c1
commit dc202c1ff9
2 changed files with 13 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Common/Location.cpp
View File

@ -9,7 +9,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
const AP_AHRS_NavEKF *Location_Class::_ahrs = nullptr;
AP_Terrain *Location_Class::_terrain = nullptr; AP_Terrain *Location_Class::_terrain = nullptr;
/// constructors /// constructors
@ -40,13 +39,11 @@ Location_Class::Location_Class(const Vector3f &ekf_offset_neu)
set_alt_cm(ekf_offset_neu.z, ALT_FRAME_ABOVE_ORIGIN); set_alt_cm(ekf_offset_neu.z, ALT_FRAME_ABOVE_ORIGIN);
// calculate lat, lon // calculate lat, lon
if (_ahrs != nullptr) { Location ekf_origin;
Location ekf_origin; if (AP::ahrs().get_origin(ekf_origin)) {
if (_ahrs->get_origin(ekf_origin)) { lat = ekf_origin.lat;
lat = ekf_origin.lat; lng = ekf_origin.lng;
lng = ekf_origin.lng; offset(ekf_offset_neu.x / 100.0f, ekf_offset_neu.y / 100.0f);
offset(ekf_offset_neu.x / 100.0f, ekf_offset_neu.y / 100.0f);
}
} }
} }
@ -125,7 +122,7 @@ bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) co
float alt_terr_cm = 0; float alt_terr_cm = 0;
if (frame == ALT_FRAME_ABOVE_TERRAIN || desired_frame == ALT_FRAME_ABOVE_TERRAIN) { if (frame == ALT_FRAME_ABOVE_TERRAIN || desired_frame == ALT_FRAME_ABOVE_TERRAIN) {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
if (_ahrs == nullptr || _terrain == nullptr || !_terrain->height_amsl(*(Location *)this, alt_terr_cm, true)) { if (_terrain == nullptr || !_terrain->height_amsl(*(Location *)this, alt_terr_cm, true)) {
return false; return false;
} }
// convert terrain alt to cm // convert terrain alt to cm
@ -142,16 +139,16 @@ bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) co
alt_abs = alt; alt_abs = alt;
break; break;
case ALT_FRAME_ABOVE_HOME: case ALT_FRAME_ABOVE_HOME:
if (!_ahrs->home_is_set()) { if (!AP::ahrs().home_is_set()) {
return false; return false;
} }
alt_abs = alt + _ahrs->get_home().alt; alt_abs = alt + AP::ahrs().get_home().alt;
break; break;
case ALT_FRAME_ABOVE_ORIGIN: case ALT_FRAME_ABOVE_ORIGIN:
{ {
// fail if we cannot get ekf origin // fail if we cannot get ekf origin
Location ekf_origin; Location ekf_origin;
if (_ahrs == nullptr || !_ahrs->get_origin(ekf_origin)) { if (!AP::ahrs().get_origin(ekf_origin)) {
return false; return false;
} }
alt_abs = alt + ekf_origin.alt; alt_abs = alt + ekf_origin.alt;
@ -171,16 +168,16 @@ bool Location_Class::get_alt_cm(ALT_FRAME desired_frame, int32_t &ret_alt_cm) co
ret_alt_cm = alt_abs; ret_alt_cm = alt_abs;
return true; return true;
case ALT_FRAME_ABOVE_HOME: case ALT_FRAME_ABOVE_HOME:
if (!_ahrs->home_is_set()) { if (!AP::ahrs().home_is_set()) {
return false; return false;
} }
ret_alt_cm = alt_abs - _ahrs->get_home().alt; ret_alt_cm = alt_abs - AP::ahrs().get_home().alt;
return true; return true;
case ALT_FRAME_ABOVE_ORIGIN: case ALT_FRAME_ABOVE_ORIGIN:
{ {
// fail if we cannot get ekf origin // fail if we cannot get ekf origin
Location ekf_origin; Location ekf_origin;
if (_ahrs == nullptr || !_ahrs->get_origin(ekf_origin)) { if (!AP::ahrs().get_origin(ekf_origin)) {
return false; return false;
} }
ret_alt_cm = alt_abs - ekf_origin.alt; ret_alt_cm = alt_abs - ekf_origin.alt;
@ -198,7 +195,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_Class::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
{ {
Location ekf_origin; Location ekf_origin;
if (!_ahrs->get_origin(ekf_origin)) { if (!AP::ahrs().get_origin(ekf_origin)) {
return false; return false;
} }
vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM; vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;

3
libraries/AP_Common/Location.h
View File

@ -32,8 +32,6 @@ public:
Location_Class(const Location& loc); Location_Class(const Location& loc);
Location_Class(const Vector3f &ekf_offset_neu); Location_Class(const Vector3f &ekf_offset_neu);
/// accept reference to ahrs and (indirectly) EKF
static void set_ahrs(const AP_AHRS_NavEKF* ahrs) { _ahrs = ahrs; }
static void set_terrain(AP_Terrain* terrain) { _terrain = terrain; } static void set_terrain(AP_Terrain* terrain) { _terrain = terrain; }
// operators // operators
@ -73,7 +71,6 @@ public:
void zero(void) { lat = lng = alt = 0; options = 0; } void zero(void) { lat = lng = alt = 0; options = 0; }
private: private:
static const AP_AHRS_NavEKF *_ahrs;
static AP_Terrain *_terrain; static AP_Terrain *_terrain;
}; };