Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

reset current setpoint when entering RTL

Browse Source
This commit is contained in:
Andreas Antener
2016-03-14 22:38:27 +01:00
committed by Lorenz Meier
parent f3f01ad960
commit 4847023cad
3 changed files with 26 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+18
View File
@@ -503,6 +503,24 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
item->origin = ORIGIN_ONBOARD;
}
void
MissionBlock::set_current_position_item(struct mission_item_s *item)
{
item->nav_cmd = NAV_CMD_WAYPOINT;
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude_is_relative = false;
item->altitude = _navigator->get_global_position()->alt;
item->yaw = NAN;
item->loiter_radius = _navigator->get_loiter_radius();
item->loiter_direction = 1;
item->acceptance_radius = _navigator->get_acceptance_radius();
item->time_inside = 0.0f;
item->pitch_min = 0.0f;
item->autocontinue = true;
item->origin = ORIGIN_ONBOARD;
}
void
MissionBlock::set_idle_item(struct mission_item_s *item)
{
src/modules/navigator/mission_block.h
+2
View File
@@ -108,6 +108,8 @@ protected:
*/
void set_land_item(struct mission_item_s *item, bool at_current_location);
void set_current_position_item(struct mission_item_s *item);
/**
* Set idle mission item
*/
src/modules/navigator/rtl.cpp
+6 -2
View File
@@ -85,6 +85,12 @@ RTL::on_inactive()
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);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
@@ -96,7 +102,6 @@ RTL::on_activation()
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
_rtl_start_lock = false;
/* otherwise go straight to return */
} else {
@@ -104,7 +109,6 @@ RTL::on_activation()
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
_rtl_start_lock = false;
}
}