From f6125b26e8e0c1d136b307fd81d88053bb5ebc93 Mon Sep 17 00:00:00 2001 From: Sachchit Vekaria Date: Sun, 16 Feb 2020 15:12:31 +0530 Subject: [PATCH] Copter: Allowing Terrain Following in Guided Mode Uses frame type of MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT message to decide the value of terrain_alt boolean. --- ArduCopter/GCS_Mavlink.cpp | 4 +++- ArduCopter/mode.h | 2 +- ArduCopter/mode_guided.cpp | 4 ++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e8fad6a222..3723a75a2b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1085,6 +1085,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) */ Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters + bool terrain_alt = false; if(!pos_ignore) { // sanity check location @@ -1102,6 +1103,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) int32_t(packet.alt*100), frame, }; + terrain_alt = (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN); if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { break; } @@ -1124,7 +1126,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } else if (pos_ignore && !vel_ignore && acc_ignore) { copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (!pos_ignore && vel_ignore && acc_ignore) { - copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, terrain_alt); } break; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 131df34a32..30412c9d61 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -784,7 +784,7 @@ public: bool requires_terrain_failsafe() const override { return true; } 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 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 terrain_alt = false); bool set_destination(const Location& 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); bool get_wp(Location &loc) override; 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); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 25313eb7dd..05fc647e7b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -176,7 +176,7 @@ void ModeGuided::angle_control_start() // guided_set_destination - sets guided mode's target destination // Returns true if the fence is enabled and guided waypoint is within the fence // else return false if the waypoint is outside the fence -bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -197,7 +197,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // no need to check return status because terrain data is not used - wp_nav->set_wp_destination(destination, false); + wp_nav->set_wp_destination(destination, terrain_alt); // log target copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());