diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 4937f2df63..3aa27e6941 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -29,6 +29,7 @@ //#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support //#define MODE_CIRCLE_ENABLED DISABLED // disable circle mode support //#define MODE_DRIFT_ENABLED DISABLED // disable drift mode support +//#define MODE_FOLLOW_ENABLED DISABLED // disable follow mode support //#define MODE_GUIDED_ENABLED DISABLED // disable guided mode support //#define MODE_GUIDED_NOGPS_ENABLED DISABLED // disable guided/nogps mode support //#define MODE_LOITER_ENABLED DISABLED // disable loiter mode support diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2d9bc687cf..e3fd9df9f7 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -84,7 +84,6 @@ #include #include #include -#include // Configuration #include "defines.h" @@ -121,6 +120,9 @@ #if ADSB_ENABLED == ENABLED # include #endif +#if MODE_FOLLOW_ENABLED == ENABLED + # include +#endif #if AC_FENCE == ENABLED # include #endif @@ -980,7 +982,9 @@ private: ModeDrift mode_drift; #endif ModeFlip mode_flip; +#if MODE_FOLLOW_ENABLED == ENABLED ModeFollow mode_follow; +#endif #if MODE_GUIDED_ENABLED == ENABLED ModeGuided mode_guided; #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 29131f08be..eed69773dc 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -696,8 +696,10 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status, copter.avoidance_adsb.handle_msg(msg); } #endif +#if MODE_FOLLOW_ENABLED == ENABLED // pass message to follow library copter.g2.follow.handle_msg(msg); +#endif GCS_MAVLINK::packetReceived(status, msg); } @@ -828,11 +830,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) switch(packet.command) { case MAV_CMD_DO_FOLLOW: +#if MODE_FOLLOW_ENABLED == ENABLED // 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; } +#endif break; case MAV_CMD_DO_SET_HOME: { @@ -955,11 +959,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_FOLLOW: +#if MODE_FOLLOW_ENABLED == ENABLED // 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; } +#endif break; case MAV_CMD_CONDITION_YAW: diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a6c34e33b6..c070a3a337 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -297,42 +297,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: Flight Mode 2 // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: Flight Mode 3 // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: Flight Mode 4 // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: Flight Mode 5 // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: Flight Mode 6 // @Description: Flight mode when Channel 5 pwm is >=1750 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:Follow // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), @@ -988,9 +988,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPPTR(mode_flowhold_ptr, "FHLD", 26, ParametersG2, Copter::ModeFlowHold), #endif +#if MODE_FOLLOW_ENABLED == ENABLED // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 27, ParametersG2, AP_Follow), +#endif AP_GROUPEND }; @@ -1016,7 +1018,9 @@ ParametersG2::ParametersG2(void) #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif +#if MODE_FOLLOW_ENABLED == ENABLED ,follow(copter.ahrs) +#endif { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3945084d4f..7a217cad21 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -581,9 +581,10 @@ public: // we need a pointer to the mode for the G2 table void *mode_flowhold_ptr; #endif - +#if MODE_FOLLOW_ENABLED == ENABLED // follow AP_Follow follow; +#endif }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 08e6bde337..3774fbb85c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -291,6 +291,12 @@ # define MODE_DRIFT_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Follow - follow another vehicle or GCS +#ifndef MODE_FOLLOW_ENABLED +# define MODE_FOLLOW_ENABLED !HAL_MINIMIZE_FEATURES +#endif + ////////////////////////////////////////////////////////////////////////////// // Guided mode - control vehicle's position or angles from GCS #ifndef MODE_GUIDED_ENABLED diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 061b5f1b87..4f8378f4d2 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -145,10 +145,12 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) ret = (Copter::Mode *)g2.mode_flowhold_ptr; break; #endif - + +#if MODE_FOLLOW_ENABLED == ENABLED case FOLLOW: ret = &mode_follow; break; +#endif default: break; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d2e670f0a7..6da5e76e74 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -690,7 +690,7 @@ public: void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); bool set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); - void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); + void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true); bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); void limit_clear(); @@ -1098,9 +1098,8 @@ class ModeFollow : public ModeGuided { public: - ModeFollow(Copter &copter) : - Copter::ModeGuided(copter) { - } + // inherit constructor + using Copter::ModeGuided::Mode; bool init(bool ignore_checks) override; void run() override; @@ -1110,10 +1109,10 @@ public: bool allows_arming(bool from_gcs) const override { return false; } bool is_autopilot() const override { return true; } - bool set_velocity(const Vector3f& velocity_neu); - protected: const char *name() const override { return "FOLLOW"; } const char *name4() const override { return "FOLL"; } + + uint32_t last_log_ms; // system time of last time desired velocity was logging }; diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index 959224e23d..66cb1917d7 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -1,9 +1,10 @@ #include "Copter.h" +#if MODE_FOLLOW_ENABLED == ENABLED + /* * mode_follow.cpp - follow another mavlink-enabled vehicle by system id * - * TODO: set ROI yaw mode / point camera at target * TODO: stick control to move around on sphere * TODO: stick control to change sphere diameter * TODO: "channel 7 option" to lock onto "pointed at" target @@ -12,27 +13,26 @@ * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions */ -#if 0 -#define Debug(fmt, args ...) do {::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#if 1 +#define Debug(fmt, args ...) do { \ + gcs().send_text(MAV_SEVERITY_WARNING, fmt, ## args); \ + } while(0) +#elif 0 +#define Debug(fmt, args ...) do { \ + ::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); #else #define Debug(fmt, args ...) #endif -// initialise avoid_adsb controller +// initialise follow mode bool Copter::ModeFollow::init(const bool ignore_checks) { // re-use guided mode - return Copter::ModeGuided::init(ignore_checks); -} - -bool Copter::ModeFollow::set_velocity(const Vector3f& velocity_neu) -{ - // check flight mode - if (_copter.flightmode != &_copter.mode_follow) { + gcs().send_text(MAV_SEVERITY_WARNING, "Follow-mode init"); + if (!g2.follow.enabled()) { return false; } - - return true; + return Copter::ModeGuided::init(ignore_checks); } void Copter::ModeFollow::run() @@ -57,15 +57,21 @@ void Copter::ModeFollow::run() 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); + static uint16_t counter = 0; + counter++; + if (counter >= 400) { + counter = 0; + Debug("dist to veh: %f %f %f", (double)dist_vec.x, (double)dist_vec.y, (double)dist_vec.z); + } // 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); // 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(); + const float kp = g2.follow.get_pos_p().kP(); + desired_velocity.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp); + desired_velocity.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp); + desired_velocity.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp); // scale desired velocity to stay within horizontal speed limit float desired_speed_xy = safe_sqrt(sq(desired_velocity.x) + sq(desired_velocity.y)); @@ -79,7 +85,7 @@ void Copter::ModeFollow::run() // 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 + // unit vector towards target position (i.e. vector to lead vehicle + offset) 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)) { @@ -97,17 +103,17 @@ void Copter::ModeFollow::run() // 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); + copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_accel_xy() * 0.5f, 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); + 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); + const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_accel_z() * 0.5f, 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 @@ -116,7 +122,7 @@ void Copter::ModeFollow::run() // 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); + const 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; @@ -125,7 +131,7 @@ void Copter::ModeFollow::run() } case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: { - float target_hdg = 0.0f;; + float target_hdg = 0.0f; if (g2.follow.get_target_heading(target_hdg)) { yaw_cd = target_hdg * 100.0f; use_yaw = true; @@ -134,7 +140,7 @@ void Copter::ModeFollow::run() } case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: { - Vector3f vel_vec(desired_velocity.x, desired_velocity.y, 0.0f); + const 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; @@ -150,9 +156,17 @@ void Copter::ModeFollow::run() } } + // log output at 10hz + uint32_t now = AP_HAL::millis(); + bool log_request = false; + if ((now - last_log_ms >= 100) || (last_log_ms == 0)) { + log_request = true; + last_log_ms = now; + } // re-use guided mode's velocity controller (takes NEU) - Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd); + Copter::ModeGuided::set_velocity(desired_velocity, use_yaw, yaw_cd, false, 0.0f, false, log_request); Copter::ModeGuided::run(); } +#endif // MODE_FOLLOW_ENABLED == ENABLED diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index cc27ea530a..ba88b8ae2b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -241,7 +241,7 @@ bool Copter::ModeGuided::set_destination(const Location_Class& dest_loc, bool us } // guided_set_velocity - sets guided mode's target velocity -void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool log_request) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { @@ -256,7 +256,9 @@ void Copter::ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, fl vel_update_time_ms = millis(); // log target - copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); + if (log_request) { + copter.Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); + } } // set guided mode posvel target