fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo: Moved the map_projection_* functions and state into a self-contained C++ class

Browse Source
This commit is contained in:
Thomas Debrunner
2021-12-06 14:30:14 +01:00
committed by Matthias Grob
parent fe23718e2c
commit 8db7a6225b
38 changed files with 324 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_pos_control/RoverPositionControl.cpp
+7 -7
View File
@@ -419,11 +419,11 @@ RoverPositionControl::Run()
// Convert Local setpoints to global setpoints
if (_control_mode.flag_control_offboard_enabled) {
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
if (!_global_local_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
@@ -431,9 +431,9 @@ RoverPositionControl::Run()
_trajectory_setpoint_sub.update(&_trajectory_setpoint);
// local -> global
map_projection_reproject(&_global_local_proj_ref,
_trajectory_setpoint.x, _trajectory_setpoint.y,
&_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
_global_local_proj_ref.reproject(
_trajectory_setpoint.x, _trajectory_setpoint.y,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z;
_pos_sp_triplet.current.valid = true;