fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: Wrap get time inside into function call and set time inside to zero

Browse Source 
This ensures that the dual-use of the pitch_min / time_inside field is handled
properly between takeoff and non-takeoff items. Flight tested in SITL.
This commit is contained in:
Lorenz Meier 2016-10-23 11:40:08 +02:00 committed by Lorenz Meier
parent 5ddd69c872
commit 05bc9acfb7
4 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/navigator/mission.cpp
View File

@ -519,7 +519,7 @@ Mission::set_mission_items()
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
_mission_item.time_inside = 0.0f;
}
/* if we just did a takeoff navigate to the actual waypoint now */
@ -548,6 +548,7 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
_mission_item.time_inside = 0.0f;
}
}
@ -581,7 +582,7 @@ Mission::set_mission_items()
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
_mission_item.time_inside = 0.0f;
}
/* transition to MC */
@ -620,7 +621,7 @@ Mission::set_mission_items()
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
_mission_item.time_inside = 0.0f;
}
/* we just moved to the landing waypoint, now descend */
@ -687,9 +688,7 @@ Mission::set_mission_items()
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) {
/* try to process next mission item */
if (has_next_position_item) {
@ -806,7 +805,7 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss
copy_positon_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current));
mission_item->altitude_is_relative = false;
mission_item->autocontinue = true;
mission_item->time_inside = 0;
mission_item->time_inside = 0.0f;
mission_item->yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
@ -854,7 +853,7 @@ Mission::heading_sp_update()
}
/* set yaw angle for the waypoint if a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) {
// XXX: should actually be param4 from mission item
// at the moment it will just keep the heading it has
//_mission_item.yaw = _on_arrival_yaw;

3
src/modules/navigator/mission_block.cpp
View File

@ -325,7 +325,8 @@ MissionBlock::is_mission_item_reached()
}
/* check if the MAV was long enough inside the waypoint orbit */
if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
// exit xtrack location
if (_mission_item.loiter_exit_xtrack &&

2
src/modules/navigator/navigator.h
View File

@ -215,6 +215,8 @@ public:
bool abort_landing();
static float get_time_inside(struct mission_item_s& item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */

4
src/modules/navigator/rtl.cpp
View File

@ -271,8 +271,8 @@ RTL::set_rtl_item()
_navigator->set_can_loiter_at_sp(true);
if (autoland && (_mission_item.time_inside > FLT_EPSILON)) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)Navigator::get_time_inside(_mission_item));
} else {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");