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

[Mission]: Remove RTL logic from mission. Will be placed in a separate module and clean up mission interface.

Browse Source 
- Removed _mission_waypoints_changed in mission mode.
- Remove execution mode from mission
- mission mode uses own global position subscription instead of data from navigator.
- remove RTL type logic from navigator, will be set in the RTL mode itself later
- Remove unnecessary APIs from mission mode
- Make additional internal states to check if the mode is active and the mission is valid.
- Split up mission check from general update_mission function. update_mission function should only be called when the mode is active.
This commit is contained in:
Konrad
2022-11-16 16:32:52 +01:00
parent eb88a1fc72
commit d8d2052b5f
4 changed files with 324 additions and 723 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+310 -569
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/mission.h
+6 -33
View File
@@ -77,8 +77,6 @@ public:
Mission(Navigator *navigator);
~Mission() override = default;
void onMissionUpdate(bool has_mission_items_changed) override;
void on_inactive() override;
void on_inactivation() override;
void on_activation() override;
@@ -86,32 +84,12 @@ public:
bool set_current_mission_index(uint16_t index);
bool land_start();
bool landing();
uint16_t get_land_start_index() const { return _land_start_index; }
bool get_land_start_available() const { return _land_start_index != _invalid_index; }
bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
bool get_mission_changed() const { return _mission_changed ; }
bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
double get_landing_start_lat() { return _land_start_pos.lat; }
double get_landing_start_lon() { return _land_start_pos.lon; }
float get_landing_start_alt() { return _land_start_pos.alt; }
double get_landing_lat() { return _land_pos.lat; }
double get_landing_lon() { return _land_pos.lon; }
float get_landing_alt() { return _land_pos.alt; }
float get_landing_loiter_rad() { return _landing_loiter_radius; }
void set_closest_item_as_current();
/**
* Set a new mission mode and handle the switching between the different modes
*
* For a list of the different modes refer to mission_result.msg
*/
void set_execution_mode(const uint8_t mode);
private:
void onMissionUpdate(bool has_mission_items_changed) override;
/**
* Update mission topic
*/
@@ -198,7 +176,7 @@ private:
/**
* Reset mission
*/
void reset_mission(struct mission_s &mission);
void reset_mission();
/**
* Returns true if we need to reset the mission (call this only when inactive)
@@ -220,13 +198,13 @@ private:
uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle tatus subscription */
float _landing_loiter_radius{0.f};
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<vehicle_global_position_s> _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
bool _is_current_planned_mission_item_valid{false};
bool _initialized_mission_checked{false};
bool _is_mission_valid{false};
bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
@@ -239,8 +217,6 @@ private:
bool _need_mission_reset{false};
bool _system_disarmed_while_inactive{false};
bool _mission_waypoints_changed{false};
bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
enum work_item_type {
@@ -252,7 +228,4 @@ private:
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};
};
src/modules/navigator/navigator.h
-16
View File
@@ -272,26 +272,10 @@ public:
void set_mission_failure_heading_timeout();
bool is_planned_mission() const { return _navigation_mode == &_mission; }
bool on_mission_landing() { return _mission.landing(); }
bool start_mission_landing() { return _mission.land_start(); }
bool get_mission_start_land_available() { return _mission.get_land_start_available(); }
int get_mission_landing_index() { return _mission.get_land_start_index(); }
double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); }
double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); }
float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); }
double get_mission_landing_lat() { return _mission.get_landing_lat(); }
double get_mission_landing_lon() { return _mission.get_landing_lon(); }
float get_mission_landing_alt() { return _mission.get_landing_alt(); }
float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
// RTL
bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; }
src/modules/navigator/navigator_main.cpp
+8 -105
View File
@@ -548,7 +548,8 @@ void Navigator::run()
/* find NAV_CMD_DO_LAND_START in the mission and
* use MAV_CMD_MISSION_START to start the mission there
*/
if (_mission.land_start()) {
uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED};
if (_mission.get_land_start_available()) {
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
vcmd.param1 = _mission.get_land_start_index();
@@ -556,9 +557,10 @@ void Navigator::run()
} else {
PX4_WARN("planned mission landing not available");
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED;
}
publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
publish_vehicle_command_ack(cmd, result);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
@@ -652,7 +654,6 @@ void Navigator::run()
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL);
navigation_mode_new = &_mission;
break;
@@ -662,108 +663,10 @@ void Navigator::run()
navigation_mode_new = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
_pos_sp_triplet_published_invalid_once = false;
const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
switch (_rtl.get_rtl_type()) {
case RTL::RTL_TYPE_MISSION_LANDING:
case RTL::RTL_TYPE_CLOSEST:
if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER
&& _rtl.getShouldEngageMissionForLanding()) {
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
if (_vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& !get_land_detected()->landed) {
start_mission_landing();
}
navigation_mode_new = &_mission;
} else {
navigation_mode_new = &_rtl;
}
break;
case RTL::RTL_TYPE_MISSION_LANDING_REVERSED:
if (_mission.get_land_start_available() && !get_land_detected()->landed) {
// the mission contains a landing spot
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
if (_navigation_mode != &_mission) {
if (_navigation_mode == nullptr) {
// switching from an manual mode, go to landing if not already landing
if (!on_mission_landing()) {
start_mission_landing();
}
} else {
// switching from an auto mode, continue the mission from the closest item
_mission.set_closest_item_as_current();
}
}
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
"RTL Mission activated, continue mission");
}
navigation_mode_new = &_mission;
} else {
// fly the mission in reverse if switching from a non-manual mode
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
(! _mission.get_mission_finished()) &&
(!get_land_detected()->landed)) {
// determine the closest mission item if switching from a non-mission mode, and we are either not already
// mission mode or the mission waypoints changed.
// The seconds condition is required so that when no mission was uploaded and one is available the closest
// mission item is determined and also that if the user changes the active mission index while rtl is active
// always that waypoint is tracked first.
if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
_mission.set_closest_item_as_current();
}
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
"RTL Mission activated, fly mission in reverse");
}
navigation_mode_new = &_mission;
} else {
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
"RTL Mission activated, fly to home");
}
navigation_mode_new = &_rtl;
}
}
break;
default:
if (rtl_activated) {
mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
}
navigation_mode_new = &_rtl;
break;
}
break;
}
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_rtl;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;