From 26cb645ee0deeeceee8bfe3438b3b7dd7d55cd15 Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Wed, 9 Mar 2016 15:31:50 -0800 Subject: [PATCH] follow me working --- msg/follow_target.msg | 7 ------- src/modules/mavlink/mavlink_receiver.cpp | 25 ++++++++++++------------ src/modules/navigator/follow_target.cpp | 22 ++++++++++----------- src/modules/navigator/follow_target.h | 1 + src/modules/navigator/loiter.cpp | 1 + src/modules/navigator/navigator.h | 12 +++++------- src/modules/navigator/navigator_main.cpp | 22 ++++----------------- 7 files changed, 34 insertions(+), 56 deletions(-) diff --git a/msg/follow_target.msg b/msg/follow_target.msg index d34e48a493..51ed91a28f 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -1,11 +1,4 @@ uint64 timestamp # timestamp, UNIX epoch (GPS synced) -#uint8 est_capabilities # Flags to indicate which quantities the tracker knows about: (POS, VEL, ACCEL, ATT + RATES) float64 lat # target position (deg * 1e7) float64 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/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 585573b3bc..567b08fc54 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -127,6 +127,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _manual_pub(nullptr), _land_detector_pub(nullptr), _time_offset_pub(nullptr), + _follow_me_pub(nullptr), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -1627,27 +1628,27 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) { -// mavlink_follow_target_t follow_target_msg; - follow_target_s follow_target = {}; + mavlink_follow_target_t follow_target_msg; + follow_target_s follow_target_topic = { }; - //mavlink_msg_follow_target_decode(msg, &follow_target_msg); + mavlink_msg_follow_target_decode(msg, &follow_target_msg); - follow_target.timestamp = hrt_absolute_time(); + follow_target_topic.timestamp = hrt_absolute_time(); // memcpy(follow_target_topic.accel, follow_target_msg.acc, sizeof(follow_target_topic.accel)); //memcpy(follow_target_topic.velocity, follow_target_msg.vel, sizeof(follow_target_topic.velocity)); -// follow_target.lat = follow_target_msg.lat*1e-7; -// follow_target.lon = follow_target_msg.lon*1e-7; -// follow_target.alt = follow_target_msg.alt; + follow_target_topic.lat = follow_target_msg.lat*1e-7; + follow_target_topic.lon = follow_target_msg.lon*1e-7; + follow_target_topic.alt = follow_target_msg.alt; if (_follow_me_pub == nullptr) { - _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target); + _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); } else { - warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target.lat, - (double) follow_target.lon, - (double) follow_target.alt); - orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target); + warnx("publishing follow lat = %f lon = %f alt = %f", (double) follow_target_topic.lat, + (double) follow_target_topic.lon, + (double) follow_target_topic.alt); + orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic); } } diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index f2d7add5ec..16ad9da02a 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -51,6 +51,7 @@ #include #include +#include #include #include "navigator.h" @@ -59,7 +60,7 @@ using namespace matrix; FollowTarget::FollowTarget(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _navigator(navigator), - //_tracker_motion_position_sub(-1), + _tracker_motion_position_sub(-1), _param_min_alt(this, "MIS_TAKEOFF_ALT", false), gps_valid(false), _last_message_time(0), @@ -92,6 +93,10 @@ FollowTarget::on_activation() Vector2f vel; vel.setZero(); + if(_tracker_motion_position_sub < 0) { + _tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target)); + } + // inital set point is same as loiter sp set_loiter_item(&_mission_item, _param_min_alt.get()); @@ -121,11 +126,10 @@ FollowTarget::on_active() { _current_vel(1) = _navigator->get_global_position()->vel_e; } - orb_check(_navigator->_tracker_motion_position_sub, &updated); + orb_check(_tracker_motion_position_sub, &updated); if (updated) { - warnx("UPDASTD "); - if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) { + if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { float dt = ((double)(current_time - _last_message_time) * 1e-6); @@ -173,14 +177,8 @@ FollowTarget::on_active() { sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10); } - } - - if ((current_time - _last_message_time) / (1000*1000) > 5) { - // on_activation(); - static int gg = 0; - if(!(gg++%100)) { - warnx("timed out loitering %llu %d", (current_time - _last_message_time) / (1000*1000), _navigator->_tracker_motion_position_sub); - } + } else if (((double)(current_time - _last_message_time) * 1e-6) > 10) { + on_activation(); } if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) { diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 2d616ecded..d5604d43c9 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -63,6 +63,7 @@ public: private: Navigator *_navigator; + int _tracker_motion_position_sub; /**< tracker motion subscription */ control::BlockParamFloat _param_min_alt; matrix::Vector2f pos_pair[2]; matrix::Vector2f gps_pair; diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index c30a95a201..98eeb86abd 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -79,6 +79,7 @@ Loiter::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->current.vx = pos_sp_triplet->current.vy = pos_sp_triplet->current.vz = 0; pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9f2a993517..a55b8bd6a1 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,7 +57,6 @@ #include #include #include -#include #include "navigator_mode.h" #include "mission.h" @@ -75,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 { @@ -183,7 +182,7 @@ public: void increment_mission_instance_count() { _mission_instance_count++; } void set_mission_failure(const char *reason); - int _tracker_motion_position_sub; /**< tracker motion subscription */ + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -203,7 +202,6 @@ private: int _param_update_sub; /**< param update subscription */ int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */ - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; orb_advert_t _geofence_result_pub; @@ -220,7 +218,7 @@ private: mission_item_s _mission_item; /**< current mission item */ navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - follow_target_s _follow_target_pk; + mission_result_s _mission_result; geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; @@ -253,7 +251,7 @@ private: (FW only!) */ GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ -// FollowTarget _follow_target; + FollowTarget _follow_target; NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ @@ -301,7 +299,7 @@ private: * Retrieve vehicle control mode */ void vehicle_control_mode_update(); - void follow_target_update(); + /** * Update parameters */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 848a2aa994..5349943958 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -148,7 +148,7 @@ Navigator::Navigator() : _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), -// _follow_target(this, "TAR"), + _follow_target(this, "TAR"), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -157,7 +157,6 @@ Navigator::Navigator() : _param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false), _mission_cruising_speed(-1.0f) { - _tracker_motion_position_sub = -1; /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; @@ -168,7 +167,7 @@ Navigator::Navigator() : _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; _navigation_mode_array[8] = &_land; - //_navigation_mode_array[9] = nullptr;//&_follow_target; + _navigation_mode_array[9] = &_follow_target; updateParams(); } @@ -213,13 +212,7 @@ Navigator::gps_position_update() void Navigator::sensor_combined_update() { - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); -} - -void -Navigator::follow_target_update() -{ - orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &_follow_target_pk); + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); } void @@ -303,7 +296,6 @@ Navigator::task_main() _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - _tracker_motion_position_sub = orb_subscribe(ORB_ID(follow_target)); /* copy all topics first time */ vehicle_status_update(); @@ -394,12 +386,6 @@ Navigator::task_main() home_position_update(); } - orb_check(_tracker_motion_position_sub, &updated); - if(updated) { - follow_target_update(); - warnx("updated in nav"); - } - orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd; @@ -517,7 +503,7 @@ Navigator::task_main() break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; -// _navigation_mode = &_follow_target; + _navigation_mode = &_follow_target; break; default: _navigation_mode = nullptr;