AC_PrecLand: remove zero terms from math

9 years ago · 0eac5a5c8f
1 changed files with 3 additions and 3 deletions
--- a/libraries/AC_PrecLand/AC_PrecLand.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand.cpp
@ -155,9 +155,9 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
        float sin_theta = sinf(theta);
        float cos_theta = cosf(theta);
-        target_vec_ned.x = axis.x*axis.z*(1.0f - cos_theta) + axis.y*sin_theta;
+        target_vec_ned.x = axis.y*sin_theta;
-        target_vec_ned.y = axis.y*axis.z*(1.0f - cos_theta) - axis.x*sin_theta;
+        target_vec_ned.y = -axis.x*sin_theta;
-        target_vec_ned.z = cos_theta + sq(axis.z)*(1.0f-cos_theta);
+        target_vec_ned.z = cos_theta;
    }
    // rotate into NED frame