fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 09:53:38 +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/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+7 -7
View File
@@ -365,8 +365,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_lock_position_xy.setAll(NAN);
// Convert from global to local frame.
map_projection_project(&_reference_position,
_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1));
_reference_position.project(_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon,
tmp_target(0), tmp_target(1));
}
tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
@@ -407,8 +407,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_prev_prev_wp = _triplet_prev_wp;
if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat,
_sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
_reference_position.project(_sub_triplet_setpoint.get().previous.lat,
_sub_triplet_setpoint.get().previous.lon, _triplet_prev_wp(0), _triplet_prev_wp(1));
_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
} else {
@@ -421,8 +421,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_triplet_next_wp = _triplet_target;
} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat,
_sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
_reference_position.project(_sub_triplet_setpoint.get().next.lat,
_sub_triplet_setpoint.get().next.lon, _triplet_next_wp(0), _triplet_next_wp(1));
_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
} else {
@@ -591,7 +591,7 @@ bool FlightTaskAuto::_evaluateGlobalReference()
}
// init projection
map_projection_init(&_reference_position, ref_lat, ref_lon);
_reference_position.initReference(ref_lat, ref_lon);
// check if everything is still finite
return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon);