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

FW use loiter to achieve waypoint altitude

Browse Source 
- fixes #5061
This commit is contained in:
Daniel Agar
2016-11-05 10:30:36 +01:00
committed by Lorenz Meier
parent 39cc75b4a0
commit a8369c6ac8
2 changed files with 33 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+1 -1
View File
@@ -1032,7 +1032,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !_l1_control.circle_mode()) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
src/modules/navigator/mission_block.cpp
+32
View File
@@ -169,6 +169,38 @@ MissionBlock::is_mission_item_reached()
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
/* close to waypoint, but altitude error greater than twice acceptance */
if ((dist >= 0.0f)
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
curr_sp->loiter_radius = _navigator->get_loiter_radius();
curr_sp->loiter_direction = 1;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
/* restore SETPOINT_TYPE_POSITION */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */
if ((dist >= 0.0f)
&& (dist_z < _navigator->get_loiter_radius())
&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
}
}
}
}
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {