aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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;
const AP_AHRS_NavEKF *Location_Class::_ahrs = nullptr;
AP_Terrain *Location_Class::_terrain = nullptr;
/// 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);
// calculate lat, lon
if (_ahrs != nullptr) {
Location ekf_origin;
if (_ahrs->get_origin(ekf_origin)) {
lat = ekf_origin.lat;
lng = ekf_origin.lng;
offset(ekf_offset_neu.x / 100.0f, ekf_offset_neu.y / 100.0f);
}
Location ekf_origin;
if (AP::ahrs().get_origin(ekf_origin)) {
lat = ekf_origin.lat;
lng = ekf_origin.lng;
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;
if (frame == ALT_FRAME_ABOVE_TERRAIN || desired_frame == ALT_FRAME_ABOVE_TERRAIN) {
#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;
}
// 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;
break;
case ALT_FRAME_ABOVE_HOME:
if (!_ahrs->home_is_set()) {
if (!AP::ahrs().home_is_set()) {
return false;
}
alt_abs = alt + _ahrs->get_home().alt;
alt_abs = alt + AP::ahrs().get_home().alt;
break;
case ALT_FRAME_ABOVE_ORIGIN:
{
// fail if we cannot get ekf origin
Location ekf_origin;
if (_ahrs == nullptr || !_ahrs->get_origin(ekf_origin)) {
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
}
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;
return true;
case ALT_FRAME_ABOVE_HOME:
if (!_ahrs->home_is_set()) {
if (!AP::ahrs().home_is_set()) {
return false;
}
ret_alt_cm = alt_abs - _ahrs->get_home().alt;
ret_alt_cm = alt_abs - AP::ahrs().get_home().alt;
return true;
case ALT_FRAME_ABOVE_ORIGIN:
{
// fail if we cannot get ekf origin
Location ekf_origin;
if (_ahrs == nullptr || !_ahrs->get_origin(ekf_origin)) {
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
}
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
{
Location ekf_origin;
if (!_ahrs->get_origin(ekf_origin)) {
if (!AP::ahrs().get_origin(ekf_origin)) {
return false;
}
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 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; }
// operators
@ -73,7 +71,6 @@ public:
void zero(void) { lat = lng = alt = 0; options = 0; }
private:
static const AP_AHRS_NavEKF *_ahrs;
static AP_Terrain *_terrain;
};