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

FW move altitude first order hold (FOH) and loiter to position special cases from navigator to position controller

Browse Source 
Co-authored-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-06 16:57:12 -04:00
committed by GitHub
co-authored by RomanBapst
parent 9838e340e0
commit c01fabaf11
7 changed files with 125 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+19 -86
View File
@@ -150,40 +150,6 @@ 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()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_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()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -218,6 +184,7 @@ MissionBlock::is_mission_item_reached()
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
@@ -239,50 +206,24 @@ MissionBlock::is_mission_item_reached()
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
if (dist >= 0.f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
// first check if the altitude setpoint is the mission setpoint
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
_waypoint_position_reached = true;
if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
// check if the initial loiter has been accepted
dist_xy = -1.0f;
dist_z = -1.0f;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
_waypoint_yaw_reached = false;
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
}
@@ -365,7 +306,6 @@ MissionBlock::is_mission_item_reached()
}
/* Check if the waypoint and the requested yaw setpoint. */
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@@ -615,20 +555,13 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TO_ALT:
// initially use current altitude, and switch to mission item altitude once in loiter position
if (_navigator->get_loiter_min_alt() > 0.0f) { // ignore _param_loiter_min_alt if smaller than 0 (-1)
sp->alt = math::max(_navigator->get_global_position()->alt,
_navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
} else {
sp->alt = _navigator->get_global_position()->alt;
}
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
// FALLTHROUGH
case NAV_CMD_LOITER_UNLIMITED:
// FALLTHROUGH
case NAV_CMD_LOITER_TO_ALT:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
break;