diff --git a/.gitmodules b/.gitmodules index ec579aaa28..370151cd80 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "mavlink/include/mavlink/v1.0"] path = mavlink/include/mavlink/v1.0 - url = git://github.com/mavlink/c_library.git + url = https://github.com/catch-twenty-two/mavlink.git [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git diff --git a/msg/follow_target.msg b/msg/follow_target.msg index 56f5ab3390..b69643a056 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -1,7 +1,7 @@ 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) +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. diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8de553f2d9..f00ea08f9a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1625,28 +1625,28 @@ 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); - +void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) +{ + mavlink_follow_target_t follow_target_msg; follow_target_s follow_target_topic = { }; + mavlink_msg_follow_target_decode(msg, &follow_target_msg); + 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; + 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_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_topic); } else { - warnx("publishing follow"); + warnx("publishing follow %f %f %f", (double) follow_target_topic.velocity[0], (double) follow_target_topic.velocity[1], (double) follow_target_topic.velocity[2]); orb_publish(ORB_ID(follow_target), _follow_me_pub, &follow_target_topic); } - - warnx("got message follow"); } void diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 64f50f5ec7..4f1aad97c3 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -57,7 +57,8 @@ 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) { /* load initial params */ updateParams(); @@ -90,17 +91,15 @@ FollowTarget::on_active() { if (updated) { if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { - /* predict target location*/ + set_follow_target_item(&_mission_item, _param_min_alt.get(), target); - 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); + /* 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; - 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(); - } + _navigator->set_position_setpoint_triplet_updated(); } } } diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index e4d3b93750..cc489bd895 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -62,4 +62,6 @@ public: virtual void on_active(); +private: + control::BlockParamFloat _param_min_alt; }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6f399eea19..a949cc84c2 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -434,6 +434,39 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } } +void +MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target) +{ + if (_navigator->get_vstatus()->condition_landed) { + /* landed, don't takeoff, but switch to IDLE mode */ + item->nav_cmd = NAV_CMD_IDLE; + + } else { + item->nav_cmd = NAV_CMD_FOLLOW_TARGET; + + /* use current target position */ + + item->lat = target.lat; + item->lon = target.lon; + item->altitude = target.alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } + } + + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; +} + + void MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4580d3fb17..0fea354ec7 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -49,6 +49,7 @@ #include #include #include +#include #include "navigator_mode.h" @@ -116,10 +117,15 @@ protected: */ void set_idle_item(struct mission_item_s *item); - /** - * Convert a mission item to a command - */ - void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); + /** + * Convert a mission item to a command + */ + void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd); + + /** + * set follow_target item + */ + void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s & target); void issue_command(const struct mission_item_s *item); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index c90a9dd864..f7f7bb4158 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -61,6 +61,7 @@ enum NAV_CMD { NAV_CMD_TAKEOFF = 22, NAV_CMD_ROI = 80, NAV_CMD_PATHPLANNING = 81, + NAV_CMD_FOLLOW_TARGET = 194, // temporary placeholder NAV_CMD_DO_JUMP = 177, NAV_CMD_DO_CHANGE_SPEED = 178, NAV_CMD_DO_SET_SERVO=183,