fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 00:53:34 +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);
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+1 -1
View File
@@ -196,7 +196,7 @@ private:
_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
MapProjection _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
float _reference_altitude{NAN}; /**< Altitude relative to ground. */
hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+4 -5
View File
@@ -154,12 +154,11 @@ void FlightTask::_evaluateVehicleLocalPosition()
// global frame reference coordinates to enable conversions
if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) {
if (!_geo_projection.isInitialized()
|| (_geo_projection.getProjectionReferenceTimestamp() != _sub_vehicle_local_position.get().ref_timestamp)) {
map_projection_init_timestamped(&_global_local_proj_ref,
_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
_sub_vehicle_local_position.get().ref_timestamp);
_geo_projection.initReference(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
_sub_vehicle_local_position.get().ref_timestamp);
_global_local_alt0 = _sub_vehicle_local_position.get().ref_alt;
}
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+1 -1
View File
@@ -206,7 +206,7 @@ protected:
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
virtual void _ekfResetHandlerHeading(float delta_psi) {};
map_projection_reference_s _global_local_proj_ref{};
MapProjection _geo_projection{};
float _global_local_alt0{NAN};
/* Time abstraction */
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+5 -7
View File
@@ -84,10 +84,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
// commanded center coordinates
if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
if (map_projection_initialized(&_global_local_proj_ref)) {
map_projection_project(&_global_local_proj_ref,
command.param5, command.param6,
&_center(0), &_center(1));
if (_geo_projection.isInitialized()) {
_center.xy() = _geo_projection.project(command.param5, command.param6);
} else {
ret = false;
@@ -96,7 +94,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
// commanded altitude
if (PX4_ISFINITE(command.param7)) {
if (map_projection_initialized(&_global_local_proj_ref)) {
if (_geo_projection.isInitialized()) {
_center(2) = _global_local_alt0 - command.param7;
} else {
@@ -121,9 +119,9 @@ bool FlightTaskOrbit::sendTelemetry()
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
orbit_status.yaw_behaviour = _yaw_behaviour;
if (map_projection_initialized(&_global_local_proj_ref)) {
if (_geo_projection.isInitialized()) {
// local -> global
map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y);
_geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y);
orbit_status.z = _global_local_alt0 - _position_setpoint(2);
} else {