fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 12:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

use current local position for land and not GPS -> e.g. flow

Browse Source
This commit is contained in:
ChristophTobler
2017-05-03 16:52:37 +02:00
committed by Dennis Mannhart
parent 5bfe6d7fec
commit dce28454c8
2 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+23 -8
View File
@@ -755,9 +755,9 @@ MulticopterPositionControl::poll_subscriptions()
if (updated) { if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid //set current position setpoint invalid if none of them (lat, lon and alt) is finite
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
!PX4_ISFINITE(_pos_sp_triplet.current.lon) || !PX4_ISFINITE(_pos_sp_triplet.current.lon) &&
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) { !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
} }
@@ -1384,11 +1384,26 @@ void MulticopterPositionControl::control_auto(float dt)
if (_pos_sp_triplet.current.valid) { if (_pos_sp_triplet.current.valid) {
/* project setpoint to local frame */ //only project setpoints if they are finite, else use current position
map_projection_project(&_ref_pos, if (PX4_ISFINITE(_pos_sp_triplet.current.lat) &&
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, PX4_ISFINITE(_pos_sp_triplet.current.lon)) {
&_curr_pos_sp.data[0], &_curr_pos_sp.data[1]); /* project setpoint to local frame */
_curr_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_curr_pos_sp.data[0], &_curr_pos_sp.data[1]);
} else {
_curr_pos_sp(0) = _pos(0);
_curr_pos_sp(1) = _pos(1);
}
//only project setpoints if they are finite, else use current position
if (PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_curr_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
} else {
_curr_pos_sp(2) = _pos(2);
}
if (PX4_ISFINITE(_curr_pos_sp(0)) && if (PX4_ISFINITE(_curr_pos_sp(0)) &&
PX4_ISFINITE(_curr_pos_sp(1)) && PX4_ISFINITE(_curr_pos_sp(1)) &&
src/modules/navigator/mission_block.cpp
+2 -2
View File
@@ -699,8 +699,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
/* use current position */ /* use current position */
if (at_current_location) { if (at_current_location) {
item->lat = _navigator->get_global_position()->lat; item->lat = NAN; //descend at current position
item->lon = _navigator->get_global_position()->lon; item->lon = NAN; //descend at current position
item->yaw = _navigator->get_local_position()->yaw; item->yaw = _navigator->get_local_position()->yaw;
} else { } else {