diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 3fc20d84b6..9adb329c3c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1085,6 +1085,14 @@ void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cms accel_y_cmss = (GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.5f); } +// returns the NED target acceleration vector for attitude control +Vector3f AC_PosControl::get_thrust_vector() const +{ + Vector3f accel_target = get_accel_target(); + accel_target.z = -GRAVITY_MSS * 100.0f; + return accel_target; +} + /// 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 4c43e1defd..4ca9b15aa3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -272,6 +272,7 @@ public: /// get desired roll, pitch which should be fed into stabilize controllers float get_roll() const { return _roll_target; } float get_pitch() const { return _pitch_target; } + Vector3f get_thrust_vector() const; // get_leash_xy - returns horizontal leash length in cm float get_leash_xy() const { return _leash; }