forked from Archive/PX4-Autopilot
navigator: loiter fixes
This commit is contained in:
parent
39454ca99d
commit
c5a5604ae9
|
@ -70,11 +70,10 @@ bool
|
|||
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
/* set loiter item, don't reuse an existing position setpoint */
|
||||
return set_loiter_item(false, pos_sp_triplet);;
|
||||
return set_loiter_item(pos_sp_triplet);
|
||||
}
|
||||
|
||||
void
|
||||
Loiter::on_inactive()
|
||||
{
|
||||
}
|
||||
|
||||
|
|
|
@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
|
|||
"#audio: onboard mission running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_ONBOARD;
|
||||
_navigator->set_is_in_loiter(false);
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
/* try setting offboard mission item */
|
||||
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
|
||||
|
@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
|
|||
"#audio: offboard mission running");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_OFFBOARD;
|
||||
_navigator->set_is_in_loiter(false);
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
} else {
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(),
|
||||
|
@ -253,9 +253,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
|
|||
"#audio: no mission available");
|
||||
}
|
||||
_mission_type = MISSION_TYPE_NONE;
|
||||
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
|
||||
|
||||
bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
|
||||
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
|
||||
set_loiter_item(pos_sp_triplet);
|
||||
reset_mission_item_reached();
|
||||
report_mission_finished();
|
||||
}
|
||||
|
|
|
@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
|
|||
}
|
||||
|
||||
bool
|
||||
MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
|
||||
{
|
||||
if (_navigator_priv->get_is_in_loiter()) {
|
||||
/* already loitering, bail out */
|
||||
return false;
|
||||
}
|
||||
|
||||
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
|
||||
/* leave position setpoint as is */
|
||||
} else {
|
||||
/* don't change setpoint if 'can_loiter_at_sp' flag set */
|
||||
if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
|
||||
/* use current position */
|
||||
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
|
||||
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
|
||||
pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
|
||||
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
|
||||
|
||||
_navigator_priv->set_can_loiter_at_sp(true);
|
||||
}
|
||||
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
|
||||
pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
|
||||
pos_sp_triplet->current.loiter_direction = 1;
|
||||
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|
||||
|| pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
|
||||
|| pos_sp_triplet->current.loiter_direction != 1
|
||||
|| pos_sp_triplet->previous.valid
|
||||
|| !pos_sp_triplet->current.valid
|
||||
|| pos_sp_triplet->next.valid) {
|
||||
/* position setpoint triplet should be updated */
|
||||
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
|
||||
pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
|
||||
pos_sp_triplet->current.loiter_direction = 1;
|
||||
|
||||
_navigator_priv->set_is_in_loiter(true);
|
||||
return true;
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->next.valid = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
@ -90,11 +90,10 @@ public:
|
|||
/**
|
||||
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
|
||||
*
|
||||
* @param true if the current position setpoint should be re-used
|
||||
* @param the position setpoint triplet to set
|
||||
* @return true if setpoint has changed
|
||||
*/
|
||||
bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
|
||||
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
|
||||
|
||||
bool _waypoint_position_reached;
|
||||
bool _waypoint_yaw_reached;
|
||||
|
|
|
@ -102,7 +102,7 @@ public:
|
|||
/**
|
||||
* Setters
|
||||
*/
|
||||
void set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
|
||||
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
|
||||
|
||||
/**
|
||||
* Getters
|
||||
|
@ -113,7 +113,7 @@ public:
|
|||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
int get_offboard_mission_sub() { return _offboard_mission_sub; }
|
||||
Geofence& get_geofence() { return _geofence; }
|
||||
bool get_is_in_loiter() { return _is_in_loiter; }
|
||||
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
|
||||
float get_loiter_radius() { return _param_loiter_radius.get(); }
|
||||
float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
|
||||
int get_mavlink_fd() { return _mavlink_fd; }
|
||||
|
@ -161,7 +161,7 @@ private:
|
|||
|
||||
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
|
||||
|
||||
bool _is_in_loiter; /**< flags if current position SP can be used to loiter */
|
||||
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
|
||||
bool _update_triplet; /**< flags if position SP triplet needs to be published */
|
||||
|
||||
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
|
||||
|
|
|
@ -347,7 +347,7 @@ Navigator::task_main()
|
|||
case NAVIGATION_STATE_ALTCTL:
|
||||
case NAVIGATION_STATE_POSCTL:
|
||||
_navigation_mode = nullptr;
|
||||
_is_in_loiter = false;
|
||||
_can_loiter_at_sp = false;
|
||||
break;
|
||||
case NAVIGATION_STATE_AUTO_MISSION:
|
||||
_navigation_mode = &_mission;
|
||||
|
@ -365,7 +365,7 @@ Navigator::task_main()
|
|||
case NAVIGATION_STATE_TERMINATION:
|
||||
default:
|
||||
_navigation_mode = nullptr;
|
||||
_is_in_loiter = false;
|
||||
_can_loiter_at_sp = false;
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
|
@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
}
|
||||
|
||||
set_previous_pos_setpoint(pos_sp_triplet);
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_CLIMB: {
|
||||
|
@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
|
||||
(int)(climb_alt - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
|
@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
|
@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(true);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
|
||||
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
|
||||
break;
|
||||
|
@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
|
|||
_mission_item.autocontinue = true;
|
||||
_mission_item.origin = ORIGIN_ONBOARD;
|
||||
|
||||
_navigator->set_is_in_loiter(false);
|
||||
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
|
||||
break;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue