fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 19:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Fix RTL state sequence

Browse Source
This commit is contained in:
Lorenz Meier
2017-04-09 23:16:50 +02:00
parent 83cf4fb16e
commit 85b074f8d8
2 changed files with 6 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+6 -23
View File
@@ -60,7 +60,6 @@
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_rtl_start_lock(false),
_param_return_alt(this, "RTL_RETURN_ALT", false),
_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
_param_descend_alt(this, "RTL_DESCEND_ALT", false),
@@ -94,8 +93,6 @@ RTL::get_rtl_altitude()
void
RTL::on_activation()
{
/* reset starting point so we override what the triplet contained from the previous navigation state */
_rtl_start_lock = false;
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
@@ -139,10 +136,6 @@ RTL::set_rtl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
if (!_rtl_start_lock) {
set_previous_pos_setpoint();
}
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
@@ -158,7 +151,7 @@ RTL::set_rtl_item()
// we are close to home, limit climb to min
if (home_dist < _param_rtl_min_dist.get()) {
climb_alt = _navigator->get_home_position()->alt + _param_min_loiter_alt.get();
climb_alt = _navigator->get_home_position()->alt + _param_descend_alt.get();
}
_mission_item.lat = _navigator->get_global_position()->lat;
@@ -194,18 +187,10 @@ RTL::set_rtl_item()
_mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
// use current heading to home
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
@@ -218,8 +203,6 @@ RTL::set_rtl_item()
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
@@ -297,8 +280,8 @@ RTL::set_rtl_item()
}
case RTL_STATE_LAND: {
_mission_item.yaw = _navigator->get_home_position()->yaw;
set_land_item(&_mission_item, false);
_mission_item.yaw = _navigator->get_home_position()->yaw;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
src/modules/navigator/rtl.h
-2
View File
@@ -94,8 +94,6 @@ private:
RTL_STATE_LANDED,
} _rtl_state;
bool _rtl_start_lock;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_min_loiter_alt;
control::BlockParamFloat _param_descend_alt;