From 4158c37cfae5b8b96ba45866f91e7f7a9faf82da Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 23 Apr 2021 10:46:58 +0930 Subject: [PATCH] AC_PosControl: Calculate heading --- .../AC_AttitudeControl/AC_PosControl.cpp | 32 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 16 +++++++++- 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9adb329c3c..d870c42c2e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -816,6 +816,8 @@ void AC_PosControl::init_xy_controller() const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd(); _roll_target = att_target_euler_cd.x; _pitch_target = att_target_euler_cd.y; + _yaw_target = att_target_euler_cd.z; + _yaw_rate_target = 0.0f; // initialise I terms from lean angles _pid_vel_xy.reset_filter(); @@ -901,6 +903,8 @@ void AC_PosControl::init_vel_controller_xyz() // set roll, pitch lean angle targets to current attitude _roll_target = _ahrs.roll_sensor; _pitch_target = _ahrs.pitch_sensor; + _yaw_target = _ahrs.yaw_sensor; // todo: this should be thrust vector heading, not yaw. + _yaw_rate_target = 0.0f; _pid_vel_xy.reset_filter(); lean_angles_to_accel(_accel_target.x, _accel_target.y); @@ -1054,6 +1058,7 @@ void AC_PosControl::run_xy_controller(float dt) // update angle targets that will be passed to stabilize controller accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target); + calculate_yaw_and_rate_yaw(); } // get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s @@ -1093,6 +1098,33 @@ Vector3f AC_PosControl::get_thrust_vector() const return accel_target; } +// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s +bool AC_PosControl::calculate_yaw_and_rate_yaw() +{ + // Calculate the turn rate + float turn_rate = 0.0f; + const Vector2f vel_desired_xy(_vel_desired.x, _vel_desired.y); + const Vector2f accel_desired_xy(_accel_desired.x, _accel_desired.y); + const float vel_desired_xy_len = vel_desired_xy.length(); + if (is_positive(vel_desired_xy_len)) { + const float accel_forward = (accel_desired_xy.x * vel_desired_xy.x + accel_desired_xy.y * vel_desired_xy.y)/vel_desired_xy_len; + const Vector2f accel_turn = accel_desired_xy - vel_desired_xy * accel_forward / vel_desired_xy_len; + const float accel_turn_xy_len = accel_turn.length(); + turn_rate = accel_turn_xy_len / vel_desired_xy_len; + if ((accel_turn.y * vel_desired_xy.x - accel_turn.x * vel_desired_xy.y) < 0.0) { + turn_rate = -turn_rate; + } + } + + // update the target yaw if origin and destination are at least 2m apart horizontally + if (vel_desired_xy_len > _speed_cms * 0.05f) { + _yaw_target = degrees(vel_desired_xy.angle()) * 100.0f; + _yaw_rate_target = turn_rate*degrees(100.0f); + return true; + } + return false; +} + /// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 4ca9b15aa3..351c4cc763 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -269,11 +269,23 @@ public: /// throttle targets will be sent directly to the motors void update_vel_controller_xyz(); - /// get desired roll, pitch which should be fed into stabilize controllers + /// get desired roll and pitch to be passed to the attitude controller float get_roll() const { return _roll_target; } float get_pitch() const { return _pitch_target; } + + /// get desired yaw to be passed to the attitude controller + float get_yaw_cd() const { return _yaw_target; } + + /// get desired yaw rate to be passed to the attitude controller + float get_yaw_rate_cds() const { return _yaw_rate_target; } + + + /// get desired roll and pitch to be passed to the attitude controller Vector3f get_thrust_vector() const; + // lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s + bool calculate_yaw_and_rate_yaw(); + // get_leash_xy - returns horizontal leash length in cm float get_leash_xy() const { return _leash; } @@ -406,6 +418,8 @@ protected: // output from controller float _roll_target; // desired roll angle in centi-degrees calculated by position controller float _pitch_target; // desired roll pitch in centi-degrees calculated by position controller + float _yaw_target; // desired yaw in centi-degrees calculated by position controller + float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller // position controller internal variables Vector3f _pos_target; // target location in cm from home