diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 27fa905bf7..45e0c6a7b7 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -84,6 +84,7 @@ #include #include #include +#include // Configuration #include "defines.h" diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 900b3c60a9..b9a619018b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -696,7 +696,8 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status, copter.avoidance_adsb.handle_msg(msg); } #endif - copter.mode_chase.mavlink_packet_received(msg); + // pass message to follow library + copter.g2.follow.handle_msg(msg); GCS_MAVLINK::packetReceived(status, msg); } @@ -826,6 +827,14 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_command_int_decode(msg, &packet); switch(packet.command) { + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + result = MAV_RESULT_ACCEPTED; + } + break; + case MAV_CMD_DO_SET_HOME: { // assume failure result = MAV_RESULT_FAILED; @@ -945,6 +954,14 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + copter.g2.follow.set_target_sysid((uint8_t)packet.param1); + result = MAV_RESULT_ACCEPTED; + } + break; + case MAV_CMD_CONDITION_YAW: // param1 : target angle [0-360] // param2 : speed during change [deg per second] diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 02859d5f8c..a6c34e33b6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -987,7 +987,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: mode_flowhold.cpp AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold), #endif - + + // @Group: FOLL + // @Path: ../libraries/AP_Follow/AP_Follow.cpp + AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), + AP_GROUPEND }; @@ -1012,6 +1016,7 @@ ParametersG2::ParametersG2(void) #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif + ,follow(copter.ahrs) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f63f6e31cb..3945084d4f 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -581,6 +581,9 @@ public: // we need a pointer to the mode for the G2 table void *mode_flowhold_ptr; #endif + + // follow + AP_Follow follow; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d36d3ab1fb..9915f34c27 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1099,7 +1099,6 @@ public: ModeChase(Copter &copter) : Copter::ModeGuided(copter) { - target_loc.flags.relative_alt = true; } bool init(bool ignore_checks) override; @@ -1112,29 +1111,8 @@ public: bool set_velocity(const Vector3f& velocity_neu); - void mavlink_packet_received(const mavlink_message_t &msg); - protected: const char *name() const override { return "CHASE"; } const char *name4() const override { return "CHAS"; } - -private: - - uint8_t target_srcid = 255; - - Location_Class target_loc; - Vector3f target_vel; - uint32_t target_last_update_ms; - const uint16_t target_update_timeout_ms = 1000; - - float sphere_radius_min = 5.0f; // no closer than this to target - float sphere_radius_max = 50.0f; // give up when further than this - - const float closure_speed = 10.0f; // metres/second - const float distance_slop = 2.0f; // metres - - void run_lonely_mode(); - Copter::Mode *lonely_mode; - }; diff --git a/ArduCopter/mode_chase.cpp b/ArduCopter/mode_chase.cpp index e67a518f67..12c2e7253b 100644 --- a/ArduCopter/mode_chase.cpp +++ b/ArduCopter/mode_chase.cpp @@ -9,9 +9,10 @@ * TODO: "channel 7 option" to lock onto "pointed at" target * TODO: do better in terms of loitering around the moving point; may need a PID? Maybe use loiter controller somehow? * TODO: extrapolate target vehicle position using its velocity and acceleration + * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions */ -#if 1 +#if 0 #define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) #else #define Debug(fmt, args ...) @@ -34,25 +35,6 @@ bool Copter::ModeChase::set_velocity(const Vector3f& velocity_neu) return true; } -void Copter::ModeChase::run_lonely_mode() -{ - if (lonely_mode == nullptr) { - if (copter.mode_loiter.init(false)) { - lonely_mode = &copter.mode_loiter; - } else if(copter.mode_rtl.init(false)) { - lonely_mode = &copter.mode_rtl; - } else { - copter.mode_land.init(false); - lonely_mode = &copter.mode_land; - } - - gcs().send_text(MAV_SEVERITY_INFO, "Chase: Lonely; %s", lonely_mode->name()); - } - - lonely_mode->run(); -} - - void Copter::ModeChase::run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately @@ -65,70 +47,112 @@ void Copter::ModeChase::run() // Note: this is safe from interference from GCSs and companion computer's whose guided mode // position and velocity requests will be ignored while the vehicle is not in guided mode - const uint32_t now = AP_HAL::millis(); - if (now - target_last_update_ms > target_update_timeout_ms) { - return run_lonely_mode(); - } + // variables to be sent to velocity controller + Vector3f desired_velocity; + bool use_yaw = false; + float yaw_cd = 0.0f; - Vector3f to_vehicle = location_3d_diff_NED(_copter.current_loc, target_loc); - Debug("to_vehicle: %f %f %f", to_vehicle.x, to_vehicle.y, to_vehicle.z); - const float distance_to_vehicle = to_vehicle.length(); + Vector3f dist_vec; // vector to lead vehicle + Vector3f dist_vec_offs; // vector to lead vehicle + offset + Vector3f vel_of_target; // velocity of lead vehicle + if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) { + // debug + Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z); - if (distance_to_vehicle > sphere_radius_max) { - return run_lonely_mode(); - } - lonely_mode = nullptr; + // convert dist_vec_offs to cm in NEU + const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f); - const float distance_to_stop = pos_control->get_stopping_distance_xyz() * 0.01f; + // calculate desired velocity vector in cm/s in NEU + desired_velocity.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * pos_control->get_pos_xy_p().kP()); + desired_velocity.y = (vel_of_target.y * 100.0f) + dist_vec_offs_neu.y * pos_control->get_pos_xy_p().kP(); + desired_velocity.z = (-vel_of_target.z * 100.0f) + dist_vec_offs_neu.z * pos_control->get_pos_z_p().kP(); + + // scale desired velocity to stay within horizontal speed limit + float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y)); + if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_speed_xy())) { + const float scalar_xy = pos_control->get_speed_xy() / desired_speed_xy; + desired_velocity.x *= scalar_xy; + desired_velocity.y *= scalar_xy; + desired_speed_xy = pos_control->get_speed_xy(); + } + + // limit desired velocity to be between maximum climb and descent rates + desired_velocity.z = constrain_float(desired_velocity.z, -fabsf(pos_control->get_speed_down()), pos_control->get_speed_up()); + + // unit vector towards target position + Vector3f dir_to_target_neu = dist_vec_offs_neu; + const float dir_to_target_neu_len = dir_to_target_neu.length(); + if (!is_zero(dir_to_target_neu_len)) { + dir_to_target_neu /= dir_to_target_neu_len; + } + + // create horizontal desired velocity vector (required for slow down calculations) + Vector2f desired_velocity_xy(desired_velocity.x, desired_velocity.y); + + // create horizontal unit vector towards target (required for slow down calculations) + Vector2f dir_to_target_xy(desired_velocity_xy.x, desired_velocity_xy.y); + if (!dir_to_target_xy.is_zero()) { + dir_to_target_xy.normalize(); + } + + // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down) + const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length(); + _copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() / 2.0f, desired_velocity_xy, dir_to_target_xy, dist_to_target_xy, _copter.G_Dt); + + // limit the horizontal velocity to prevent fence violations + _copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy(), desired_velocity_xy, G_Dt); + + // copy horizontal velocity limits back to 3d vector + desired_velocity.x = desired_velocity_xy.x; + desired_velocity.y = desired_velocity_xy.y; + + // limit vertical desired_velocity to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down) + const float des_vel_z_max = _copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() / 2.0f, fabsf(dist_vec_offs_neu.z), _copter.G_Dt); + desired_velocity.z = constrain_float(desired_velocity.z, -des_vel_z_max, des_vel_z_max); + + // get avoidance adjusted climb rate + desired_velocity.z = get_avoidance_adjusted_climbrate(desired_velocity.z); + + // calculate vehicle heading + switch (g2.follow.get_yaw_behave()) { + case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: { + Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f); + if (dist_vec_xy.length() > 1.0f) { + yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy); + use_yaw = true; + } + break; + } + + case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: { + float target_hdg = 0.0f;; + if (g2.follow.get_target_heading(target_hdg)) { + yaw_cd = target_hdg * 100.0f; + use_yaw = true; + } + break; + } + + case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { + Vector3f vel_vec(desired_velocity.x, desired_velocity.y, 0.0f); + if (vel_vec.length() > 100.0f) { + yaw_cd = get_bearing_cd(Vector3f(), vel_vec); + use_yaw = true; + } + break; + } + + case AP_Follow::YAW_BEHAVE_NONE: + default: + // do nothing + break; - const float distance_to_move = distance_to_vehicle - sphere_radius_min; - Debug("distance_to_vehicle=%f move=%f stop=%f", - distance_to_vehicle, - distance_to_move, - distance_to_stop); - to_vehicle.normalize(); - if (fabsf(distance_to_move) > distance_to_stop) { - to_vehicle *= closure_speed * 100; // m/s to cm/s (which set_velocity takes) - to_vehicle.z = -to_vehicle.z; // translate to NEU - if (distance_to_move < 0) { - to_vehicle = -to_vehicle; // too close! back up! } - } else { - to_vehicle.x = 0; - to_vehicle.y = 0; - to_vehicle.z = 0; } - to_vehicle += target_vel; // re-use guided mode's velocity controller (takes NEU) - Copter::ModeGuided::set_velocity(to_vehicle); + Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd); Copter::ModeGuided::run(); } - - -void Copter::ModeChase::mavlink_packet_received(const mavlink_message_t &msg) -{ - if (copter.flightmode != &copter.mode_chase) { - return; - } - if (msg.sysid != target_srcid) { - return; - } - if (msg.msgid != MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { - // handle position only for now - return; - } - - mavlink_global_position_int_t packet; - mavlink_msg_global_position_int_decode(&msg, &packet); - target_loc.lat = packet.lat; - target_loc.lng = packet.lon; - target_loc.alt = packet.relative_alt / 10; // mm -> cm - target_vel.x = packet.vx/100.0f; // cm/s to m/s - target_vel.y = packet.vy/100.0f; // cm/s to m/s - target_vel.z = packet.vz/100.0f; // cm/s to m/s - - target_last_update_ms = AP_HAL::millis(); -}