From 2b98fd765d1492829b5e8525fa19be304f75b891 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Jul 2017 14:06:18 +0900 Subject: [PATCH] Copter: guided_set_destination accepts relative yaw --- ArduCopter/Copter.h | 10 +++++----- ArduCopter/GCS_Mavlink.cpp | 36 +++++++++++++++++++++++++++++------ ArduCopter/control_guided.cpp | 21 ++++++++++---------- 3 files changed, 46 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1268ea80a8..1626789ace 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -876,10 +876,10 @@ private: void guided_vel_control_start(); void guided_posvel_control_start(); void guided_angle_control_start(); - bool guided_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 guided_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); - void guided_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); - void guided_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 guided_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 guided_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 guided_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 guided_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 guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); void guided_run(); void guided_takeoff_run(); @@ -894,7 +894,7 @@ private: bool guided_limit_check(); bool guided_nogps_init(bool ignore_checks); void guided_nogps_run(); - void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds); + void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); bool land_init(bool ignore_checks); void land_run(); void land_gps_run(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index d736c6bdd6..dcde9e43c3 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1611,13 +1611,25 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } } + // prepare yaw + float yaw_cd = 0.0f; + bool yaw_relative = false; + float yaw_rate_cds = 0.0f; + if (!yaw_ignore) { + yaw_cd = ToDeg(packet.yaw) * 100.0f; + yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; + } + if (!yaw_rate_ignore) { + yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; + } + // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f); + copter.guided_set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_velocity(vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f); + copter.guided_set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (!copter.guided_set_destination(pos_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) { + if (!copter.guided_set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_FAILED; } } else { @@ -1691,12 +1703,24 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) pos_ned = copter.pv_location_to_vector(loc); } + // prepare yaw + float yaw_cd = 0.0f; + bool yaw_relative = false; + float yaw_rate_cds = 0.0f; + if (!yaw_ignore) { + yaw_cd = ToDeg(packet.yaw) * 100.0f; + yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED; + } + if (!yaw_rate_ignore) { + yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f; + } + if (!pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f); + copter.guided_set_destination_posvel(pos_ned, 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.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f); + copter.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) { - if (!copter.guided_set_destination(pos_ned, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) { + if (!copter.guided_set_destination(pos_ned, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_FAILED; } } else { diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 45e255893e..87c47d8a6f 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -177,7 +177,7 @@ void Copter::guided_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 Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) +bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -195,7 +195,7 @@ bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, f #endif // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + guided_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); @@ -208,7 +208,7 @@ bool Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, f // sets guided mode's target from a Location object // returns false if destination could not be set (probably caused by missing terrain data) // or if the fence is enabled and guided waypoint is outside the fence -bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) +bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -233,7 +233,7 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw } // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // log target Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); @@ -241,7 +241,7 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw } // guided_set_velocity - sets guided mode's target velocity -void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) +void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { @@ -249,7 +249,7 @@ void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float y } // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); // record velocity target guided_vel_target_cms = velocity; @@ -260,14 +260,15 @@ void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float y } // set guided mode posvel target -void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) { +void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw) +{ // check we are in velocity control mode if (guided_mode != Guided_PosVel) { guided_posvel_control_start(); } // set yaw state - guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); posvel_update_time_ms = millis(); guided_pos_target_cm = destination; @@ -684,10 +685,10 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto } // helper function to set yaw state and targets -void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) +void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { if (use_yaw) { - set_auto_yaw_look_at_heading(yaw_cd, 0.0f, 0, 0); + set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle); } else if (use_yaw_rate) { set_auto_yaw_rate(yaw_rate_cds); }