fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 08:40:35 +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
src/modules/commander/HomePosition.cpp
+12 -22
View File
@@ -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<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,
_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<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);
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<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;
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<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 bool time = (now < vehicle_gps_position.timestamp + 1_s);
const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;