forked from Archive/PX4-Autopilot
Implement Pacakge delivery via Gripper during mission
This feature allows user to use a Gripper type pacakge delivery mechanism on a drone to trigger the delivery during a mission via the mission item `DO_GRIPPER`. This is a minimal change that is intended to have simplest pacakge delivery feature on PX4, however the future scope would extend this feature out of Navigator, and rather move towards a federated PX4 (flight-mode flexibility) architecture. But until then, this will serve the purpose. Update Tools/sitl_gazebo submodule to remove sdf file overwrite error - There was an error happening due to .sdf file being overwritten, it was caused by a wrongfully added. sdf file. - This update pulls in the PR commit: https://github.com/Auterion/sitl_gazebo/pull/147 Initial cut on supporing PAYLOAD_PLACE mission item Tidy and comment on navigation.h to clarify mission item definition - Convert vehicle command ack subscription data type to SubscriptionData, to not care about having a dedicated struct for copying the latest data - Tidy and comment on navigation.h to clarify the definition of mission_item_s, which is confusing as it is an intergration of MAVLink Standard into PX4's internal Mission Item structure Rename mission_block's mission item reached function & cleanup navigator - Isolated Handle Vehicle Commands function inside the Navigator - Rename mission_block's mission item reached function to 'reached or completed', as the navigation command can also be an action (e.g. DO_SET_SERVO, which doesn't make sense to refer to as 'reached' when we have successfully done executed the command) Include MAVLink PR commit to include payload_drop message More changes to add payload_drop MAVLink message support - Comitting for testing purposes Add mission item payload_drop to vehicle command payload drop link - Now with a mission item with the nav_cmd set to 'payload drop', the appropriate 'payload drop' vehicle command will be issued Make Payload drop executable via Mission Plan Implement payload_drop module to simulate payload delivery - Simple module that acknowledges the payload drop vehicle command after certain time, to simulate a successful delivery Additional changes - payload drop module not working yet - Need to do more thread stuff to make it work :( Fix Payload Drop enum mismatch in vehicle_command enums - First functional Payload Drop Implementation MVP - Simple Ack & resuming mission from Navigator tested successfully Hold the position while executing payload drop mission item - Still the position hold is not solid, maybe I am missing something in the position setpoint part and all the internal implications of Navigator :( Add DO_WINCH command support Some fixes after rebase on develop branch - Some missed brackets - Some comment edits, etc Add DO_WINCH command support - Still has a problem of flying away from the waypoint while the DO_WINCH is being executed, probably position setpoint related stuff :( Apply braking of the vehicle for DO_WINCH command - Copies the behavior of NAV_CMD_DELAY, which executes a smooth, braking behavior when executing the delay because of the braking condition in `set_mission_items` function - This will not apply to Fixed wings - The payload deploy getting triggered may be too early, as right now as soon as the vehicle approaches the waypoint within the acceptance threshold, the payload gets deployed Add DO_GRIPPER support Implement Gripper actual Hardware triggering support - Currently not working, possibly in the mixer there's a bug - Implemented the publishing of actuator_controls_1 uORB topic - Implemented the test command for the payload_drop module, to test the grpiper functionality - Edited px4board file to include the payload_drop module - Added Holybro X500 V2 airframe file, to enable testing on X500 V2 - Created new Quad X Payload Delivery mixer, which maps the actuator controls 1 topic's data into the MAIN pin 5 output Make Payload Drop Gripper Work - Initialization of the Gripper position to CLOSED on Constructor of the payload_drop module - Setting the OPEN and CLOSED value to the appropriate actuator controls input Set vehicle_command_ack message's timestamp correctly - By not setting the timestamp, the ack commands were not correctly graphed in PlotJuggler! Rename payload drop module to payload deliverer - I think it's a more complex name (harder to type), but more generic Add Gripper class (WIP) Add Gripper class functionalities - Add gripper uORB message - Add gripper state machine Use Gripper class as main interface in payload_deliverer - Utilizes Gripper class functions for doing Gripper functionality Remove mixer based package delivery trigger logic - Remove custom mixer files that mapped actuator controls to outputs statically Additional improvements of the payload_deliverer Fix payload_deliverer module not starting - _task_id wasn't geting set appropriately in task_spawn function, which led to runtime failure Add Gripper Function to mixer_module - Still not showing up as function mapping in QGC, needs fix Add parameters to control gripper behavior - Now user can enable / disable gripper - Also select which type of gripper to use Applying review from nuno Remove timeout fetching from mission item and use gripper's timeout - Previously, it was planned to use a custom DO_GRIPPER and DO_WINCH MAVLink message definitions with information on timeout, but since now we are using original message definition, only relevant timeout information is defined in the payload_deliverer class - This change brings in the timeout parameter to the Navigator, which then sets the timeout in the mission_block class level, which then processes the timeout logic Make payload deployment work for Allmend test :P Support gripper open/close test commands in payload_deliverer Move enum definition for GRIPPER_ACTION to vehicle_command.msg Remove double call for ` ${R}etc/init.d/rc.vehicle_setup` - Was introduced during the rebase - Was causing module already running & uORB topic can't be advertised errors Fix format via `make format` command Modify S500 airframe file to use for control allocation usage - Added Control allocation related parameters as default to not have it reset every time the airframe is selected Implement mission specific payload deploy timeout and more changes Switch payload_deliverer to run on work queue Remove unnecessary files - Airframe changes from enabling control allocation are removed Address review comments - Remove debug messages - Remove unnecessary or verbose comments & code - Properly call parameter_update() function Switch payload_deliverer to scheduled interval work item & refactor - Switch to Schedeuled on Interval Work Item, as previous vehicle command subscription callback based behavior led to vehicle comamnd ack not being sent accordingly (since the Run() wouldn't be called unless there's a new vehicle command), leading to ack command not being sent out - Also, old vehicle commands were getting fetched due to the subscription callback as well, which was removed with this patch - Fix the wrong population of floating point param2 field of vehicle command by int8_t type gripper action by creating dedicated function - Refactor and add comments to increase readability Add gripper::grabbing() method and handle this in parameter update - Previously, the intermediate state 'grabbing' was not considered, and when the parameter update was called after the first initialization of the gripper, the grab() function was being called again, which would produce unnecessary duplicate vehicle command. - Also replaced direct .grab() access to sending vehicle comamnd, which unifies the gripper actuation mechanism through vehicle commands. Navigator: Change SubscriptionData to Subscription to reduce memory usage - Also removed unused vehicle command ack sub PayloadDeliverer: Remove unnecessary changes & Bring back vehicle_command sub cb
This commit is contained in:
Junwoo Hwang
Beat Küng
parent
e115095f70
commit
2542b1bb26
|
|
@ -30,4 +30,3 @@ param set-default PWM_MAIN_FUNC1 101
|
|||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
|
||||
|
|
|
|||
|
|
@ -262,6 +262,12 @@ then
|
|||
gyro_calibration start
|
||||
fi
|
||||
|
||||
# Payload deliverer module if gripper is enabled
|
||||
if param compare -s PD_GRIPPER_EN 1
|
||||
then
|
||||
payload_deliverer start
|
||||
fi
|
||||
|
||||
#user defined mavlink streams for instances can be in PATH
|
||||
. px4-rc.mavlink
|
||||
|
||||
|
|
|
|||
|
|
@ -505,6 +505,12 @@ else
|
|||
px4flow start -X &
|
||||
fi
|
||||
|
||||
# Payload deliverer module if gripper is enabled
|
||||
if param compare -s PD_GRIPPER_EN 1
|
||||
then
|
||||
payload_deliverer start
|
||||
fi
|
||||
|
||||
#
|
||||
# Optional board supplied extras: rc.board_extras
|
||||
#
|
||||
|
|
|
|||
|
|
@ -51,3 +51,5 @@ CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
|
|||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_MODULES_FW_POS_CONTROL_L1=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
|||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
|||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_REPLAY=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -96,6 +96,7 @@ set(msg_files
|
|||
gps_dump.msg
|
||||
gps_inject_data.msg
|
||||
health_report.msg
|
||||
gripper.msg
|
||||
heater_status.msg
|
||||
home_position.msg
|
||||
hover_thrust_estimate.msg
|
||||
|
|
|
|||
|
|
@ -0,0 +1,7 @@
|
|||
## Used to command an actuation in the gripper, which is mapped to a specific output in the mixer module
|
||||
|
||||
uint64 timestamp
|
||||
|
||||
int8 command # Commanded state for the gripper
|
||||
int8 COMMAND_GRAB = 0
|
||||
int8 COMMAND_RELEASE = 1
|
||||
|
|
@ -57,6 +57,7 @@ uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofenc
|
|||
uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|
|
||||
uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty|
|
||||
uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|
|
||||
uint16 VEHICLE_CMD_DO_GRIPPER = 211 # Command to operate a gripper
|
||||
uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
|
||||
uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|
|
||||
uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|
|
||||
|
|
@ -96,7 +97,7 @@ 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_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.
|
||||
|
||||
uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch.
|
||||
|
||||
# PX4 vehicle commands (beyond 16 bit mavlink commands)
|
||||
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX)
|
||||
|
|
@ -160,6 +161,10 @@ uint8 SPEED_TYPE_DESCEND_SPEED = 3
|
|||
int8 ARMING_ACTION_DISARM = 0
|
||||
int8 ARMING_ACTION_ARM = 1
|
||||
|
||||
# param2 in VEHICLE_CMD_DO_GRIPPER
|
||||
uint8 GRIPPER_ACTION_RELEASE = 0
|
||||
uint8 GRIPPER_ACTION_GRAB = 1
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 8
|
||||
|
||||
float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,69 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FunctionProviderBase.hpp"
|
||||
|
||||
#include <uORB/topics/gripper.h>
|
||||
|
||||
/**
|
||||
* @brief Function: Gripper (Used for actuating a Gripper)
|
||||
*/
|
||||
class FunctionGripper : public FunctionProviderBase
|
||||
{
|
||||
public:
|
||||
FunctionGripper() = default;
|
||||
static FunctionProviderBase *allocate(const Context &context) { return new FunctionGripper(); }
|
||||
|
||||
void update() override
|
||||
{
|
||||
gripper_s gripper;
|
||||
|
||||
if (_gripper_sub.update(&gripper)) {
|
||||
if (gripper.command == gripper_s::COMMAND_RELEASE) {
|
||||
_data = -1.f; // Minimum command for release
|
||||
|
||||
} else if (gripper.command == gripper_s::COMMAND_GRAB) {
|
||||
_data = 1.f; // Maximum command for grab
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float value(OutputFunction func) override { return _data; }
|
||||
|
||||
private:
|
||||
uORB::Subscription _gripper_sub{ORB_ID(gripper)};
|
||||
float _data{-1.f};
|
||||
};
|
||||
|
|
@ -62,6 +62,7 @@ static const FunctionProvider all_function_providers[] = {
|
|||
{OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate},
|
||||
{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
|
||||
{OutputFunction::Parachute, &FunctionParachute::allocate},
|
||||
{OutputFunction::Gripper, &FunctionGripper::allocate},
|
||||
{OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate},
|
||||
{OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate},
|
||||
};
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
#include "functions/FunctionConstantMax.hpp"
|
||||
#include "functions/FunctionConstantMin.hpp"
|
||||
#include "functions/FunctionGimbal.hpp"
|
||||
#include "functions/FunctionGripper.hpp"
|
||||
#include "functions/FunctionLandingGear.hpp"
|
||||
#include "functions/FunctionManualRC.hpp"
|
||||
#include "functions/FunctionMotors.hpp"
|
||||
|
|
|
|||
|
|
@ -34,6 +34,8 @@ functions:
|
|||
Gimbal_Pitch: 421
|
||||
Gimbal_Yaw: 422
|
||||
|
||||
Gripper: 430
|
||||
|
||||
# Add your own here:
|
||||
#MyCustomFunction: 10000
|
||||
|
||||
|
|
|
|||
|
|
@ -1518,6 +1518,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
|
||||
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
|
||||
/* ignore commands that are handled by other parts of the system */
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -137,6 +137,14 @@ private:
|
|||
|
||||
void manual_control_check();
|
||||
|
||||
/**
|
||||
* @brief Handle incoming vehicle command relavant to Commander
|
||||
*
|
||||
* It ignores irrelevant vehicle commands defined inside the switch case statement
|
||||
* in the function.
|
||||
*
|
||||
* @param cmd Vehicle command to handle
|
||||
*/
|
||||
bool handle_command(const vehicle_command_s &cmd);
|
||||
|
||||
unsigned handle_command_motor_test(const vehicle_command_s &cmd);
|
||||
|
|
|
|||
|
|
@ -68,6 +68,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("gimbal_manager_set_attitude", 500);
|
||||
add_optional_topic("generator_status");
|
||||
add_optional_topic("gps_dump");
|
||||
add_optional_topic("gripper");
|
||||
add_optional_topic("heater_status");
|
||||
add_topic("home_position");
|
||||
add_topic("hover_thrust_estimate", 100);
|
||||
|
|
@ -106,6 +107,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("vehicle_attitude", 50);
|
||||
add_topic("vehicle_attitude_setpoint", 50);
|
||||
add_topic("vehicle_command");
|
||||
add_topic("vehicle_command_ack");
|
||||
add_topic("vehicle_constraints", 1000);
|
||||
add_topic("vehicle_control_mode");
|
||||
add_topic("vehicle_global_position", 200);
|
||||
|
|
|
|||
|
|
@ -1331,6 +1331,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
|
||||
(_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
|
||||
mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) {
|
||||
// This is a mission item with a global coordinate
|
||||
|
||||
// Switch to int mode if that is what we are receiving
|
||||
if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
|
||||
|
|
@ -1363,6 +1364,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
mission_item->altitude_is_relative = true;
|
||||
}
|
||||
|
||||
// Depending on the received MAV_CMD_* (MAVLink Commands), assign the corresponding
|
||||
// NAV_CMD value to the mission item's nav_cmd.
|
||||
switch (mavlink_mission_item->command) {
|
||||
case MAV_CMD_NAV_WAYPOINT:
|
||||
mission_item->nav_cmd = NAV_CMD_WAYPOINT;
|
||||
|
|
@ -1438,11 +1441,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
break;
|
||||
|
||||
case MAV_CMD_CONDITION_GATE:
|
||||
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
||||
mission_item->nav_cmd = NAV_CMD_CONDITION_GATE;
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_FENCE_RETURN_POINT:
|
||||
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
||||
mission_item->nav_cmd = NAV_CMD_FENCE_RETURN_POINT;
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
|
||||
|
|
@ -1463,7 +1466,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
|
||||
default:
|
||||
mission_item->nav_cmd = NAV_CMD_INVALID;
|
||||
|
||||
PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command);
|
||||
|
||||
return MAV_MISSION_UNSUPPORTED;
|
||||
|
|
@ -1473,7 +1475,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
|
||||
} else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
|
||||
|
||||
// this is a mission item with no coordinates
|
||||
// This is a mission item with no coordinates
|
||||
|
||||
mission_item->params[0] = mavlink_mission_item->param1;
|
||||
mission_item->params[1] = mavlink_mission_item->param2;
|
||||
|
|
@ -1517,6 +1519,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|||
}
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_WINCH:
|
||||
case MAV_CMD_DO_GRIPPER:
|
||||
mission_item->nav_cmd = mavlink_mission_item->command;
|
||||
MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(mission_item, mavlink_mission_item, 1, 2);
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_CHANGE_SPEED:
|
||||
case MAV_CMD_DO_SET_HOME:
|
||||
case MAV_CMD_DO_SET_SERVO:
|
||||
|
|
@ -1748,6 +1756,39 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
|||
return PX4_OK;
|
||||
}
|
||||
|
||||
void MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
|
||||
const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx, int8_t end_idx)
|
||||
{
|
||||
// Copy each param1 ~ 7 if they are within the range specified
|
||||
if (start_idx <= 1 && 1 <= end_idx) {
|
||||
mission_item->params[0] = mavlink_mission_item->param1;
|
||||
}
|
||||
|
||||
if (start_idx <= 2 && 2 <= end_idx) {
|
||||
mission_item->params[1] = mavlink_mission_item->param2;
|
||||
}
|
||||
|
||||
if (start_idx <= 3 && 3 <= end_idx) {
|
||||
mission_item->params[2] = mavlink_mission_item->param3;
|
||||
}
|
||||
|
||||
if (start_idx <= 4 && 4 <= end_idx) {
|
||||
mission_item->params[3] = mavlink_mission_item->param4;
|
||||
}
|
||||
|
||||
/* Param5, 6 and 7 are named x, y and z since it is used as position coordinates as well */
|
||||
if (start_idx <= 5 && 5 <= end_idx) {
|
||||
mission_item->params[4] = mavlink_mission_item->x;
|
||||
}
|
||||
|
||||
if (start_idx <= 6 && 6 <= end_idx) {
|
||||
mission_item->params[5] = mavlink_mission_item->y;
|
||||
}
|
||||
|
||||
if (start_idx <= 7 && 7 <= end_idx) {
|
||||
mission_item->params[6] = mavlink_mission_item->z;
|
||||
}
|
||||
}
|
||||
|
||||
void MavlinkMissionManager::check_active_mission()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -245,4 +245,19 @@ private:
|
|||
* set _state to idle (and do necessary cleanup)
|
||||
*/
|
||||
void switch_to_idle_state();
|
||||
|
||||
/**
|
||||
* Copies the specified range [1, 7] of param of MAVLink mission to params[] array of
|
||||
* the Mission item struct (Very useful for mission items for non-navigation
|
||||
* like MAV_CMD_DO*, as they use a parameter mapping 1 ~ 7, which directly
|
||||
* gets the value from MAVlink's Mission Item parameters.
|
||||
*
|
||||
* @param start_idx [1, 7] Start index of Param to copy from Mavlink Mission Item
|
||||
* @param end_idx [1, 7] End index of Param to copy from Mavlink Mission Item
|
||||
*
|
||||
* Note: The Index is in range [1, 7], so if you want to copy param2 ~ param5 into
|
||||
* params[1] and params[4], you need to call with 'start_idx = 2' and 'end_idx = 5'!
|
||||
*/
|
||||
void copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
|
||||
const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx = 1, int8_t end_idx = 7);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@
|
|||
|
||||
/**
|
||||
* @file mavlink_receiver.h
|
||||
* MAVLink receiver thread
|
||||
*
|
||||
* MAVLink receiver thread that converts the received MAVLink messages to the appropriate
|
||||
* uORB topic publications, to decouple the uORB message and MAVLink message.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
|
|
|
|||
|
|
@ -216,8 +216,8 @@ Mission::on_active()
|
|||
{
|
||||
check_mission_valid(false);
|
||||
|
||||
/* check if anything has changed */
|
||||
bool mission_sub_updated = _mission_sub.updated();
|
||||
/* Check if stored mission plan has changed */
|
||||
const bool mission_sub_updated = _mission_sub.updated();
|
||||
|
||||
if (mission_sub_updated) {
|
||||
_navigator->reset_triplets();
|
||||
|
|
@ -245,7 +245,7 @@ Mission::on_active()
|
|||
}
|
||||
|
||||
/* lets check if we reached the current mission item */
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) {
|
||||
/* If we just completed a takeoff which was inserted before the right waypoint,
|
||||
there is no need to report that we reached it because we didn't. */
|
||||
if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
|
||||
|
|
@ -668,7 +668,6 @@ Mission::set_mission_items()
|
|||
_mission_type = MISSION_TYPE_MISSION;
|
||||
|
||||
} else {
|
||||
/* no mission available or mission finished, switch to loiter */
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
|
||||
if (_navigator->get_land_detected()->landed) {
|
||||
|
|
@ -1114,9 +1113,6 @@ Mission::set_mission_items()
|
|||
}
|
||||
|
||||
/*********************************** set setpoints and check next *********************************************/
|
||||
|
||||
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
// The logic in this section establishes the tracking between the current waypoint
|
||||
// which we are approaching and the next waypoint, which will tell us in which direction
|
||||
// we will change our trajectory right after reaching it.
|
||||
|
|
@ -1125,8 +1121,9 @@ Mission::set_mission_items()
|
|||
// we are searching around the current mission item in the list to find the closest
|
||||
// gate and the closest waypoint. We then store them separately.
|
||||
|
||||
// Check if the mission item is a gate
|
||||
// along the current trajectory
|
||||
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
// Check if the mission item is a gate along the current trajectory
|
||||
if (item_contains_gate(_mission_item)) {
|
||||
|
||||
// The mission item is a gate, let's check if the next item in the list provides
|
||||
|
|
@ -1179,10 +1176,8 @@ Mission::set_mission_items()
|
|||
set_current_mission_item();
|
||||
}
|
||||
|
||||
// If the mission item under evaluation contains a gate,
|
||||
// then we need to check if we have a next position item so
|
||||
// the controller can fly the correct line between the
|
||||
// current and next setpoint
|
||||
// If the mission item under evaluation contains a gate, we need to check if we have a next position item so
|
||||
// the controller can fly the correct line between the current and next setpoint
|
||||
if (item_contains_gate(_mission_item)) {
|
||||
if (has_after_next_position_item) {
|
||||
/* got next mission item, update setpoint triplet */
|
||||
|
|
@ -1194,9 +1189,13 @@ Mission::set_mission_items()
|
|||
}
|
||||
|
||||
} else {
|
||||
/* allow the vehicle to decelerate before reaching a wp with a hold time */
|
||||
// Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout
|
||||
// This is done by setting the position triplet's next position's valid flag to false,
|
||||
// which makes the FlightTask disregard the next position
|
||||
// TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior
|
||||
// seems hacky, handle this more properly.
|
||||
const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
&& get_time_inside(_mission_item) > FLT_EPSILON;
|
||||
&& (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item));
|
||||
|
||||
if (_mission_item.autocontinue && !brake_for_hold) {
|
||||
/* try to process next mission item */
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@
|
|||
/**
|
||||
* @file mission.h
|
||||
*
|
||||
* Navigator mode to access missions
|
||||
* Mission mode class that handles everything related to executing a mission.
|
||||
* This class gets included as one of the 'modes' in the Navigator, along with other
|
||||
* modes like RTL, Loiter, etc.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
|
|
@ -118,7 +120,10 @@ private:
|
|||
void advance_mission();
|
||||
|
||||
/**
|
||||
* Set new mission items
|
||||
* @brief Configures mission items in current setting
|
||||
*
|
||||
* Configure the mission items depending on current mission item index and settings such
|
||||
* as terrain following, etc.
|
||||
*/
|
||||
void set_mission_items();
|
||||
|
||||
|
|
@ -273,6 +278,7 @@ private:
|
|||
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 {
|
||||
WORK_ITEM_TYPE_DEFAULT, /**< default mission item */
|
||||
WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */
|
||||
|
|
|
|||
|
|
@ -70,24 +70,15 @@ MissionBlock::MissionBlock(Navigator *navigator) :
|
|||
}
|
||||
|
||||
bool
|
||||
MissionBlock::is_mission_item_reached()
|
||||
MissionBlock::is_mission_item_reached_or_completed()
|
||||
{
|
||||
/* handle non-navigation or indefinite waypoints */
|
||||
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Handle indefinite waypoints and action commands
|
||||
switch (_mission_item.nav_cmd) {
|
||||
|
||||
// Action Commands that doesn't have timeout completes instantaneously
|
||||
case NAV_CMD_DO_SET_SERVO:
|
||||
return true;
|
||||
|
||||
case NAV_CMD_LAND: /* fall through */
|
||||
case NAV_CMD_VTOL_LAND:
|
||||
return _navigator->get_land_detected()->landed;
|
||||
|
||||
case NAV_CMD_IDLE: /* fall through */
|
||||
case NAV_CMD_LOITER_UNLIMITED:
|
||||
return false;
|
||||
|
||||
case NAV_CMD_DO_LAND_START:
|
||||
case NAV_CMD_DO_TRIGGER_CONTROL:
|
||||
case NAV_CMD_DO_DIGICAM_CONTROL:
|
||||
|
|
@ -114,6 +105,15 @@ MissionBlock::is_mission_item_reached()
|
|||
case NAV_CMD_DO_SET_HOME:
|
||||
return true;
|
||||
|
||||
// Indefinite Waypoints
|
||||
case NAV_CMD_LAND: /* fall through */
|
||||
case NAV_CMD_VTOL_LAND:
|
||||
return _navigator->get_land_detected()->landed;
|
||||
|
||||
case NAV_CMD_IDLE: /* fall through */
|
||||
case NAV_CMD_LOITER_UNLIMITED:
|
||||
return false;
|
||||
|
||||
case NAV_CMD_DO_VTOL_TRANSITION:
|
||||
|
||||
if (int(_mission_item.params[0]) == 3) {
|
||||
|
|
@ -146,11 +146,49 @@ MissionBlock::is_mission_item_reached()
|
|||
_time_wp_reached = now;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case NAV_CMD_DO_WINCH: {
|
||||
const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) *
|
||||
1E-6f; // TODO: Add proper microseconds_to_seconds function
|
||||
|
||||
if (_payload_deploy_ack_successful) {
|
||||
PX4_DEBUG("Winch Deploy Ack received! Resuming mission");
|
||||
return true;
|
||||
|
||||
} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
|
||||
PX4_DEBUG("Winch Deploy Timed out, resuming mission!");
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
// We are still waiting for the acknowledgement / execution of deploy
|
||||
return false;
|
||||
}
|
||||
|
||||
case NAV_CMD_DO_GRIPPER: {
|
||||
const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
|
||||
|
||||
if (_payload_deploy_ack_successful) {
|
||||
PX4_DEBUG("Gripper Deploy Ack received! Resuming mission");
|
||||
return true;
|
||||
|
||||
} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
|
||||
PX4_DEBUG("Gripper Deploy Timed out, resuming mission!");
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
// We are still waiting for the acknowledgement / execution of deploy
|
||||
return false;
|
||||
}
|
||||
|
||||
default:
|
||||
/* do nothing, this is a 3D waypoint */
|
||||
break;
|
||||
}
|
||||
|
||||
// Update the 'waypoint position reached' status
|
||||
if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
|
||||
|
||||
float dist = -1.0f;
|
||||
|
|
@ -198,13 +236,13 @@ MissionBlock::is_mission_item_reached()
|
|||
|
||||
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|
||||
&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
||||
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
|
||||
// Accept takeoff waypoint to be reached if the distance in 2D plane is within acceptance radius
|
||||
if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
}
|
||||
|
||||
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
|
||||
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
|
||||
// For takeoff mission items use the parameter for the takeoff acceptance radius
|
||||
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
|
||||
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
|
||||
_waypoint_position_reached = true;
|
||||
|
|
@ -373,8 +411,7 @@ MissionBlock::is_mission_item_reached()
|
|||
}
|
||||
}
|
||||
|
||||
/* Check if the requested yaw setpoint is reached (only for rotary wing flight). */
|
||||
|
||||
// Update the 'waypoint position reached' status (only for rotary wing flight)
|
||||
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
|
||||
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
|
|
@ -410,7 +447,7 @@ MissionBlock::is_mission_item_reached()
|
|||
}
|
||||
}
|
||||
|
||||
/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
|
||||
// Handle Loiter/Delay Timeout if the waypoint position and yaw setpoint got reached
|
||||
if (_waypoint_position_reached && _waypoint_yaw_reached) {
|
||||
|
||||
bool time_inside_reached = false;
|
||||
|
|
@ -533,6 +570,23 @@ MissionBlock::issue_command(const mission_item_s &item)
|
|||
|
||||
_actuator_pub.publish(actuators);
|
||||
|
||||
} else if (item.nav_cmd == NAV_CMD_DO_WINCH ||
|
||||
item.nav_cmd == NAV_CMD_DO_GRIPPER) {
|
||||
// Initiate Payload Deployment
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = item.nav_cmd;
|
||||
vcmd.param1 = item.params[0];
|
||||
vcmd.param2 = item.params[1];
|
||||
vcmd.param3 = item.params[2];
|
||||
vcmd.param4 = item.params[3];
|
||||
vcmd.param5 = static_cast<double>(item.params[4]);
|
||||
vcmd.param6 = static_cast<double>(item.params[5]);
|
||||
_navigator->publish_vehicle_cmd(&vcmd);
|
||||
|
||||
// Reset payload deploy flag & data to get ready to receive deployment ack result
|
||||
_payload_deploy_ack_successful = false;
|
||||
_payload_deployed_time = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
|
||||
// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
|
||||
|
|
@ -540,8 +594,8 @@ MissionBlock::issue_command(const mission_item_s &item)
|
|||
_navigator->acquire_gimbal_control();
|
||||
}
|
||||
|
||||
// we're expecting a mission command item here so assign the "raw" inputs to the command
|
||||
// (MAV_FRAME_MISSION mission item)
|
||||
// Mission item's NAV_CMD enums directly map to the according vehicle command
|
||||
// So set the raw value directly (MAV_FRAME_MISSION mission item)
|
||||
vehicle_command_s vcmd = {};
|
||||
vcmd.command = item.nav_cmd;
|
||||
vcmd.param1 = item.params[0];
|
||||
|
|
@ -581,6 +635,17 @@ MissionBlock::get_time_inside(const mission_item_s &item) const
|
|||
return 0.0f;
|
||||
}
|
||||
|
||||
// TODO: get_time_inside and item_has_timeout is quite redundant. Separate them out
|
||||
// Problem arises from the fact that DO_WINCH and DO_GRIPPER *should be an instantaneous command,
|
||||
// and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER
|
||||
// or so, and have a function named 'item_is_conditional'
|
||||
// Reference: https://mavlink.io/en/services/mission.html#mavlink_commands
|
||||
bool
|
||||
MissionBlock::item_has_timeout(const mission_item_s &item)
|
||||
{
|
||||
return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER;
|
||||
}
|
||||
|
||||
bool
|
||||
MissionBlock::item_contains_position(const mission_item_s &item)
|
||||
{
|
||||
|
|
@ -609,7 +674,7 @@ MissionBlock::item_contains_marker(const mission_item_s &item)
|
|||
bool
|
||||
MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
|
||||
{
|
||||
/* don't change the setpoint for non-position items */
|
||||
// Don't change the setpoint for non-position items
|
||||
if (!item_contains_position(item)) {
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@
|
|||
/**
|
||||
* @file mission_block.h
|
||||
*
|
||||
* Helper class to use mission items
|
||||
* Base class for Mission class and special flight modes like
|
||||
* RTL, Land, Loiter, Takeoff, Geofence, etc.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
|
@ -74,6 +75,11 @@ public:
|
|||
*/
|
||||
static bool item_contains_position(const mission_item_s &item);
|
||||
|
||||
/**
|
||||
* Returns true if the mission item is not an instant action, but has a delay / timeout
|
||||
*/
|
||||
bool item_has_timeout(const mission_item_s &item);
|
||||
|
||||
/**
|
||||
* Check if the mission item contains a gate condition
|
||||
*
|
||||
|
|
@ -88,12 +94,42 @@ public:
|
|||
*/
|
||||
static bool item_contains_marker(const mission_item_s &item);
|
||||
|
||||
/**
|
||||
* @brief Set the payload deployment successful flag object
|
||||
*
|
||||
* Function is accessed in Navigator (navigator_main.cpp) to flag when a successful
|
||||
* payload deployment ack command has been received. Which in turn allows the mission item
|
||||
* to continue to the next in the 'is_mission_item_reached_or_completed' function below
|
||||
*/
|
||||
void set_payload_depolyment_successful_flag(bool payload_deploy_result)
|
||||
{
|
||||
_payload_deploy_ack_successful = payload_deploy_result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the payload deployment timeout
|
||||
*
|
||||
* Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful
|
||||
* payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will
|
||||
* continue into the next item automatically.
|
||||
*
|
||||
* @param timeout_s Timeout in seconds
|
||||
*/
|
||||
void set_payload_deployment_timeout(const float timeout_s)
|
||||
{
|
||||
_payload_deploy_timeout_s = timeout_s;
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Check if mission item has been reached
|
||||
* @return true if successfully reached
|
||||
* Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items)
|
||||
*
|
||||
* Mission Item's 'nav_cmd' can be either Waypoint or Action based. In order to check whether current mission item's
|
||||
* execution was successful, we need to check either the waypoint was 'reached' or the action was 'completed'.
|
||||
*
|
||||
* @return true if successfully reached or completed
|
||||
*/
|
||||
bool is_mission_item_reached();
|
||||
bool is_mission_item_reached_or_completed();
|
||||
|
||||
/**
|
||||
* Reset all reached flags
|
||||
|
|
@ -140,18 +176,39 @@ protected:
|
|||
*/
|
||||
void mission_apply_limitation(mission_item_s &item);
|
||||
|
||||
/**
|
||||
* @brief Issue a command for mission items with a nav_cmd that specifies an action
|
||||
*
|
||||
* Execute the specified command inside the mission item. The action depends on the nav_cmd
|
||||
* value (which correlates to the MAVLink's MAV_CMD enum values) and the params defined in the
|
||||
* mission item.
|
||||
*
|
||||
* This is used for commands like MAV_CMD_DO* in MAVLink, where immediate actions are defined.
|
||||
* For more information, refer to: https://mavlink.io/en/services/mission.html#mavlink_commands
|
||||
*
|
||||
* @param item Mission item to execute
|
||||
*/
|
||||
void issue_command(const mission_item_s &item);
|
||||
|
||||
/**
|
||||
* [s] Get the time to stay that's specified in the mission item
|
||||
*/
|
||||
float get_time_inside(const mission_item_s &item) const;
|
||||
|
||||
float get_absolute_altitude_for_item(const mission_item_s &mission_item) const;
|
||||
|
||||
mission_item_s _mission_item{};
|
||||
mission_item_s _mission_item{}; // Current mission item that is being executed
|
||||
|
||||
bool _waypoint_position_reached{false};
|
||||
bool _waypoint_yaw_reached{false};
|
||||
|
||||
hrt_abstime _time_wp_reached{0};
|
||||
|
||||
/* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */
|
||||
bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment
|
||||
hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts
|
||||
float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
|
||||
|
||||
// Used for MAV_CMD_DO_SET_SERVO command of a Mission item
|
||||
uORB::Publication<actuator_controls_s> _actuator_pub{ORB_ID(actuator_controls_2)};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -250,6 +250,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
|
|||
missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
|
||||
missionitem.nav_cmd != NAV_CMD_DELAY &&
|
||||
missionitem.nav_cmd != NAV_CMD_CONDITION_GATE &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_WINCH &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_GRIPPER &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
|
||||
missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
|
||||
|
|
|
|||
|
|
@ -156,3 +156,14 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
|
|||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
|
||||
|
||||
/**
|
||||
* Timeout for a successful payload deployment acknowledgement
|
||||
*
|
||||
* @unit s
|
||||
* @min 0
|
||||
* @decimal 1
|
||||
* @increment 1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f);
|
||||
|
|
|
|||
|
|
@ -33,7 +33,9 @@
|
|||
|
||||
/**
|
||||
* @file navigation.h
|
||||
*
|
||||
* Definition of a mission consisting of mission items.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
|
|
@ -82,6 +84,7 @@ enum NAV_CMD {
|
|||
NAV_CMD_DO_DIGICAM_CONTROL = 203,
|
||||
NAV_CMD_DO_MOUNT_CONFIGURE = 204,
|
||||
NAV_CMD_DO_MOUNT_CONTROL = 205,
|
||||
NAV_CMD_DO_GRIPPER = 211,
|
||||
NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
|
||||
NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
|
||||
NAV_CMD_OBLIQUE_SURVEY = 260,
|
||||
|
|
@ -102,6 +105,7 @@ enum NAV_CMD {
|
|||
NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
|
||||
NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
|
||||
NAV_CMD_CONDITION_GATE = 4501,
|
||||
NAV_CMD_DO_WINCH = 42600,
|
||||
NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
|
||||
};
|
||||
|
||||
|
|
@ -126,34 +130,31 @@ enum NAV_FRAME {
|
|||
NAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Global position setpoint in WGS84 coordinates.
|
||||
*
|
||||
* This is the position the MAV is heading towards. If it is of type loiter,
|
||||
* the MAV is circling around it with the given loiter radius in meters.
|
||||
*
|
||||
* Corresponds to one of the DM_KEY_WAYPOINTS_OFFBOARD_* dataman items
|
||||
*/
|
||||
|
||||
// Mission Item structure
|
||||
// We explicitly handle struct padding to ensure consistency between in memory and on disk formats across different platforms, toolchains, etc
|
||||
// The use of #pragma pack is avoided to prevent the possibility of unaligned memory accesses.
|
||||
|
||||
#if (__GNUC__ >= 5) || __clang__
|
||||
// Disabled in GCC 4.X as the warning doesn't seem to "pop" correctly
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic error "-Wpadded"
|
||||
#endif // GCC >= 5 || Clang
|
||||
|
||||
/**
|
||||
* Mission Item structure
|
||||
*
|
||||
* We explicitly handle struct padding to ensure consistency between in memory and on disk formats
|
||||
* across different platforms, toolchains, etc. The use of #pragma pack is avoided to prevent the
|
||||
* possibility of unaligned memory accesses.
|
||||
*/
|
||||
struct mission_item_s {
|
||||
double lat; /**< latitude in degrees */
|
||||
double lon; /**< longitude in degrees */
|
||||
|
||||
// Union to support both Mission Item categories in MAVLink such as:
|
||||
// 1. With Global coordinate (param5 ~ 7 corresponds to lat, lon and altitude)
|
||||
// 2. Without global coordinate (when frame = MAV_FRAME_MISSION)
|
||||
|
||||
// Note: the structure and definition of params depends on the nav_cmd, which is
|
||||
// compatible with MAVLink's MAV_CMD enum definitions.
|
||||
union {
|
||||
// Navigation command parameters
|
||||
struct {
|
||||
union {
|
||||
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
|
||||
|
|
@ -166,7 +167,9 @@ struct mission_item_s {
|
|||
float ___lon_float; /**< padding */
|
||||
float altitude; /**< altitude in meters (AMSL) */
|
||||
};
|
||||
float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/
|
||||
|
||||
// Non-Navigation command parameters (implicit)
|
||||
float params[7]; /**< array to store mission command values with no global coordinates (frame = MAV_FRAME_MISSION) */
|
||||
};
|
||||
|
||||
uint16_t nav_cmd; /**< navigation command */
|
||||
|
|
|
|||
|
|
@ -118,6 +118,14 @@ public:
|
|||
*/
|
||||
void load_fence_from_file(const char *filename);
|
||||
|
||||
/**
|
||||
* @brief Publish a given specified vehicle command
|
||||
*
|
||||
* Sets the target_component of the vehicle command accordingly depending on the
|
||||
* vehicle command value (e.g. For Camera control, sets target system component id)
|
||||
*
|
||||
* @param vcmd Vehicle command to execute
|
||||
*/
|
||||
void publish_vehicle_cmd(vehicle_command_s *vcmd);
|
||||
|
||||
/**
|
||||
|
|
@ -373,9 +381,7 @@ private:
|
|||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
Geofence _geofence; /**< class that handles the geofence */
|
||||
|
||||
GeofenceBreachAvoidance _gf_breach_avoidance;
|
||||
|
||||
hrt_abstime _last_geofence_check = 0;
|
||||
|
||||
bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
|
||||
|
|
@ -430,6 +436,7 @@ private:
|
|||
void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
|
||||
|
||||
bool geofence_allows_position(const vehicle_global_position_s &pos);
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad, /**< loiter radius for fixedwing */
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, /**< acceptance for takeoff */
|
||||
|
|
@ -442,14 +449,13 @@ private:
|
|||
(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu, /**< avoidance Distance Unmanned*/
|
||||
(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm, /**< avoidance Distance Manned*/
|
||||
|
||||
// non-navigator parameters
|
||||
// Mission (MIS_*)
|
||||
// non-navigator parameters: Mission (MIS_*)
|
||||
(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
|
||||
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
|
||||
(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
|
||||
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
|
||||
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
|
||||
(ParamFloat<px4::params::MIS_PD_TO>) _param_mis_payload_delivery_timeout,
|
||||
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max
|
||||
|
||||
)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -98,6 +98,9 @@ Navigator::Navigator() :
|
|||
_mission_sub = orb_subscribe(ORB_ID(mission));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
|
||||
// Update the timeout used in mission_block (which can't hold it's own parameters)
|
||||
_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
|
||||
|
||||
reset_triplets();
|
||||
}
|
||||
|
||||
|
|
@ -128,6 +131,8 @@ void Navigator::params_update()
|
|||
if (_handle_mpc_acc_hor != PARAM_INVALID) {
|
||||
param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor);
|
||||
}
|
||||
|
||||
_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
|
||||
}
|
||||
|
||||
void Navigator::run()
|
||||
|
|
@ -203,7 +208,7 @@ void Navigator::run()
|
|||
}
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
/* check for parameter updates */
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
|
|
@ -217,8 +222,10 @@ void Navigator::run()
|
|||
_position_controller_status_sub.update();
|
||||
_home_pos_sub.update(&_home_pos);
|
||||
|
||||
// Handle Vehicle commands
|
||||
while (_vehicle_command_sub.updated()) {
|
||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||
|
||||
vehicle_command_s cmd{};
|
||||
_vehicle_command_sub.copy(&cmd);
|
||||
|
||||
|
|
|
|||
|
|
@ -298,7 +298,7 @@ void RTL::on_activation()
|
|||
|
||||
void RTL::on_active()
|
||||
{
|
||||
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
|
||||
if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) {
|
||||
advance_rtl();
|
||||
set_rtl_item();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ Takeoff::on_active()
|
|||
// reset the position
|
||||
set_takeoff_position();
|
||||
|
||||
} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
|
||||
} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
|
||||
_navigator->get_mission_result()->finished = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ VtolTakeoff::on_activation()
|
|||
void
|
||||
VtolTakeoff::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
if (is_mission_item_reached_or_completed()) {
|
||||
reset_mission_item_reached();
|
||||
|
||||
switch (_takeoff_state) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,45 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE modules__payload_deliverer
|
||||
MAIN payload_deliverer
|
||||
SRCS
|
||||
payload_deliverer.cpp
|
||||
gripper.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
COMPILE_FLAGS
|
||||
#-DDEBUG_BUILD
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
menuconfig MODULES_PAYLOAD_DELIVERER
|
||||
bool "payload_deliverer"
|
||||
default n
|
||||
---help---
|
||||
Enable payload_deliverer module
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "gripper.h"
|
||||
|
||||
void Gripper::init(const GripperConfig &config)
|
||||
{
|
||||
switch (config.type) {
|
||||
case GripperConfig::GripperType::SERVO:
|
||||
break;
|
||||
|
||||
case GripperConfig::GripperType::UNDEFINED:
|
||||
|
||||
// FALL THROUGH
|
||||
default:
|
||||
_valid = false;
|
||||
return;
|
||||
}
|
||||
|
||||
switch (config.sensor) {
|
||||
case GripperConfig::GripperSensorType::ENCODER:
|
||||
// Handle encoder sensor setup
|
||||
_has_feedback_sensor = true;
|
||||
break;
|
||||
|
||||
case GripperConfig::GripperSensorType::NONE:
|
||||
default:
|
||||
// No feedback sensor
|
||||
_has_feedback_sensor = false;
|
||||
break;
|
||||
}
|
||||
|
||||
_timeout_us = config.timeout_us;
|
||||
|
||||
// We have valid gripper type & sensor configuration
|
||||
_valid = true;
|
||||
}
|
||||
|
||||
void Gripper::grab()
|
||||
{
|
||||
publish_gripper_command(gripper_s::COMMAND_GRAB);
|
||||
_state = GripperState::GRABBING;
|
||||
_last_command_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void Gripper::release()
|
||||
{
|
||||
publish_gripper_command(gripper_s::COMMAND_RELEASE);
|
||||
_state = GripperState::RELEASING;
|
||||
_last_command_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void Gripper::update()
|
||||
{
|
||||
const bool command_timed_out = (hrt_elapsed_time(&_last_command_time) >= _timeout_us);
|
||||
|
||||
// Handle transition from intermediate state to definite state
|
||||
switch (_state) {
|
||||
case GripperState::GRABBING:
|
||||
if (_has_feedback_sensor) {
|
||||
// Handle feedback sensor input, return true for now (not supported)
|
||||
_state = GripperState::GRABBED;
|
||||
break;
|
||||
}
|
||||
|
||||
if (command_timed_out) {
|
||||
_state = GripperState::GRABBED;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case GripperState::RELEASING:
|
||||
if (_has_feedback_sensor) {
|
||||
// Handle feedback sensor input, return true for now (not supported)
|
||||
_released_state_cache = true;
|
||||
_state = GripperState::RELEASED;
|
||||
break;
|
||||
}
|
||||
|
||||
if (command_timed_out) {
|
||||
_released_state_cache = true;
|
||||
_state = GripperState::RELEASED;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
// DO NOTHING
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Gripper::publish_gripper_command(const int8_t gripper_command)
|
||||
{
|
||||
gripper_s gripper{};
|
||||
gripper.timestamp = hrt_absolute_time();
|
||||
gripper.command = gripper_command;
|
||||
_gripper_pub.publish(gripper);
|
||||
}
|
||||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/gripper.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* Griper class to handle functionality of using a Gripper
|
||||
*/
|
||||
|
||||
typedef struct GripperConfig {
|
||||
enum class GripperType {
|
||||
UNDEFINED = -1,
|
||||
SERVO = 0,
|
||||
};
|
||||
|
||||
GripperType type{GripperType::UNDEFINED};
|
||||
|
||||
// Gripper state feedback sensor type
|
||||
enum class GripperSensorType {
|
||||
NONE = -1,
|
||||
ENCODER,
|
||||
};
|
||||
|
||||
GripperSensorType sensor{GripperSensorType::NONE};
|
||||
|
||||
hrt_abstime timeout_us{0};
|
||||
|
||||
} GripperConfig;
|
||||
|
||||
enum class GripperState {
|
||||
IDLE = 0,
|
||||
GRABBING,
|
||||
GRABBED,
|
||||
RELEASING,
|
||||
RELEASED
|
||||
};
|
||||
|
||||
class Gripper
|
||||
{
|
||||
public:
|
||||
Gripper() = default;
|
||||
|
||||
// Initialize the gripper
|
||||
void init(const GripperConfig &config);
|
||||
|
||||
// Actuate the gripper to the grab position
|
||||
void grab();
|
||||
|
||||
// Actuate the gripper to release position
|
||||
void release();
|
||||
|
||||
// Update gripper state
|
||||
void update();
|
||||
|
||||
// Returns true if in grabbed position either sensed or timeout based
|
||||
bool grabbed() { return _state == GripperState::GRABBED; }
|
||||
bool grabbing() { return _state == GripperState::GRABBING; }
|
||||
|
||||
// Returns true if in released position either sensed or timeout based
|
||||
bool released() { return _state == GripperState::RELEASED; }
|
||||
|
||||
// Returns true only once after the state transitioned into released
|
||||
// Used for sending vehicle command ack only when the gripper actually releases
|
||||
// in payload deliverer
|
||||
bool released_read_once()
|
||||
{
|
||||
if (_released_state_cache) {
|
||||
_released_state_cache = false;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Returns true if Gripper output function is assigned properly
|
||||
bool is_valid() const { return _valid; }
|
||||
|
||||
private:
|
||||
// Internal function to send gripper command via uORB
|
||||
void publish_gripper_command(const int8_t gripper_command);
|
||||
|
||||
// Internal settings
|
||||
bool _valid{false};
|
||||
bool _has_feedback_sensor{false};
|
||||
hrt_abstime _timeout_us{0};
|
||||
|
||||
// Internal state
|
||||
GripperState _state{GripperState::IDLE};
|
||||
hrt_abstime _last_command_time{0};
|
||||
bool _released_state_cache{false}; // Cached flag that is set once gripper release was successful, gets reset once read
|
||||
|
||||
uORB::Publication<gripper_s> _gripper_pub{ORB_ID(gripper)};
|
||||
};
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
module_name: payload_deliverer
|
||||
|
||||
parameters:
|
||||
- group: Payload Deliverer
|
||||
definitions:
|
||||
|
||||
# Gripper configuration
|
||||
PD_GRIPPER_EN:
|
||||
description:
|
||||
short: Enable Gripper actuation in Payload Deliverer
|
||||
type: boolean
|
||||
default: 0
|
||||
|
||||
PD_GRIPPER_TYPE:
|
||||
description:
|
||||
short: Type of Gripper (Servo, etc.)
|
||||
type: enum
|
||||
values:
|
||||
-1: Undefined
|
||||
0: Servo
|
||||
min: -1
|
||||
max: 0
|
||||
default: 0
|
||||
|
||||
PD_GRIPPER_TO:
|
||||
description:
|
||||
short: Timeout for successful gripper actuation acknowledgement
|
||||
long: |
|
||||
Maximum time Gripper will wait while the successful griper actuation isn't recognised.
|
||||
If the gripper has no feedback sensor, it will simply wait for
|
||||
this time before considering gripper actuation successful and publish a
|
||||
'vehicle_command_ack' signaling successful gripper action
|
||||
type: float
|
||||
unit: s
|
||||
min: 0
|
||||
default: 3
|
||||
|
|
@ -0,0 +1,279 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include "payload_deliverer.h"
|
||||
|
||||
PayloadDeliverer::PayloadDeliverer()
|
||||
: ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
|
||||
{
|
||||
}
|
||||
|
||||
bool PayloadDeliverer::init()
|
||||
{
|
||||
ScheduleOnInterval(1_s);
|
||||
|
||||
// Additionallly to 1Hz scheduling, add cb for vehicle_command message
|
||||
if (!_vehicle_command_sub.registerCallback()) {
|
||||
PX4_ERR("callback registration failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
initialize_gripper();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PayloadDeliverer::initialize_gripper()
|
||||
{
|
||||
// If gripper instance is invalid, try initializing it
|
||||
if (!_gripper.is_valid() && _param_gripper_enable.get()) {
|
||||
GripperConfig config{};
|
||||
config.type = (GripperConfig::GripperType)_param_gripper_type.get();
|
||||
config.sensor = GripperConfig::GripperSensorType::NONE; // Feedback sensor isn't supported for now
|
||||
config.timeout_us = hrt_abstime(_param_gripper_timeout_s.get() * 1000000ULL);
|
||||
_gripper.init(config);
|
||||
}
|
||||
|
||||
// NOTE: Support for changing gripper sensor type / gripper type configuration when
|
||||
// the parameter change is detected isn't added as we don't have actual use case for that
|
||||
// yet!
|
||||
|
||||
if (!_gripper.is_valid()) {
|
||||
PX4_DEBUG("Gripper object initialization invalid!");
|
||||
return false;
|
||||
|
||||
} else {
|
||||
_gripper.update();
|
||||
|
||||
// If gripper wasn't commanded to go to grab position, command.
|
||||
if (!_gripper.grabbed() && !_gripper.grabbing()) {
|
||||
PX4_DEBUG("Gripper intialize: putting to grab position!");
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void PayloadDeliverer::parameter_update()
|
||||
{
|
||||
updateParams();
|
||||
initialize_gripper();
|
||||
}
|
||||
|
||||
void PayloadDeliverer::Run()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
vehicle_command_s vcmd{};
|
||||
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
_vehicle_command_sub.unregisterCallback();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s param_update_dummy;
|
||||
_parameter_update_sub.copy(¶m_update_dummy);
|
||||
parameter_update();
|
||||
}
|
||||
|
||||
if (_vehicle_command_sub.update(&vcmd)) {
|
||||
handle_vehicle_command(now, &vcmd);
|
||||
|
||||
} else {
|
||||
handle_vehicle_command(now);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now, const vehicle_command_s *vehicle_command)
|
||||
{
|
||||
if (!_gripper.is_valid()) {
|
||||
PX4_WARN("Gripper instance not valid but vehicle command was received. Gripper won't work!");
|
||||
return;
|
||||
}
|
||||
|
||||
_gripper.update();
|
||||
|
||||
// Process successful gripper release acknowledgement
|
||||
if (_gripper.released_read_once()) {
|
||||
vehicle_command_ack_s vcmd_ack{};
|
||||
vcmd_ack.timestamp = now;
|
||||
vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
|
||||
vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
_vehicle_command_ack_pub.publish(vcmd_ack);
|
||||
PX4_DEBUG("Payload Drop Successful Ack Sent!");
|
||||
}
|
||||
|
||||
// If there's no vehicle command to process, just return
|
||||
if (vehicle_command == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Process DO_GRIPPER vehicle command
|
||||
if (vehicle_command->command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
|
||||
const int32_t gripper_action = *(int32_t *)&vehicle_command->param2; // Convert the action to integer
|
||||
|
||||
switch (gripper_action) {
|
||||
case vehicle_command_s::GRIPPER_ACTION_GRAB:
|
||||
_gripper.grab();
|
||||
break;
|
||||
|
||||
case vehicle_command_s::GRIPPER_ACTION_RELEASE:
|
||||
_gripper.release();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool PayloadDeliverer::send_gripper_vehicle_command(const int32_t gripper_action)
|
||||
{
|
||||
vehicle_command_s vcmd;
|
||||
vcmd.timestamp = hrt_absolute_time();
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
|
||||
*(int32_t *)&vcmd.param2 = gripper_action;
|
||||
return _vehicle_command_pub.publish(vcmd);
|
||||
}
|
||||
|
||||
void PayloadDeliverer::gripper_test()
|
||||
{
|
||||
if (!_gripper.is_valid()) {
|
||||
PX4_DEBUG("Gripper is not initialized correctly!");
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_INFO("Test: Opening the Gripper!");
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_RELEASE);
|
||||
|
||||
px4_usleep(5_s);
|
||||
|
||||
PX4_INFO("Test: Closing the Gripper!");
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
|
||||
}
|
||||
|
||||
void PayloadDeliverer::gripper_open()
|
||||
{
|
||||
if (!_gripper.is_valid()) {
|
||||
PX4_DEBUG("Gripper is not initialized correctly!");
|
||||
return;
|
||||
}
|
||||
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_RELEASE);
|
||||
}
|
||||
|
||||
void PayloadDeliverer::gripper_close()
|
||||
{
|
||||
if (!_gripper.is_valid()) {
|
||||
PX4_DEBUG("Gripper is not initialized correctly!");
|
||||
return;
|
||||
}
|
||||
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
|
||||
}
|
||||
|
||||
int PayloadDeliverer::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 1) {
|
||||
// Tests the basic payload open / close ability
|
||||
if (strcmp(argv[0], "gripper_test") == 0) {
|
||||
get_instance()->gripper_test();
|
||||
return 0;
|
||||
|
||||
} else if (strcmp(argv[0], "gripper_open") == 0) {
|
||||
get_instance()->gripper_open();
|
||||
return 0;
|
||||
|
||||
} else if (strcmp(argv[0], "gripper_close") == 0) {
|
||||
get_instance()->gripper_close();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return print_usage("Unrecognized command");
|
||||
}
|
||||
|
||||
int PayloadDeliverer::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Handles payload delivery with either Gripper or a Winch with an appropriate timeout / feedback sensor setting,
|
||||
and communicates back the delivery result as an acknowledgement internally
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("payload_deliverer", "command");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_test", "Tests the Gripper's release & grabbing sequence");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_open", "Opens the gripper");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_close", "Closes the gripper");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PayloadDeliverer::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
PayloadDeliverer *instance = new PayloadDeliverer();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Alloc failed");
|
||||
}
|
||||
|
||||
// Cleanup instance in memory and mark this module as invalid to run
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int payload_deliverer_main(int argc, char *argv[])
|
||||
{
|
||||
return PayloadDeliverer::main(argc, argv);
|
||||
}
|
||||
|
|
@ -0,0 +1,137 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file payload_deliverer.h
|
||||
* @author Junwoo Hwang (junwoo091400@gmail.com)
|
||||
*/
|
||||
#include "gripper.h"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
extern "C" __EXPORT int payload_deliverer_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @brief Payload Deliverer Module
|
||||
*
|
||||
* Activates a winch / gripper when the DO_WINCH or DO_GRIPPER vehicle command is received,
|
||||
* after which the vehicle_command_ack command gets sent upon successful confirmation
|
||||
* of the payload deployment.
|
||||
*
|
||||
* The module runs based on having a subscription callback mechanism to the 'vehicle_command' topic
|
||||
* as it only needs to run when we receive the relevant vehicle command.
|
||||
*
|
||||
* This module communicates with the Navigator which handles publishing such vehicle commands.
|
||||
*/
|
||||
class PayloadDeliverer : public ModuleBase<PayloadDeliverer>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
PayloadDeliverer();
|
||||
|
||||
/** @see ModuleBase **/
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
// Initializes the module
|
||||
bool init();
|
||||
|
||||
// Gripper test commands provoked by custom_command() via CLI interface
|
||||
void gripper_test();
|
||||
void gripper_open();
|
||||
void gripper_close();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Main Run function that runs when subscription callback is triggered
|
||||
*/
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* @brief Called to update the parameters and initialize gripper instance if necessary
|
||||
*/
|
||||
void parameter_update();
|
||||
|
||||
/**
|
||||
* @brief Initialize gripper with current parameter settings
|
||||
*/
|
||||
bool initialize_gripper();
|
||||
|
||||
/**
|
||||
* @brief Commands the payload delivery mechanism based on the received vehicle command
|
||||
*
|
||||
* Also handle vehicle command acknowledgement once the delivery is confirmed from the mechanism
|
||||
*/
|
||||
void handle_vehicle_command(const hrt_abstime &now, const vehicle_command_s *vehicle_command = nullptr);
|
||||
|
||||
/**
|
||||
* @brief Send DO_GRIPPER vehicle command with specified gripper action correctly formatted
|
||||
*
|
||||
* This is useful since vehicle command uses float types for param2, where the gripper action is
|
||||
* specified, but we want to use int32_t data type for it instead, hence filling out the floating
|
||||
* point data structure like integer type is necessary.
|
||||
*
|
||||
* @param gripper_command GRIPPER_ACTION_GRAB or GRIPPER_ACTION_RELEASE
|
||||
*/
|
||||
bool send_gripper_vehicle_command(const int32_t gripper_action);
|
||||
|
||||
Gripper _gripper; // Gripper object to handle gripper action
|
||||
|
||||
// Subscription
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
|
||||
|
||||
// Publications
|
||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::PD_GRIPPER_TO>) _param_gripper_timeout_s,
|
||||
(ParamInt<px4::params::PD_GRIPPER_TYPE>) _param_gripper_type,
|
||||
(ParamBool<px4::params::PD_GRIPPER_EN>) _param_gripper_enable
|
||||
)
|
||||
};
|
||||
Loading…
Reference in New Issue