dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Adding new follow target navigation and main states. New follow target 

topic added. New follow fsm added to the navigator
This commit is contained in:
Jimmy Johnson 2016-02-18 06:57:01 -08:00 committed by Lorenz Meier
parent fc061ea94d
commit bbc8eaefd7
14 changed files with 266 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
msg/follow_target.msg Normal file
View File

@ -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

6
msg/vehicle_status.msg
View File

@ -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

4
src/modules/commander/commander.cpp
View File

@ -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:

3
src/modules/commander/px4_custom_mode.h
View File

@ -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 {

22
src/modules/commander/state_machine_helper.cpp
View File

@ -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 */

8
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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:

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

@ -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.
*/

3
src/modules/mavlink/mavlink_receiver.h
View File

@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_force_setpoint.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#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;

1
src/modules/navigator/CMakeLists.txt
View File

@ -52,6 +52,7 @@ px4_add_module(
rcloss.cpp
enginefailure.cpp
gpsfailure.cpp
follow_target.cpp
DEPENDS
platforms__common
)

106
src/modules/navigator/follow_target.cpp Normal file
View File

@ -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 <catch22@fastmail.net>
*/
#include "follow_target.h"
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/follow_target.h>
#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();
}
}
}
}

65
src/modules/navigator/follow_target.h Normal file
View File

@ -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 <catch22@fastmail.net>
*/
#pragma once
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#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();
};

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

@ -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 */

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

@ -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;

5
src/modules/uORB/objects_common.cpp
View File

@ -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);