From 33e0044f8a6386d8cd5a7f0cb6650478e7abedc2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 5 Oct 2024 22:54:47 +0930 Subject: [PATCH] Revert "Testing" This reverts commit 568df981ad50e4b7f25f596c1bfb0aa30941df5b. --- ArduCopter/mode_althold.cpp | 1 - .../AC_AttitudeControl/AC_AttitudeControl.cpp | 25 ------------------- 2 files changed, 26 deletions(-) diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 5757adfa2f7f9..357d701f9593f 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -98,7 +98,6 @@ void ModeAltHold::run() } // call attitude controller - attitude_control->limit_target_thrust_angle(copter.aparm.angle_max); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // run the vertical position controller and set output throttle diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0805b5976d276..83a3727428040 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -2,7 +2,6 @@ #include #include #include -#include extern const AP_HAL::HAL& hal; @@ -339,30 +338,6 @@ void AC_AttitudeControl::limit_target_thrust_angle(float thrust_angle_max_cd) float ang_vel_scalar = MAX( 0.0, 1.0f - (abs(thrust_correction_angle) / (0.25 * thrust_angle_max_cd))); _ang_vel_target *= ang_vel_scalar; ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); - - // Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame - Vector3f att_target_thrust_vec = _attitude_target * thrust_vector_up; // target thrust vector - - // the dot product is used to calculate the current lean angle for use of external functions - float target_thrust_angle = acosf(constrain_float(att_target_thrust_vec * thrust_vector_up, -1.0f, 1.0f)); - - AP::logger().Write("ALIM", - "TimeUS,r,p,y,tr,tp,ty,er,ep,ey,ca,ta", - "sddddddkkkdd", - "F00000000000", - "Qfffffffffff", - AP_HAL::micros64(), - (double)degrees(attitude_body.get_euler_roll()), - (double)degrees(attitude_body.get_euler_pitch()), - (double)wrap_360(degrees(attitude_body.get_euler_yaw())), - (double)degrees(_attitude_target.get_euler_roll()), - (double)degrees(_attitude_target.get_euler_pitch()), - (double)wrap_360(degrees(_attitude_target.get_euler_yaw())), - (double)degrees(gyro.x), - (double)degrees(gyro.y), - (double)degrees(gyro.z), - (double)degrees(thrust_correction_angle), - (double)degrees(target_thrust_angle)); } // The attitude controller works around the concept of the desired attitude, target attitude