fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 00:43:35 +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/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