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

navigator: add ability to process custom action waypoints

Browse Source 
A custom action waypoint is an action defined in a waypoint that is scripted and processed outside the flight controller, usually in a mission computer process. The ability to process that action in the mission computer should be coded by the user, or instead using the MAVSDK custom_action plugin implementation (a full-scale example can be seen in the integration tests directory). The custom action command associated, for now, is the MAV_CMD_WAYPOINT_USER_1 but will eventually be replaced with an appropriate MAV_CMD to the purpose. The modifications in the navigator module allow to block the navigator for an amount of time defined on param3 of the MAV_CMD_WAYPONT_USER_1 mission item. The navigator expects to receive a progress status of the action being process, which in the case of not received, will result on the mission continuing to the next waypoint, and a COMMAND_CANCEL to be broadcasted to cancel that same command. The mission will continue to the next waypoint as soon as an ACCEPTED ACK gets sent from the component processing the custom action.
This commit is contained in:
TSC21 2021-10-25 13:01:27 +02:00
parent c109a7c6ff
commit 13016bae1f
No known key found for this signature in database
GPG Key ID: 2724F5BF12FDC590
11 changed files with 213 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_command.msg
View File

@ -86,6 +86,7 @@ uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition
uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization
uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan
uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment
uint16 VEHICLE_CMD_NAV_WAYPOINT_USER_1 = 31000 # Currently used for processing custom action waypoints on component outside the FMU
uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.

1
msg/vehicle_command_ack.msg
View File

@ -5,6 +5,7 @@ uint8 VEHICLE_RESULT_DENIED = 2
uint8 VEHICLE_RESULT_UNSUPPORTED = 3
uint8 VEHICLE_RESULT_FAILED = 4
uint8 VEHICLE_RESULT_IN_PROGRESS = 5
uint8 VEHICLE_RESULT_CANCELLED = 6
uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0
uint16 ARM_AUTH_DENIED_REASON_NONE = 1

2
src/modules/mavlink/mavlink_mission.cpp
View File

@ -1537,6 +1537,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
case MAV_CMD_DO_SET_ROI_NONE:
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
case MAV_CMD_WAYPOINT_USER_1:
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
break;
@ -1629,6 +1630,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
case NAV_CMD_SET_CAMERA_ZOOM:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_DO_VTOL_TRANSITION:
case MAV_CMD_WAYPOINT_USER_1:
break;
default:

48
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -580,6 +580,31 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
_cmd_pub.publish(vehicle_command);
} else if (cmd_mavlink.command == MAV_CMD_WAYPOINT_USER_1) {
// Processes MAV_CMD_WAYPOINT_USER_1 as a custom action command to be
// processed by a mission computer. For more context, check the MAVSDK
// PR in this link: https://github.com/mavlink/MAVSDK/pull/1581
//
// Note: this command will be eventually replaced by the command proposed
// in https://github.com/mavlink/mavlink/pull/1516 or similar
//
// Note for this case, the command is sent from an external entity
// to trigger a custom action on the Mission Computer, so a timeout
// check is not handled by the autopilot, but should rather be handled
// by the entity sending this command
vehicle_command_s custom_action_cmd = vehicle_command;
custom_action_cmd.target_system = 0;
custom_action_cmd.target_component = 0; // Broadcast the command
custom_action_cmd.from_external = false;
PX4_DEBUG("received MAV_CMD_WAYPOINT_USER_1 command from %d/%d",
custom_action_cmd.source_system,
custom_action_cmd.source_component);
_cmd_pub.publish(custom_action_cmd);
send_ack = true;
} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
bool has_module = true;
@ -680,29 +705,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
break;
}
} else if (cmd_mavlink.command == MAV_CMD_WAYPOINT_USER_1) {
// Processes MAV_CMD_WAYPOINT_USER_1 as a custom action command to be
// processed by a mission computer. For more context, check the MAVSDK
// PR in this link: https://github.com/mavlink/MAVSDK/pull/1581
//
// Note: this command will be eventually replaced by the command proposed
// in https://github.com/mavlink/mavlink/pull/1516 or similar
//
// Note for this case, the command is sent from an external entity
// to trigger a custom action on the Mission Computer, so a timeout
// check is not handled by the autopilot, but should rather be handled
// by the entity sending this command
vehicle_command_s custom_action_cmd = vehicle_command;
custom_action_cmd.target_system = 0;
custom_action_cmd.target_component = 0; // Broadcast the command
custom_action_cmd.from_external = false;
PX4_DEBUG("received MAV_CMD_WAYPOINT_USER_1 command from %d/%d", custom_action_cmd.command,
custom_action_cmd.source_system,
custom_action_cmd.source_component);
_cmd_pub.publish(custom_action_cmd);
} else {
result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
}

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

@ -1029,6 +1029,23 @@ Mission::set_mission_items()
case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* If a custom action exists, set it and make it available to the navigator
main routine. Note that NAV_CMD_WAYPOINT_USER_1 is being used as the
command to associate a custom action to a waypoint, but will be replaced
by https://github.com/mavlink/mavlink/pull/1516 or similar */
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT_USER_1
&& !_navigator->get_in_custom_action()
&& _current_mission_index != _previous_custom_action_mission_index) {
custom_action_s custom_action{};
custom_action.id = _mission_item.params[0];
custom_action.timeout = _mission_item.params[2] * 1000000;
_navigator->set_custom_action(custom_action);
_navigator->set_in_custom_action();
PX4_INFO("Processing custom action #%d", custom_action.id);
}
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
@ -1132,7 +1149,25 @@ Mission::set_mission_items()
}
/* issue command if ready (will do nothing for position mission items) */
issue_command(_mission_item);
if (_mission_item.nav_cmd != NAV_CMD_WAYPOINT_USER_1) {
issue_command(_mission_item);
}
/* In the case of NAV_CMD_WAYPOINT_USER_1 mission items (for custom actions)
* it will not send the same MAV_CMD_WAYPOINT_USER_1 command if for some
* reason we left Mission mode and returned back to it. This avoids that it
* gets sent at least twice after getting back to Mission mode and before
* advancing to the next waypoint.
* (e.g. if a custom action command changes the FMU to another mode, this
* will avoid that the same gets sent again when the FMU changes back to
* Mission mode).
*/
if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT_USER_1
&& _current_mission_index != _previous_custom_action_mission_index) {
issue_command(_mission_item);
}
_previous_custom_action_mission_index = _current_mission_index;
/* set current work item type */
_work_item_type = new_work_item_type;

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

@ -249,6 +249,7 @@ private:
mission_s _mission {};
int32_t _current_mission_index{-1};
int32_t _previous_custom_action_mission_index{-1};
// track location of planned mission landing
bool _land_start_available{false};

1
src/modules/navigator/mission_block.cpp
View File

@ -108,6 +108,7 @@ MissionBlock::is_mission_item_reached()
case NAV_CMD_SET_CAMERA_MODE:
case NAV_CMD_SET_CAMERA_ZOOM:
case NAV_CMD_SET_CAMERA_FOCUS:
case NAV_CMD_WAYPOINT_USER_1:
return true;
case NAV_CMD_DO_VTOL_TRANSITION:

6
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -278,7 +278,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT_USER_1) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t",
(int)(i + 1),
@ -422,7 +423,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION &&
missionitem.nav_cmd != NAV_CMD_WAYPOINT_USER_1);
}
}

1
src/modules/navigator/navigation.h
View File

@ -101,6 +101,7 @@ enum NAV_CMD {
NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
NAV_CMD_CONDITION_GATE = 4501,
NAV_CMD_WAYPOINT_USER_1 = 31000,
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
};

26
src/modules/navigator/navigator.h
View File

@ -74,6 +74,7 @@
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_command_cancel.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
@ -88,6 +89,13 @@ using namespace time_literals;
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 9
struct custom_action_s {
int8_t id{-1};
uint64_t timeout;
bool timer_started;
uint64_t start_time;
};
class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
public:
@ -121,6 +129,7 @@ public:
void load_fence_from_file(const char *filename);
void publish_vehicle_cmd(vehicle_command_s *vcmd);
void publish_vehicle_cmd_cancel(vehicle_command_cancel_s *vcmd_cancel);
/**
* Generate an artificial traffic indication
@ -165,6 +174,7 @@ public:
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
struct vehicle_status_s *get_vstatus() { return &_vstatus; }
struct vehicle_command_ack_s *get_cmd_ack() { return &_vehicle_cmd_ack; }
PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
const vehicle_roi_s &get_vroi() { return _vroi; }
@ -273,6 +283,11 @@ public:
bool getMissionLandingInProgress() { return _mission_landing_in_progress; }
bool get_in_custom_action() { return _in_custom_action; }
void set_in_custom_action() { _in_custom_action = true; }
custom_action_s get_custom_action() { return _custom_action; }
void set_custom_action(const custom_action_s &custom_action) { _custom_action = custom_action; }
bool is_planned_mission() const { return _navigation_mode == &_mission; }
bool on_mission_landing() { return _mission.landing(); }
bool start_mission_landing() { return _mission.land_start(); }
@ -352,6 +367,7 @@ private:
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::Subscription _vehicle_cmd_ack_sub{ORB_ID(vehicle_command_ack)}; /**< vehicle command acks (onboard and offboard) */
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
@ -364,6 +380,7 @@ private:
uORB::Publication<vehicle_command_ack_s> _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_cancel_s> _vehicle_cmd_cancel_pub{ORB_ID(vehicle_command_cancel)};
// Subscriptions
home_position_s _home_pos{}; /**< home position for RTL */
@ -373,6 +390,7 @@ private:
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
vehicle_command_ack_s _vehicle_cmd_ack{}; /**< vehicle command_ack */
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
@ -395,6 +413,12 @@ private:
bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */
bool _mission_result_updated{false}; /**< flags if mission result has seen an update */
bool _in_custom_action{false}; /**< currently in a custom action **/
bool _custom_action_timeout{false}; /**> custom action timed out **/
custom_action_s _custom_action{}; /**< current custom action **/
uint64_t _custom_action_ack_last_time{0}; /**< last time an ack for the custom action command was received **/
bool _reset_custom_action{false}; /**< reset custom action status flag **/
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
@ -442,5 +466,7 @@ private:
void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
void reset_custom_action();
bool geofence_allows_position(const vehicle_global_position_s &pos);
};

115
src/modules/navigator/navigator_main.cpp
View File

@ -228,6 +228,89 @@ Navigator::run()
_position_controller_status_sub.update();
_home_pos_sub.update(&_home_pos);
if (_vehicle_cmd_ack_sub.update(&_vehicle_cmd_ack) &&
_vehicle_cmd_ack.command == vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT_USER_1) {
if (!_custom_action.timer_started && !_reset_custom_action) {
_custom_action.start_time = hrt_absolute_time();
_custom_action.timer_started = true;
}
if (_custom_action.timer_started && _custom_action_ack_last_time > 0) {
if ((hrt_absolute_time() - _custom_action_ack_last_time) < 1500000) {
if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED && !_reset_custom_action
&& !_custom_action_timeout) {
// This makes sure that the info is only printed once, even if multiple ACCEPTED ACKs are received
if (_custom_action.id != -1) {
mavlink_log_info(get_mavlink_log_pub(), "Custom action #%u finished successfully. Continuing mission...",
_custom_action.id);
}
reset_custom_action();
} else if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_FAILED && !_reset_custom_action
&& !_custom_action_timeout) {
// This makes sure that the warning is only printed once, even if multiple FAILED ACKs are received
if (_custom_action.id != -1) {
mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%u failed to be processed / executed. Continuing mission...",
_custom_action.id);
}
reset_custom_action();
} else if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_CANCELLED && !_reset_custom_action
&& !_custom_action_timeout) {
// This makes sure that the warning is only printed once, even if multiple CANCELLED ACKs are received
if (_custom_action.id != -1) {
mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%u cancelled. Continuing mission...",
_custom_action.id);
}
reset_custom_action();
}
} else {
mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%u progress timed out. Continuing mission...",
_custom_action.id);
// send message to cancel the action process on the external system
// processing the action. Note that the external system component
// should be identified as MAV_COMP_ID_ONBOARD_COMPUTER
vehicle_command_cancel_s vcmd_cancel = {};
vcmd_cancel.command = vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT_USER_1;
vcmd_cancel.target_system = 0;
vcmd_cancel.target_component = 0;
publish_vehicle_cmd_cancel(&vcmd_cancel);
reset_custom_action();
_custom_action_timeout = true;
}
}
_custom_action_ack_last_time = _reset_custom_action ? 0 : hrt_absolute_time();
_reset_custom_action = false;
}
if (_custom_action.timer_started && _custom_action.start_time > 0
&& (hrt_absolute_time() - _custom_action.start_time) >= _custom_action.timeout && _custom_action.timeout > 0) {
mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%u timed out. Continuing mission...",
_custom_action.id);
// send message to cancel the action process on the external system
// processing the action. Note that the external system component
// should be identified as MAV_COMP_ID_ONBOARD_COMPUTER
vehicle_command_cancel_s vcmd_cancel = {};
vcmd_cancel.command = vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT_USER_1;
vcmd_cancel.target_system = 0;
vcmd_cancel.target_component = 0;
publish_vehicle_cmd_cancel(&vcmd_cancel);
reset_custom_action();
_custom_action_timeout = true;
}
while (_vehicle_command_sub.updated()) {
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
vehicle_command_s cmd{};
@ -1525,6 +1608,13 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res
_vehicle_cmd_ack_pub.publish(command_ack);
}
void
Navigator::publish_vehicle_cmd_cancel(vehicle_command_cancel_s *vcmd_cancel)
{
vcmd_cancel->timestamp = hrt_absolute_time();
_vehicle_cmd_cancel_pub.publish(*vcmd_cancel);
}
void
Navigator::acquire_gimbal_control()
{
@ -1549,6 +1639,31 @@ Navigator::release_gimbal_control()
publish_vehicle_cmd(&vcmd);
}
void
Navigator::reset_custom_action()
{
// send cmd to get back to Mission mode if the custom action
// requested a mode change or in case of failure
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
vcmd.param1 = 1;
vcmd.param2 = 4; // PX4_CUSTOM_MAIN_MODE_AUTO
vcmd.param3 = 4; // PX4_CUSTOM_MAIN_MODE_AUTO
publish_vehicle_cmd(&vcmd);
// reset custom action timer
_custom_action.timer_started = false;
_custom_action.start_time = -1;
_custom_action.timeout = -1;
_in_custom_action = false;
_reset_custom_action = true;
// ID of -1 is read as an unset custom_action
_custom_action.id = -1;
}
bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
{
if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&