From 199455dc566fcccce928ce754f51bae43fc3b25e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 27 Dec 2017 10:09:11 +1100 Subject: [PATCH] Copter: add chase mode --- ArduCopter/Copter.h | 1 + ArduCopter/GCS_Mavlink.cpp | 2 + ArduCopter/defines.h | 1 + ArduCopter/mode.cpp | 4 ++ ArduCopter/mode.h | 46 +++++++++++++ ArduCopter/mode_chase.cpp | 134 +++++++++++++++++++++++++++++++++++++ 6 files changed, 188 insertions(+) create mode 100644 ArduCopter/mode_chase.cpp diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c0c78e24a4..27fa905bf7 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -975,6 +975,7 @@ private: #if MODE_CIRCLE_ENABLED == ENABLED ModeCircle mode_circle; #endif + ModeChase mode_chase; #if MODE_DRIFT_ENABLED == ENABLED ModeDrift mode_drift; #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 62ae7b6be7..900b3c60a9 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -47,6 +47,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan) case RTL: case LOITER: case AVOID_ADSB: + case CHASE: case GUIDED: case CIRCLE: case POSHOLD: @@ -695,6 +696,7 @@ void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status, copter.avoidance_adsb.handle_msg(msg); } #endif + copter.mode_chase.mavlink_packet_received(msg); GCS_MAVLINK::packetReceived(status, msg); } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a0927071a6..fdd883a68d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -109,6 +109,7 @@ enum control_mode_t { GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder + CHASE = 23, // chase attempts to follow a mavink system id }; enum mode_reason_t { diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 535003d569..ff40164e02 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -56,6 +56,10 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) break; #endif + case CHASE: + ret = &mode_chase; + break; + #if MODE_CIRCLE_ENABLED == ENABLED case CIRCLE: ret = &mode_circle; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index adb8f2ae61..d36d3ab1fb 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1092,3 +1092,49 @@ protected: private: }; + +class ModeChase : public ModeGuided { + +public: + + ModeChase(Copter &copter) : + Copter::ModeGuided(copter) { + target_loc.flags.relative_alt = true; + } + + bool init(bool ignore_checks) override; + void run() override; + + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return false; } + bool is_autopilot() const override { return true; } + + 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 new file mode 100644 index 0000000000..e67a518f67 --- /dev/null +++ b/ArduCopter/mode_chase.cpp @@ -0,0 +1,134 @@ +#include "Copter.h" + +/* + * mode_chase.cpp - chase 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 + * 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 + */ + +#if 1 +#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 ...) +#endif + +// initialise avoid_adsb controller +bool Copter::ModeChase::init(const bool ignore_checks) +{ + // re-use guided mode + return Copter::ModeGuided::init(ignore_checks); +} + +bool Copter::ModeChase::set_velocity(const Vector3f& velocity_neu) +{ + // check flight mode + if (_copter.flightmode != &_copter.mode_chase) { + return false; + } + + 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 + if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { + zero_throttle_and_relax_ac(); + return; + } + + // re-use guided mode's velocity controller + // 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(); + } + + 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(); + + if (distance_to_vehicle > sphere_radius_max) { + return run_lonely_mode(); + } + lonely_mode = nullptr; + + const float distance_to_stop = pos_control->get_stopping_distance_xyz() * 0.01f; + + 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::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(); +}