From 2e6e429c04ebe6423c608bcf042fa7e6b5eaba69 Mon Sep 17 00:00:00 2001 From: kouseii Date: Fri, 23 Jun 2017 00:20:14 +0900 Subject: [PATCH] Copter: guided mode supports heading and yaw-rate target --- ArduCopter/Copter.h | 9 ++--- ArduCopter/GCS_Mavlink.cpp | 20 +++++------ ArduCopter/control_guided.cpp | 65 +++++++++++++++++++++++++++-------- 3 files changed, 66 insertions(+), 28 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8e601b1ea8..43e3ac34a0 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -856,10 +856,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 guided_set_destination(const Location_Class& dest_loc); - void guided_set_velocity(const Vector3f& velocity); - void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + 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); 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(); @@ -874,6 +874,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); 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 dac23fc96c..84bfecc408 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1634,12 +1634,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; /* * for future use: * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; - * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; - * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; */ // prepare position @@ -1676,11 +1676,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_destination_posvel(pos_vector, vel_vector); + 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); } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.guided_set_velocity(vel_vector); + copter.guided_set_velocity(vel_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f); } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (!copter.guided_set_destination(pos_vector)) { + if (!copter.guided_set_destination(pos_vector, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) { result = MAV_RESULT_FAILED; } } else { @@ -1712,12 +1712,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; + bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; + bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; /* * for future use: * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE; - * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE; - * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE; */ Vector3f pos_ned; @@ -1755,11 +1755,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } 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)); + 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); } 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)); + 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); } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (!copter.guided_set_destination(pos_ned)) { + if (!copter.guided_set_destination(pos_ned, !yaw_ignore, ToDeg(packet.yaw) * 100.0f, !yaw_rate_ignore, ToDeg(packet.yaw_rate) * 100.0f)) { result = MAV_RESULT_FAILED; } } else { diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 6e97031a45..7767049570 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -23,8 +23,9 @@ struct { float yaw_cd; float yaw_rate_cds; float climb_rate_cms; + bool use_yaw; bool use_yaw_rate; -} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false}; +} static guided_angle_state = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false}; struct Guided_Limit { uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked @@ -177,7 +178,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 Copter::guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -194,6 +195,9 @@ bool Copter::guided_set_destination(const Vector3f& destination) } #endif + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + // no need to check return status because terrain data is not used wp_nav->set_wp_destination(destination, false); @@ -205,7 +209,7 @@ bool Copter::guided_set_destination(const Vector3f& destination) // 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 Copter::guided_set_destination(const Location_Class& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) { // ensure we are in position control mode if (guided_mode != Guided_WP) { @@ -229,19 +233,25 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc) return false; } + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + // log target Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); return true; } // guided_set_velocity - sets guided mode's target velocity -void Copter::guided_set_velocity(const Vector3f& velocity) +void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) { // check we are in velocity control mode if (guided_mode != Guided_Velocity) { guided_vel_control_start(); } + // set yaw state + guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds); + // record velocity target guided_vel_target_cms = velocity; vel_update_time_ms = millis(); @@ -251,12 +261,15 @@ void Copter::guided_set_velocity(const Vector3f& velocity) } // set guided mode posvel target -void Copter::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) { +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) { // 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); + posvel_update_time_ms = millis(); guided_pos_target_cm = destination; guided_vel_target_cms = velocity; @@ -426,9 +439,14 @@ void Copter::guided_pos_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + } else { + if (guided_angle_state.use_yaw_rate) { + // roll & pitch from waypoint controller, yaw rate from GCS + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); + } else { + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); + } } } @@ -480,9 +498,14 @@ void Copter::guided_vel_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + } else { + if (guided_angle_state.use_yaw_rate) { + // roll & pitch from waypoint controller, yaw rate from GCS + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); + } else { + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); + } } } @@ -554,9 +577,14 @@ void Copter::guided_posvel_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + } else { + if (guided_angle_state.use_yaw_rate) { + // roll & pitch from waypoint controller, yaw rate from GCS + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); + } else { + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); + } } } @@ -662,6 +690,15 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto pos_control->set_desired_velocity(curr_vel_des); } +// helper function to set guided yaw state +void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) +{ + guided_angle_state.use_yaw = use_yaw; + guided_angle_state.yaw_cd = yaw_cd; + guided_angle_state.use_yaw_rate = use_yaw_rate; + guided_angle_state.yaw_rate_cds = yaw_rate_cds; +} + // Guided Limit code // guided_limit_clear - clear/turn off guided limits