fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 05:13:33 +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-13 10:04:26 +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.cpp
+15 -13
View File
@@ -239,7 +239,8 @@ Mission::on_active()
}
/* see if we need to update the current yaw heading */
if (!_param_mis_mnt_yaw_ctl.get() && (_navigator->get_vstatus()->is_rotary_wing)
if (!_param_mis_mnt_yaw_ctl.get()
&& (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
&& !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
@@ -321,7 +322,7 @@ Mission::set_execution_mode(const uint8_t mode)
case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD:
if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
// command a transition if in vtol mc mode
if (_navigator->get_vstatus()->is_rotary_wing &&
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
_navigator->get_vstatus()->is_vtol &&
!_navigator->get_land_detected()->landed) {
@@ -703,7 +704,7 @@ Mission::set_mission_items()
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& _navigator->get_vstatus()->is_rotary_wing) {
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -719,7 +720,7 @@ Mission::set_mission_items()
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
if (_navigator->get_vstatus()->is_rotary_wing) {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
/* haven't transitioned yet, trigger vtol takeoff logic below */
_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
@@ -750,7 +751,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
_work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT &&
_navigator->get_vstatus()->is_rotary_wing &&
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
!_navigator->get_land_detected()->landed) {
/* check if the vtol_takeoff waypoint is on top of us */
@@ -806,7 +807,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_vstatus()->is_rotary_wing
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_navigator->get_land_detected()->landed) {
set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
@@ -929,7 +930,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& _navigator->get_vstatus()->is_rotary_wing
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
@@ -957,7 +958,7 @@ Mission::set_mission_items()
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& new_work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_vstatus()->is_rotary_wing
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_navigator->get_land_detected()->landed
&& pos_sp_triplet->current.valid) {
@@ -1061,7 +1062,7 @@ Mission::set_mission_items()
bool
Mission::do_need_vertical_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing) {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
@@ -1099,7 +1100,7 @@ Mission::do_need_vertical_takeoff()
bool
Mission::do_need_move_to_land()
{
if (_navigator->get_vstatus()->is_rotary_wing
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
@@ -1114,7 +1115,8 @@ Mission::do_need_move_to_land()
bool
Mission::do_need_move_to_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
@@ -1253,7 +1255,7 @@ Mission::altitude_sp_foh_update()
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt)
|| !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
_navigator->get_vstatus()->is_rotary_wing) {
_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return;
}
@@ -1720,7 +1722,7 @@ Mission::index_closest_mission_item() const
if (item_contains_position(missionitem)) {
// do not consider land waypoints for a fw
if (!((missionitem.nav_cmd == NAV_CMD_LAND) &&
(!_navigator->get_vstatus()->is_rotary_wing) &&
(_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
(!_navigator->get_vstatus()->is_vtol))) {
float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon,
get_absolute_altitude_for_item(missionitem),
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) {
src/modules/navigator/mission_feasibility_checker.cpp
+2 -1
View File
@@ -83,7 +83,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
// VTOL always respects rotary wing feasibility
if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) {
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| _navigator->get_vstatus()->is_vtol) {
failed = failed || !checkRotarywing(mission, home_alt);
} else {
src/modules/navigator/navigator_main.cpp
+5 -5
View File
@@ -761,7 +761,7 @@ Navigator::get_acceptance_radius()
float
Navigator::get_default_altitude_acceptance_radius()
{
if (!get_vstatus()->is_rotary_wing) {
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return _param_nav_fw_alt_rad.get();
} else {
@@ -781,7 +781,7 @@ Navigator::get_default_altitude_acceptance_radius()
float
Navigator::get_altitude_acceptance_radius()
{
if (!get_vstatus()->is_rotary_wing) {
if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
@@ -797,7 +797,7 @@ float
Navigator::get_cruising_speed()
{
/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
if (_vstatus.is_rotary_wing) {
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) {
return _mission_cruising_speed_mc;
@@ -818,7 +818,7 @@ Navigator::get_cruising_speed()
void
Navigator::set_cruising_speed(float speed)
{
if (_vstatus.is_rotary_wing) {
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_mission_cruising_speed_mc = speed;
} else {
@@ -1069,7 +1069,7 @@ bool
Navigator::force_vtol()
{
return _vstatus.is_vtol &&
(!_vstatus.is_rotary_wing || _vstatus.in_transition_to_fw)
(_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw)
&& _param_nav_force_vt.get();
}
src/modules/navigator/rtl.cpp
+2 -1
View File
@@ -318,7 +318,8 @@ RTL::advance_rtl()
_rtl_state = RTL_STATE_LAND;
}
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
if (_navigator->get_vstatus()->is_vtol
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
_rtl_state = RTL_STATE_TRANSITION_TO_MC;
}