diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 35cd997af7..0f9766bebb 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -72,19 +72,14 @@ bool HomePosition::hasMovedFromCurrentHomeLocation() epv = gpos.epv; } else if (_gps_position_for_home_valid) { - sensor_gps_s gps; - _vehicle_gps_position_sub.copy(&gps); - const double lat = static_cast(gps.lat) * 1e-7; - const double lon = static_cast(gps.lon) * 1e-7; - const float alt = static_cast(gps.alt) * 1e-3f; get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon, _home_position_pub.get().alt, - lat, lon, alt, + _gps_lat, _gps_lon, _gps_alt, &home_dist_xy, &home_dist_z); - eph = gps.eph; - epv = gps.epv; + eph = _gps_eph; + epv = _gps_epv; } } @@ -118,12 +113,7 @@ bool HomePosition::setHomePosition(bool force) } else if (_gps_position_for_home_valid) { // Set home using GNSS position - sensor_gps_s gps_pos; - _vehicle_gps_position_sub.copy(&gps_pos); - const double lat = static_cast(gps_pos.lat) * 1e-7; - const double lon = static_cast(gps_pos.lon) * 1e-7; - const float alt = static_cast(gps_pos.alt) * 1e-3f; - fillGlobalHomePos(home, lat, lon, alt); + fillGlobalHomePos(home, _gps_lat, _gps_lon, _gps_alt); setHomePosValid(); updated = true; @@ -209,20 +199,14 @@ void HomePosition::setInAirHomePosition() // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS raw) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); - sensor_gps_s gps; - _vehicle_gps_position_sub.copy(&gps); - const double lat = static_cast(gps.lat) * 1e-7; - const double lon = static_cast(gps.lon) * 1e-7; - const float alt = static_cast(gps.alt) * 1e-3f; - - MapProjection ref_pos{lat, lon}; + MapProjection ref_pos{_gps_lat, _gps_lon}; double home_lat; double home_lon; ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon); - const float home_alt = alt + home.z; + const float home_alt = _gps_alt + home.z; fillGlobalHomePos(home, home_lat, home_lon, home_alt); setHomePosValid(); @@ -320,6 +304,12 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) sensor_gps_s vehicle_gps_position; _vehicle_gps_position_sub.copy(&vehicle_gps_position); + _gps_lat = static_cast(vehicle_gps_position.lat) * 1e-7; + _gps_lon = static_cast(vehicle_gps_position.lon) * 1e-7; + _gps_alt = static_cast(vehicle_gps_position.alt) * 1e-3f; + _gps_eph = vehicle_gps_position.eph; + _gps_epv = vehicle_gps_position.epv; + const hrt_abstime now = hrt_absolute_time(); const bool time = (now < vehicle_gps_position.timestamp + 1_s); const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType; diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 6b883d4e52..8456ddb6b8 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -81,4 +81,9 @@ private: bool _valid{false}; const failsafe_flags_s &_failsafe_flags; bool _gps_position_for_home_valid{false}; + double _gps_lat{0}; + double _gps_lon{0}; + float _gps_alt{0.f}; + float _gps_eph{0.f}; + float _gps_epv{0.f}; };