diff --git a/msg/follow_target.msg b/msg/follow_target.msg index b69643a056..d34e48a493 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -1,11 +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) +#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 +#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 324e21953b..585573b3bc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1627,27 +1627,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_topic = { }; +// mavlink_follow_target_t follow_target_msg; + follow_target_s follow_target = {}; - mavlink_msg_follow_target_decode(msg, &follow_target_msg); + //mavlink_msg_follow_target_decode(msg, &follow_target_msg); - follow_target_topic.timestamp = hrt_absolute_time(); + follow_target.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)); + // 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; +// 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; if (_follow_me_pub == nullptr) { - _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); + _follow_me_pub = orb_advertise(ORB_ID(follow_target), &follow_target); } else { - 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); + 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); } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fc516818a5..ecd7a9ffc5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1369,8 +1369,8 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ if (_run_pos_control) { - _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); - _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0) + _pos_sp_triplet.current.vx; + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1) + _pos_sp_triplet.current.vy; } if (_run_alt_control) { diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index b2d0058f11..f2d7add5ec 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -51,7 +51,6 @@ #include #include -#include #include #include "navigator.h" @@ -60,7 +59,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), @@ -73,6 +72,8 @@ FollowTarget::FollowTarget(Navigator *navigator, const char *name) : pos_pair[1].setZero(); _current_vel.setZero(); _steps = 0.0f; + target_dist_x = target_dist_y = 0.0f; + target = {}; } FollowTarget::~FollowTarget() @@ -91,31 +92,42 @@ 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()); - convert_mission_item_to_sp(vel); + update_position_sp(vel); + + target.lat = _navigator->get_global_position()->lat; + target.lon = _navigator->get_global_position()->lon; } void FollowTarget::on_active() { - follow_target_s target; + bool updated; - struct map_projection_reference_s target_ref; - float target_dist_x,target_dist_y; + struct map_projection_reference_s target_ref; uint64_t current_time = hrt_absolute_time(); + // get distance to target + map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + map_projection_project(&target_ref, target.lat, target.lon, &target_dist_x, &target_dist_y); - orb_check(_tracker_motion_position_sub, &updated); + follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5); + + if(follow_target_reached == false) { + _current_vel(0) = _navigator->get_global_position()->vel_n; + _current_vel(1) = _navigator->get_global_position()->vel_e; + } + + orb_check(_navigator->_tracker_motion_position_sub, &updated); if (updated) { - if (orb_copy(ORB_ID(follow_target), _tracker_motion_position_sub, &target) == OK) { + warnx("UPDASTD "); + if (orb_copy(ORB_ID(follow_target), _navigator->_tracker_motion_position_sub, &target) == OK) { + + float dt = ((double)(current_time - _last_message_time) * 1e-6); if ((gps_valid == false) ) { gps_pair(0) = target.lat; @@ -125,34 +137,15 @@ FollowTarget::on_active() { return; } - // get distance to target - - map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - map_projection_project(&target_ref, target.lat, target.lon, &target_dist_x, &target_dist_y); - - follow_target_reached = (sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y) < 5); - - if(follow_target_reached == false && gps_valid == true) { - Vector2f go_to_vel; - - warnx("WP not reached lat %f (%f) lon %f (%f), %f", target.lat, (double)_navigator->get_global_position()->lat, (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y)); - - set_follow_target_item(&_mission_item, _param_min_alt.get(), target); - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - convert_mission_item_to_sp(go_to_vel); - _navigator->get_position_setpoint_triplet()->previous.valid = false; - _navigator->set_position_setpoint_triplet_updated(); - //target_vel - return; - } - // get last gps reference map_projection_init(&target_ref, gps_pair(0), gps_pair(1)); map_projection_project(&target_ref, target.lat, target.lon, &(pos_pair[1](0)), &(pos_pair[1](1))); - target_vel = pos_pair[1]/((double)(current_time - _last_message_time) * 1e-6); + target_vel = pos_pair[1]/(dt); + target_vel(0) += target_dist_x*.1f; + target_vel(1) += target_dist_y*.1f; warnx("tracker x %f y %f m, x %f m/s, y %f m/s gs = %f m/s dis = %f m", (double) pos_pair[1](0), @@ -169,44 +162,63 @@ FollowTarget::on_active() { _last_message_time = current_time; + if(follow_target_reached == false) { + warnx("WP not reached lat %f (%f) lon %f (%f), %f", target.lat, (double)_navigator->get_global_position()->lat, (double)_navigator->get_global_position()->lon, target.lon, (double)sqrtf(target_dist_x * target_dist_x + target_dist_y * target_dist_y)); + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + update_position_sp(target_vel); + return; + } + _steps = fabs((double)((sqrtf(_current_vel(0)*_current_vel(0) + _current_vel(1)*_current_vel(1)) - - sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double)10.0f; - //pos_pair[0] = pos_pair[1]; + sqrtf(target_vel(0)*target_vel(0) + target_vel(1)*target_vel(1))))) / (double) (dt*10); + } } - if ((((double) (current_time - _last_publish_time) * 1e-3) > 100) && (follow_target_reached == true)) { - if(_current_vel(0) <= target_vel(0)) { - _current_vel(0) += _steps; - } + 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); + } + } - if(_current_vel(0) >= target_vel(0)) { - _current_vel(0) -= _steps; - } + if ((((double) (current_time - _last_publish_time) * 1e-3) >= 100) && (follow_target_reached == true)) { - if(_current_vel(1) <= target_vel(1)) { - _current_vel(1) += _steps; - } + if (_current_vel(0) <= target_vel(0)) { + _current_vel(0) += (_steps); + } + + if (_current_vel(0) >= target_vel(0)) { + _current_vel(0) -= (_steps); + } + + if (_current_vel(1) <= target_vel(1)) { + _current_vel(1) += (_steps); + } + + if (_current_vel(1) >= target_vel(1)) { + _current_vel(1) -= (_steps); + } + + update_position_sp(_current_vel); + + warnx("updating x %f m y %f m x %f m/s y %f m/s %f ( uav gs %f)", (double )target_dist_x, (double )target_dist_y, + (double )_current_vel(0), (double )_current_vel(1), (double ) _steps, + (double ) sqrtf(_navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e + _navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n)); - if(_current_vel(1) >= target_vel(1)) { - _current_vel(1) -= _steps; - } - convert_mission_item_to_sp(_current_vel); - _navigator->set_position_setpoint_triplet_updated(); - warnx("updating x %f y %f %f", (double)_current_vel(0), (double)_current_vel(1), (double) _steps); _last_publish_time = current_time; } - } void -FollowTarget::convert_mission_item_to_sp(Vector2f & vel) { +FollowTarget::update_position_sp(Vector2f & vel) { /* 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 = false; + 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->current.vx = vel(0); diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h index 42d4bb1ca4..2d616ecded 100644 --- a/src/modules/navigator/follow_target.h +++ b/src/modules/navigator/follow_target.h @@ -63,7 +63,6 @@ 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; @@ -83,5 +82,9 @@ private: int wp_cnt; matrix::Vector2f _current_vel; matrix::Vector2f target_vel; - void convert_mission_item_to_sp(matrix::Vector2f & vel); + void update_position_sp(matrix::Vector2f & vel); + void loiter(); + float target_dist_x; + float target_dist_y; + follow_target_s target; }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 3390fbd515..a261c2b6ee 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -453,7 +453,7 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea item->lon = target.lon; item->altitude = target.alt; - if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + if (((min_clearance > 0.0f) && (item->altitude < _navigator->get_home_position()->alt + min_clearance)) || PX4_ISFINITE(target.alt)) { item->altitude = _navigator->get_home_position()->alt + min_clearance; } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a55b8bd6a1..9f2a993517 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,6 +57,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -74,7 +75,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 10 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public control::SuperBlock { @@ -182,7 +183,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 */ @@ -202,6 +203,7 @@ 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; @@ -218,7 +220,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; @@ -251,7 +253,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 */ @@ -299,7 +301,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 5349943958..848a2aa994 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,6 +157,7 @@ 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; @@ -167,7 +168,7 @@ Navigator::Navigator() : _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; _navigation_mode_array[8] = &_land; - _navigation_mode_array[9] = &_follow_target; + //_navigation_mode_array[9] = nullptr;//&_follow_target; updateParams(); } @@ -212,7 +213,13 @@ Navigator::gps_position_update() void Navigator::sensor_combined_update() { - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + 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); } void @@ -296,6 +303,7 @@ 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(); @@ -386,6 +394,12 @@ 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; @@ -503,7 +517,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;