UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-26 16:28:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: copy sensor_gps in HomePosition::update() and store relevant fields in separate variables

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-02-22 16:32:51 +01:00
committed by Beat Küng
parent 006321e278
commit 4259b5adac
2 changed files with 17 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/commander/HomePosition.cpp
View File

@@ -72,19 +72,14 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
epv = gpos.epv; epv = gpos.epv;
} else if (_gps_position_for_home_valid) { } else if (_gps_position_for_home_valid) {
sensor_gps_s gps;
_vehicle_gps_position_sub.copy(&gps);
const double lat = static_cast<double>(gps.lat) * 1e-7;
const double lon = static_cast<double>(gps.lon) * 1e-7;
const float alt = static_cast<float>(gps.alt) * 1e-3f;
get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon, get_distance_to_point_global_wgs84(_home_position_pub.get().lat, _home_position_pub.get().lon,
_home_position_pub.get().alt, _home_position_pub.get().alt,
lat, lon, alt, _gps_lat, _gps_lon, _gps_alt,
&home_dist_xy, &home_dist_z); &home_dist_xy, &home_dist_z);
eph = gps.eph; eph = _gps_eph;
epv = gps.epv; epv = _gps_epv;
} }
} }
@@ -118,12 +113,7 @@ bool HomePosition::setHomePosition(bool force)
} else if (_gps_position_for_home_valid) { } else if (_gps_position_for_home_valid) {
// Set home using GNSS position // Set home using GNSS position
sensor_gps_s gps_pos; fillGlobalHomePos(home, _gps_lat, _gps_lon, _gps_alt);
_vehicle_gps_position_sub.copy(&gps_pos);
const double lat = static_cast<double>(gps_pos.lat) * 1e-7;
const double lon = static_cast<double>(gps_pos.lon) * 1e-7;
const float alt = static_cast<float>(gps_pos.alt) * 1e-3f;
fillGlobalHomePos(home, lat, lon, alt);
setHomePosValid(); setHomePosValid();
updated = true; updated = true;
@@ -209,20 +199,14 @@ void HomePosition::setInAirHomePosition()
// Back-compute lon, lat and alt of home position given the local home position // Back-compute lon, lat and alt of home position given the local home position
// and current positions in local and global (GNSS raw) frames // and current positions in local and global (GNSS raw) frames
const vehicle_local_position_s &lpos = _local_position_sub.get(); 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<double>(gps.lat) * 1e-7; MapProjection ref_pos{_gps_lat, _gps_lon};
const double lon = static_cast<double>(gps.lon) * 1e-7;
const float alt = static_cast<float>(gps.alt) * 1e-3f;
MapProjection ref_pos{lat, lon};
double home_lat; double home_lat;
double home_lon; double home_lon;
ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, 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); fillGlobalHomePos(home, home_lat, home_lon, home_alt);
setHomePosValid(); setHomePosValid();
@@ -320,6 +304,12 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
sensor_gps_s vehicle_gps_position; sensor_gps_s vehicle_gps_position;
_vehicle_gps_position_sub.copy(&vehicle_gps_position); _vehicle_gps_position_sub.copy(&vehicle_gps_position);
_gps_lat = static_cast<double>(vehicle_gps_position.lat) * 1e-7;
_gps_lon = static_cast<double>(vehicle_gps_position.lon) * 1e-7;
_gps_alt = static_cast<float>(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 hrt_abstime now = hrt_absolute_time();
const bool time = (now < vehicle_gps_position.timestamp + 1_s); const bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType; const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;

5
src/modules/commander/HomePosition.hpp
View File

@@ -81,4 +81,9 @@ private:
bool _valid{false}; bool _valid{false};
const failsafe_flags_s &_failsafe_flags; const failsafe_flags_s &_failsafe_flags;
bool _gps_position_for_home_valid{false}; 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};
}; };