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

Removed is_rotor_wing, replaced with vehicle_type

Browse Source
This commit is contained in:
Timothy Scott
2019-06-11 12:54:22 +02:00
committed by Julian Oes
parent 2ca40bfc65
commit a134da6e12
21 changed files with 150 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+12 -8
View File
@@ -149,7 +149,7 @@ MissionBlock::is_mission_item_reached()
&dist_xy, &dist_z);
/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
if (!_navigator->get_vstatus()->is_rotary_wing &&
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;
@@ -183,7 +183,7 @@ MissionBlock::is_mission_item_reached()
}
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
/* We want to avoid the edge case where the acceptance radius is bigger or equal than
* the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
@@ -213,7 +213,7 @@ MissionBlock::is_mission_item_reached()
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
} 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
@@ -231,7 +231,7 @@ MissionBlock::is_mission_item_reached()
_time_first_inside_orbit = 0;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
@@ -297,7 +297,8 @@ MissionBlock::is_mission_item_reached()
}
/* for vtol back transition calculate acceptance radius based on time and ground speed */
if (_mission_item.vtol_back_transition && !_navigator->get_vstatus()->is_rotary_wing) {
if (_mission_item.vtol_back_transition
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx +
_navigator->get_local_position()->vy * _navigator->get_local_position()->vy);
@@ -328,12 +329,14 @@ MissionBlock::is_mission_item_reached()
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if ((_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw)))
if ((_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw)))
|| ((_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) && _mission_item.force_heading
&& PX4_ISFINITE(_mission_item.yaw))) {
/* check course if defined only for rotary wing except takeoff */
float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
float cog = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
? _navigator->get_global_position()->yaw : atan2f(
_navigator->get_global_position()->vel_e,
_navigator->get_global_position()->vel_n);
@@ -474,7 +477,8 @@ MissionBlock::issue_command(const mission_item_s &item)
float
MissionBlock::get_time_inside(const mission_item_s &item) const
{
if ((item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->get_vstatus()->is_rotary_wing) ||
if ((item.nav_cmd == NAV_CMD_WAYPOINT
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ||
item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item.nav_cmd == NAV_CMD_DELAY) {