From 361ba989bd10b7bb537be5c4e04938cb3ee21205 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 23 Apr 2021 10:47:17 +0930 Subject: [PATCH] AC_WPNav: Use Pos_Control Heading --- libraries/AC_WPNav/AC_WPNav.cpp | 55 --------------------------------- libraries/AC_WPNav/AC_WPNav.h | 11 ++----- 2 files changed, 2 insertions(+), 64 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 2c16e2d9b6..47533c643a 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -520,26 +520,6 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) } } - // Calculate the turn rate - float turn_rate = 0.0f; - const float target_vel_xy_len = Vector2f(target_vel.x, target_vel.y).length(); - if (is_positive(target_vel_xy_len)) { - const float accel_forward = (target_accel.x * target_vel.x + target_accel.y * target_vel.y + target_accel.z * target_vel.z)/target_vel.length(); - const Vector3f accel_turn = target_accel - target_vel * accel_forward / target_vel.length(); - const float accel_turn_xy_len = Vector2f(accel_turn.x, accel_turn.y).length(); - turn_rate = accel_turn_xy_len / target_vel_xy_len; - if ((accel_turn.y * target_vel.x - accel_turn.x * target_vel.y) < 0.0) { - turn_rate = -turn_rate; - } - } - - // update the target yaw if origin and destination are at least 2m apart horizontally - const Vector2f target_vel_xy(target_vel.x, target_vel.y); - if (target_vel_xy.length() > WPNAV_YAW_VEL_MIN) { - set_yaw_cd(degrees(target_vel_xy.angle()) * 100.0f); - set_yaw_rate_cds(turn_rate*degrees(100.0f)); - } - // successfully advanced along track return true; } @@ -610,41 +590,6 @@ bool AC_WPNav::is_active() const return (AP_HAL::millis() - _wp_last_update) < 200; } -// returns target yaw in centi-degrees (used for wp and spline navigation) -float AC_WPNav::get_yaw() const -{ - if (_flags.wp_yaw_set) { - return _yaw; - } else { - // if yaw has not been set return attitude controller's current target - return _attitude_control.get_att_target_euler_cd().z; - } -} - -// returns target yaw rate in centi-degrees / second (used for wp and spline navigation) -float AC_WPNav::get_yaw_rate_cds() const -{ - if (_flags.wp_yaw_set) { - return _yaw_rate_cds; - } - - // if yaw has not been set return zero turn rate - return 0.0f; -} - -// set heading used for spline and waypoint navigation -void AC_WPNav::set_yaw_cd(float heading_cd) -{ - _yaw = heading_cd; - _flags.wp_yaw_set = true; -} - -// set yaw rate used for spline and waypoint navigation -void AC_WPNav::set_yaw_rate_cds(float yaw_rate_cds) -{ - _yaw_rate_cds = yaw_rate_cds; -} - // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) bool AC_WPNav::get_terrain_offset(float& offset_cm) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 358602a83a..cd5f954b0b 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -23,7 +23,6 @@ #define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity #define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s -#define WPNAV_YAW_VEL_MIN 10 // target velocity must be at least 10cm/s for vehicle's yaw to change class AC_WPNav { @@ -162,8 +161,8 @@ public: /// // get target yaw in centi-degrees (used for wp and spline navigation) - float get_yaw() const; - float get_yaw_rate_cds() const; + float get_yaw() const { return _pos_control.get_yaw_cd(); } + float get_yaw_rate_cds() const { return _pos_control.get_yaw_rate_cds(); } /// set_spline_destination waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated @@ -229,10 +228,6 @@ protected: // returns false if conversion failed (likely because terrain data was not available) bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt); - // set heading used for spline and waypoint navigation - void set_yaw_cd(float heading_cd); - void set_yaw_rate_cds(float yaw_rate_cds); - // helper function to calculate scurve jerk and jerk_time values // updates _scurve_jerk and _scurve_jerk_time void calc_scurve_jerk_and_jerk_time(); @@ -275,8 +270,6 @@ protected: Vector3f _destination; // target destination in cm from ekf origin float _track_error_xy; // horizontal error of the actual position vs the desired position float _track_scalar_dt; // time compression multiplier to slow the progress along the track - float _yaw; // current yaw heading in centi-degrees based on track direction - float _yaw_rate_cds; // current yaw rate in centi-degrees/second based on track curvature // terrain following variables bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin