From bbc8eaefd727af506832d54ad52160f202ab1e94 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Thu, 18 Feb 2016 06:57:01 -0800 Subject: [PATCH] Adding new follow target navigation and main states. New follow target topic added. New follow fsm added to the navigator --- msg/follow_target.msg | 11 ++ msg/vehicle_status.msg | 6 +- src/modules/commander/commander.cpp | 4 + src/modules/commander/px4_custom_mode.h | 3 +- .../commander/state_machine_helper.cpp | 22 ++++ src/modules/mavlink/mavlink_messages.cpp | 8 +- src/modules/mavlink/mavlink_receiver.cpp | 29 ++++- src/modules/mavlink/mavlink_receiver.h | 3 + src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/follow_target.cpp | 106 ++++++++++++++++++ src/modules/navigator/follow_target.h | 65 +++++++++++ src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 6 + src/modules/uORB/objects_common.cpp | 5 +- 14 files changed, 266 insertions(+), 8 deletions(-) create mode 100644 msg/follow_target.msg create mode 100644 src/modules/navigator/follow_target.cpp create mode 100644 src/modules/navigator/follow_target.h diff --git a/msg/follow_target.msg b/msg/follow_target.msg new file mode 100644 index 0000000000..56f5ab3390 --- /dev/null +++ b/msg/follow_target.msg @@ -0,0 +1,11 @@ +uint64 timestamp # timestamp, UNIX epoch (GPS synced) +uint8 est_capabilities # Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES) +uint32 lat # target position (deg * 1e7) +uint32 lon # target position (deg * 1e7) +float32 alt # target position +float32[3] velocity # target velocity (AMSL, meters) # target position (0 0 0 for unknown) +float32[3] accel # target acceleration (linear) in earth frame. +float32[4] attitude_q # where the target is facing. +float32[3] rates # (0 0 0 for unknown) +float32[3] pos_cov # uncertainly in earth frame for X, Y and Z. We will need to agree on the exact format here -1 for unknown +uint64 custom_state # A custom vector, can be used to transmit e.g. button states or switches of a tracker device diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2df2ad6d87..4582504c96 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -11,7 +11,8 @@ uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 uint8 MAIN_STATE_AUTO_TAKEOFF = 10 uint8 MAIN_STATE_AUTO_LAND = 11 -uint8 MAIN_STATE_MAX = 12 +uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 +uint8 MAIN_STATE_MAX = 13 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -47,7 +48,8 @@ uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land -uint8 NAVIGATION_STATE_MAX = 19 +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_MAX = 20 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index da7c4726a9..52d58169d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -701,6 +701,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_LAND: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND); break; + case PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET); + break; default: main_ret = TRANSITION_DENIED; @@ -3214,6 +3217,7 @@ set_control_mode() /* override is not ok for the RTL and recovery mode */ control_mode.flag_external_manual_override_ok = false; /* fallthrough */ + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index ebe9c73601..85f2ef590e 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -61,7 +61,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, - PX4_CUSTOM_SUB_MODE_AUTO_RTGS + PX4_CUSTOM_SUB_MODE_AUTO_RTGS, + PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index b87bb778c9..117401a3d1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -359,6 +359,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: @@ -764,6 +765,27 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_FOLLOW_TARGET: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET; + } + break; + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: /* require global position and home */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 905cd718a5..cb461e2b60 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -187,7 +187,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; - + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_FOLLOW_TARGET; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 98481361bc..8de553f2d9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -227,7 +227,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_DISTANCE_SENSOR: handle_message_distance_sensor(msg); break; - + case MAVLINK_MSG_ID_FOLLOW_TARGET: + handle_message_follow_target(msg); + break; default: break; } @@ -1623,6 +1625,30 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) } } +void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) { + mavlink_follow_target_t follow_me_msg; + mavlink_msg_follow_target_decode(msg, &follow_me_msg); + + follow_target_s follow_target_topic = { }; + + follow_target_topic.timestamp = hrt_absolute_time(); + + memcpy(follow_target_topic.accel, follow_me_msg.acc, sizeof(follow_target_topic.accel)); + memcpy(follow_target_topic.velocity, follow_me_msg.acc, sizeof(follow_target_topic.velocity)); + //memcpy(follow_target_topic.attitude_q, follow_me_msg.attitude quaternion, sizeof(follow_target_topic.attitude_q)); + follow_target_topic.lat = follow_me_msg.lat; + follow_target_topic.lon = follow_me_msg.lon; + + if (_follow_me_pub == nullptr) { + _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); + } else { + warnx("publishing follow"); + orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic); + } + + warnx("got message follow"); +} + void MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { @@ -1804,7 +1830,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } - /** * Receive data from UART. */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b62b5cfa4e..dfecf23982 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -74,6 +74,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -136,6 +137,7 @@ private: void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); void handle_message_distance_sensor(mavlink_message_t *msg); + void handle_message_follow_target(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -197,6 +199,7 @@ private: orb_advert_t _manual_pub; orb_advert_t _land_detector_pub; orb_advert_t _time_offset_pub; + orb_advert_t _follow_me_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 0ce9399a27..3ab04c2e23 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -52,6 +52,7 @@ px4_add_module( rcloss.cpp enginefailure.cpp gpsfailure.cpp + follow_target.cpp DEPENDS platforms__common ) diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp new file mode 100644 index 0000000000..64f50f5ec7 --- /dev/null +++ b/src/modules/navigator/follow_target.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 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 followme.cpp + * + * Helper class to track and follow a given position + * + * @author Jimmy Johnson + */ + +#include "follow_target.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "navigator.h" + +FollowTarget::FollowTarget(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _navigator(navigator), + _tracker_motion_position_sub(-1) +{ + /* load initial params */ + updateParams(); +} + +FollowTarget::~FollowTarget() +{ +} + +void +FollowTarget::on_inactive() +{ +} + +void +FollowTarget::on_activation() +{ + if(_tracker_motion_position_sub < 0) { + _tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target)); + } +} + +void +FollowTarget::on_active() { + follow_target_s target; + bool updated; + + orb_check(_tracker_motion_position_sub, &updated); + + if (updated) { + if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { + + /* predict target location*/ + + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + set_idle_item(&_mission_item); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); + } + } + } +} diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h new file mode 100644 index 0000000000..e4d3b93750 --- /dev/null +++ b/src/modules/navigator/follow_target.h @@ -0,0 +1,65 @@ +/*************************************************************************** + * + * Copyright (c) 2016 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 followme.cpp + * + * Helper class to track and follow a given position + * + * @author Jimmy Johnson + */ + +#pragma once + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class FollowTarget : public MissionBlock +{ + Navigator *_navigator; + int _tracker_motion_position_sub; /**< tracker motion subscription */ + +public: + FollowTarget(Navigator *navigator, const char *name); + + ~FollowTarget(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7e93624eb6..a55b8bd6a1 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -66,6 +66,7 @@ #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" +#include "follow_target.h" #include "gpsfailure.h" #include "rcloss.h" #include "geofence.h" @@ -73,7 +74,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 9 +#define NAVIGATOR_MODE_ARRAY_SIZE 10 class Navigator : public control::SuperBlock { @@ -250,6 +251,8 @@ private: (FW only!) */ GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ + FollowTarget _follow_target; + NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index edfbbd10f4..5349943958 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -148,6 +148,7 @@ Navigator::Navigator() : _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), + _follow_target(this, "TAR"), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -166,6 +167,7 @@ Navigator::Navigator() : _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; _navigation_mode_array[8] = &_land; + _navigation_mode_array[9] = &_follow_target; updateParams(); } @@ -499,6 +501,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_follow_target; + break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index d920d9c983..2ee138ea7c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 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 @@ -285,3 +285,6 @@ ORB_DEFINE(qshell_req, struct qshell_req_s); #include "topics/mavlink_log.h" ORB_DEFINE(mavlink_log, struct mavlink_log_s); + +#include "topics/follow_target.h" +ORB_DEFINE(follow_target, struct follow_target_s);