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

mission: publish sub flight mode for flightmode manager

Browse Source
This commit is contained in:
Alessandro Simovic 2022-04-08 10:47:27 +02:00
parent f08268adbc
commit 2bf072e5cd
6 changed files with 58 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/NavigatorMissionItem.msg
View File

@ -6,6 +6,11 @@ uint16 sequence_current # Sequence of the current mission item
uint16 nav_cmd
uint8 MISSION_SUBMODE_NONE = 0
uint8 MISSION_SUBMODE_PRECLAND = 1
uint8 nav_sub_cmd # sub-command, for example AUTO_PRECLAND in main command AUTO_MISSION
float32 latitude
float32 longitude

14
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -107,6 +107,7 @@ void FlightModeManager::Run()
_vehicle_control_mode_sub.update();
_vehicle_land_detected_sub.update();
_vehicle_status_sub.update();
_navigator_mission_item_sub.update();
start_flight_task();
@ -139,6 +140,14 @@ void FlightModeManager::start_flight_task()
bool task_failure = false;
bool should_disable_task = true;
const bool land_should_be_precland = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
||
_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
&& _param_rtl_pld_md.get() > 0;
const bool precland_mission_item_active = _vehicle_status_sub.get().nav_state ==
vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION &&
_navigator_mission_item_sub.get().nav_sub_cmd == navigator_mission_item_s::MISSION_SUBMODE_PRECLAND;
// Do not run any flight task for VTOLs in fixed-wing mode
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
switchTask(FlightTaskIndex::None);
@ -164,11 +173,8 @@ void FlightModeManager::start_flight_task()
task_failure = true;
}
} else if (land_should_be_precland || precland_mission_item_active) {
// Take-over landing from navigator if precision landing is enabled
} else if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL ||
_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)
&& _param_rtl_pld_md.get() > 0) {
should_disable_task = false;
if (switchTask(FlightTaskIndex::AutoPrecisionLanding) != FlightTaskError::NoError) {

2
src/modules/flight_mode_manager/FlightModeManager.hpp
View File

@ -44,6 +44,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/navigator_mission_item.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/takeoff_status.h>
@ -150,6 +151,7 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<navigator_mission_item_s> _navigator_mission_item_sub{ORB_ID(navigator_mission_item)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};

12
src/modules/flight_mode_manager/tasks/AutoPrecisionLanding/FlightTaskAutoPrecisionLanding.cpp
View File

@ -121,20 +121,24 @@ bool FlightTaskAutoPrecisionLanding::update()
void
FlightTaskAutoPrecisionLanding::run_state_start()
{
_position_setpoint = _target; // Follow navigator triplet
// In this state simply track the navigator setpoint triplet
// This ensures that the behaviour for precision landing during RTL / LAND
// is the same as without precision landing.
// This flight task will generate independent setpoints once the vehicle
// is in the vertical landing phase.
_position_setpoint = _target;
// check if target visible and go to horizontal approach directly
if (switch_to_state_horizontal_approach()) {
// If target visible and go to horizontal approach directly
return;
} else if ((PrecLandMode)_param_rtl_pld_md.get() == PrecLandMode::Opportunistic) {
// could not see the target immediately, so just fall back to normal landing
switch_to_state_fallback();
} else if (_type == WaypointType::land) {
// Navigator already entered land stage. Take over with precision landing
// This is where the vehicle starts to behave differently than in regular land!
switch_to_state_search();
}
}

34
src/modules/navigator/mission.cpp
View File

@ -746,7 +746,7 @@ Mission::set_mission_items()
user_feedback_done = true;
}
publish_navigator_mission_item(); // for logging
publish_navigator_mission_item();
_navigator->set_position_setpoint_triplet_updated();
return;
@ -967,6 +967,20 @@ Mission::set_mission_items()
// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
pos_sp_triplet->previous.valid = false;
} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
}
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND &&
new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND;
}
}
/* ignore yaw for landing items */
@ -1100,8 +1114,10 @@ Mission::set_mission_items()
} else {
// The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items)
// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
}
// ELSE: The current position setpoint stays unchanged.
}
@ -1119,6 +1135,13 @@ Mission::set_mission_items()
/* set current work item type */
_work_item_type = new_work_item_type;
if (new_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) {
_mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_PRECLAND;
} else {
_mission_sub_mode = navigator_mission_item_s::MISSION_SUBMODE_NONE;
}
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
@ -1171,7 +1194,7 @@ Mission::set_mission_items()
}
}
publish_navigator_mission_item(); // for logging
publish_navigator_mission_item();
_navigator->set_position_setpoint_triplet_updated();
}
@ -1418,7 +1441,7 @@ Mission::do_abort_landing()
_navigator->get_position_setpoint_triplet()->next.lon = (double)NAN;
_navigator->get_position_setpoint_triplet()->next.alt = NAN;
publish_navigator_mission_item(); // for logging
publish_navigator_mission_item();
_navigator->set_position_setpoint_triplet_updated();
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t",
@ -1862,6 +1885,7 @@ void Mission::publish_navigator_mission_item()
navigator_mission_item.instance_count = _navigator->mission_instance_count();
navigator_mission_item.sequence_current = _current_mission_index;
navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
navigator_mission_item.nav_sub_cmd = _mission_sub_mode;
navigator_mission_item.latitude = _mission_item.lat;
navigator_mission_item.longitude = _mission_item.lon;
navigator_mission_item.altitude = _mission_item.altitude;

5
src/modules/navigator/mission.h
View File

@ -288,9 +288,12 @@ private:
WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */
WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */
WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION
WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION,
WORK_ITEM_TYPE_PRECISION_LAND
} _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */
uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
bool _execution_mode_changed{false};
uint8_t _mission_sub_mode{navigator_mission_item_s::MISSION_SUBMODE_NONE};
};