fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 13:03: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/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp
+8 -15
View File
@@ -52,17 +52,15 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
Vector2f waypoint_north_east_local(1.0, 1.0);
waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
float x, y;
map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y);
Vector2f waypoint_north_east_reprojected(x, y);
Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1));
EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
@@ -72,8 +70,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y);
Vector2f waypoint_south_west_reprojected(x, y);
Vector2f waypoint_south_west_reprojected = ref.project(waypoint_southwest_global(0), waypoint_southwest_global(1));
EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
@@ -88,9 +85,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.fence_violation = true;
@@ -136,9 +132,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{42.1, 8.2};
param_t param = param_handle(px4::params::MPC_ACC_HOR);
@@ -245,9 +240,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
@@ -278,9 +272,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
{
GeofenceBreachAvoidance gf_avoidance(nullptr);
FakeGeofence geo;
struct map_projection_reference_s ref = {};
Vector2d home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
MapProjection ref{home_global(0), home_global(1)};
geofence_violation_type_u gf_violation;
gf_violation.flags.dist_to_home_exceeded = true;
src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp
+2 -5
View File
@@ -122,13 +122,10 @@ private:
bool _gf_boundary_is_20m_north(double lat, double lon, float alt)
{
struct map_projection_reference_s ref = {};
matrix::Vector2<double> home_global(42.1, 8.2);
map_projection_init(&ref, home_global(0), home_global(1));
float x, y;
map_projection_project(&ref, lat, lon, &x, &y);
matrix::Vector2f waypoint_local(x, y);
MapProjection projection{home_global(0), home_global(1)};
matrix::Vector2f waypoint_local = projection.project(lat, lon);
if (waypoint_local(0) >= 20.0f) {
return false;
src/modules/navigator/follow_target.cpp
+10 -10
View File
@@ -94,7 +94,7 @@ void FollowTarget::on_activation()
void FollowTarget::on_active()
{
struct map_projection_reference_s target_ref;
MapProjection target_ref;
follow_target_s target_motion_with_offset = {};
uint64_t current_time = hrt_absolute_time();
bool radius_entered = false;
@@ -134,9 +134,9 @@ void FollowTarget::on_active()
// get distance to target
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
&_target_distance(1));
target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
target_ref.project(_current_target_motion.lat, _current_target_motion.lon, _target_distance(0),
_target_distance(1));
}
@@ -149,11 +149,11 @@ void FollowTarget::on_active()
// ignore a small dt
if (dt_ms > 10.0F) {
// get last gps known reference for target
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
&(_target_position_delta(0)), &(_target_position_delta(1)));
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
(_target_position_delta(0)), (_target_position_delta(1)));
// update the average velocity of the target based on the position
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
@@ -229,9 +229,9 @@ void FollowTarget::on_active()
// get the target position using the calculated offset
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
&target_motion_with_offset.lat, &target_motion_with_offset.lon);
target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
target_motion_with_offset.lat, target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
src/modules/navigator/geofence.cpp
+4 -4
View File
@@ -447,13 +447,13 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
return false;
}
if (!map_projection_initialized(&_projection_reference)) {
map_projection_init(&_projection_reference, lat, lon);
if (!_projection_reference.isInitialized()) {
_projection_reference.initReference(lat, lon);
}
float x1, y1, x2, y2;
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
map_projection_project(&_projection_reference, circle_point.lat, circle_point.lon, &x2, &y2);
_projection_reference.project(lat, lon, x1, y1);
_projection_reference.project(circle_point.lat, circle_point.lon, x2, y2);
float dx = x1 - x2, dy = y1 - y2;
return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius;
}
src/modules/navigator/geofence.h
+1 -1
View File
@@ -173,7 +173,7 @@ private:
PolygonInfo *_polygons{nullptr};
int _num_polygons{0};
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
DEFINE_PARAMETERS(
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
src/modules/navigator/mission_block.cpp
+4 -7
View File
@@ -248,17 +248,14 @@ MissionBlock::is_mission_item_reached()
if (curr_sp->valid) {
// location of gate (mission item)
struct map_projection_reference_s ref_pos;
map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon);
MapProjection ref_pos{_mission_item.lat, _mission_item.lon};
// current setpoint
matrix::Vector2f gate_to_curr_sp;
map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1));
matrix::Vector2f gate_to_curr_sp = ref_pos.project(curr_sp->lat, curr_sp->lon);
// system position
matrix::Vector2f vehicle_pos;
map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
&vehicle_pos(0), &vehicle_pos(1));
matrix::Vector2f vehicle_pos = ref_pos.project(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon);
const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized());
// if the dot product (projected vector) is positive, then
src/modules/navigator/precland.cpp
+6 -6
View File
@@ -78,8 +78,8 @@ PrecLand::on_activation()
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
if (!map_projection_initialized(&_map_ref)) {
map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
if (!_map_ref.isInitialized()) {
_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -279,7 +279,7 @@ PrecLand::run_state_horizontal_approach()
// XXX need to transform to GPS coords because mc_pos_control only looks at that
double lat, lon;
map_projection_reproject(&_map_ref, x, y, &lat, &lon);
_map_ref.reproject(x, y, lat, lon);
pos_sp_triplet->current.lat = lat;
pos_sp_triplet->current.lon = lon;
@@ -316,7 +316,7 @@ PrecLand::run_state_descend_above_target()
// XXX need to transform to GPS coords because mc_pos_control only looks at that
double lat, lon;
map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon);
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, lat, lon);
pos_sp_triplet->current.lat = lat;
pos_sp_triplet->current.lon = lon;
@@ -555,8 +555,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
dt = 50000 / SEC2USEC;
// set a best guess for previous setpoints for smooth transition
map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
_navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
_map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
_navigator->get_position_setpoint_triplet()->current.lon, _sp_pev(0), _sp_pev(1));
_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
}
src/modules/navigator/precland.h
+1 -1
View File
@@ -111,7 +111,7 @@ private:
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */
MapProjection _map_ref {}; /**< reference for local/global projections */
uint64_t _state_start_time{0}; /**< time when we entered current state */
uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */