From 554d1ac013a05cc0bf6c27efc8235bfdcba1048a Mon Sep 17 00:00:00 2001 From: Jimmy Johnson Date: Sat, 20 Feb 2016 19:31:08 -0800 Subject: [PATCH] making first setpoint on follow target activation loiter in place --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++- src/modules/navigator/follow_target.cpp | 46 +++++++++++++++--------- src/modules/navigator/follow_target.h | 1 + 3 files changed, 34 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f00ea08f9a..324e21953b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1644,7 +1644,9 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) if (_follow_me_pub == nullptr) { _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); } else { - warnx("publishing follow %f %f %f", (double) follow_target_topic.velocity[0], (double) follow_target_topic.velocity[1], (double) follow_target_topic.velocity[2]); + 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 4f1aad97c3..c3238c89bb 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -79,27 +79,41 @@ FollowTarget::on_activation() 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()); + + convert_mission_item_to_sp(); } void FollowTarget::on_active() { - follow_target_s target; - bool updated; + follow_target_s target; + bool updated; - orb_check(_tracker_motion_position_sub, &updated); + orb_check(_tracker_motion_position_sub, &updated); - if (updated) { - if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { - - set_follow_target_item(&_mission_item, _param_min_alt.get(), target); - - /* convert mission item to current setpoint */ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - _navigator->set_position_setpoint_triplet_updated(); + if (updated) { + if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { + set_follow_target_item(&_mission_item, _param_min_alt.get(), target); + convert_mission_item_to_sp(); + } } - } +} + +void +FollowTarget::convert_mission_item_to_sp() { + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // activate line following in pos control + + pos_sp_triplet->previous.valid = true; + pos_sp_triplet->previous = pos_sp_triplet->current; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index cc489bd895..fe343c2e90 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -64,4 +64,5 @@ public: private: control::BlockParamFloat _param_min_alt; + void convert_mission_item_to_sp(); };