From 46ec70f9fc0b703bf5bd583f1ebc1266f949d220 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Sat, 11 Sep 2021 06:25:53 -0400 Subject: [PATCH] Copter: use vector.xy().length() instead of norm(x,y) --- ArduCopter/autoyaw.cpp | 2 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index ae34ebc46b..6646abb76c 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -11,7 +11,7 @@ float Mode::AutoYaw::roi_yaw() const float Mode::AutoYaw::look_ahead_yaw() { const Vector3f& vel = copter.inertial_nav.get_velocity(); - float speed = norm(vel.x,vel.y); + float speed = vel.xy().length(); // Commanded Yaw to automatically look ahead. if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f; diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 644ee52932..65069b69b0 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -177,7 +177,7 @@ void Copter::update_throttle_mix() // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); - bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD); + bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD; // check for large external disturbance - angle error over 30 degrees const float angle_error = attitude_control->get_att_error_angle_deg(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 1e18b0ab8d..7e86ae028b 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -674,10 +674,10 @@ void Mode::land_run_horizontal_control() // there is any position estimate drift after touchdown. We // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that - float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), + const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); - float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; - float thrust_vector_mag = norm(thrust_vector.x, thrust_vector.y); + const float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; + const float thrust_vector_mag = thrust_vector.xy().length(); if (thrust_vector_mag > thrust_vector_max) { float ratio = thrust_vector_max / thrust_vector_mag; thrust_vector.x *= ratio;