AC_PrecLand - Correct yaw sign when using LOCAL_FRD frame in landing_target mavlink messages.

Tools/autotest/arducopter.py

@@ -5864,7 +5864,7 @@
             else:
                 # rotate delta_ef from NED to Local FRD
                 rotmat_yaw = Matrix3()
-                rotmat_yaw.rotate_yaw(self.mav.messages["ATTITUDE"].yaw)
+                rotmat_yaw.rotate_yaw(-self.mav.messages["ATTITUDE"].yaw) # negative yaw for earth->body
                 delta_local_frd = rotmat_yaw * delta_ef
                 angle_x = math.atan2(delta_local_frd.y, delta_local_frd.z)
                 angle_y = -math.atan2(delta_local_frd.x, delta_local_frd.z)
@@ -7094,7 +7094,7 @@
 
         # double-notch should do better, but check for within 5%
         if peakdb2 * 1.05 > peakdb1:
             raise NotAchievedException(
                 "Double-notch peak was higher than single-notch peak %fdB > %fdB" %
                 (peakdb2, peakdb1))
 

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -671,7 +671,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
             float roll_rad, pitch_rad, yaw_rad;
             _inertial_data_delayed->Tbn.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
             target_vec_unit_ned = target_vec_unit;
-            target_vec_unit_ned.rotate_xy(-yaw_rad);
+            target_vec_unit_ned.rotate_xy(yaw_rad);
             break;
         }
 

libraries/AP_Math/tests/test_rotations.cpp

@@ -138,6 +138,42 @@ TEST(RotationsTest, TestRotateMatrix)
     }
 }
 
+/* 
+Yaw-Only Rotation test for rotate_xy and full matrix multiplication.
+As used in AC_PrecLand BODY_FRD and LOCAL_FRD -> LOCAL_NED rotations
+Both of these rotation methods should match when roll and pitch = 0
+*/
+TEST(RotationsTest, TestLocalFRD)
+{
+    for (enum Rotation r = ROTATION_NONE;
+         r < ROTATION_MAX;
+         r = (enum Rotation)((uint8_t)r+1)) {
+
+        // Arbituary Unit vec
+        Vector3f vec(1.0f, 2.0f, 3.0f);
+        Vector3f vec2 = vec;
+
+        Matrix3f Tbn, Tbn_yaw;
+        // Create rot matrix from enum
+        Tbn.from_rotation(r);
+
+        // Extract euler components from rotmatrix
+        float roll_rad, pitch_rad, yaw_rad;
+        Tbn.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
+
+        // Create yaw-only rot matrix
+        Tbn_yaw.from_euler(0.0f, 0.0f, yaw_rad);
+
+        // rotate_xy method, as used by LOCAL_FRD in AC_PrecLand
+        vec.rotate_xy(yaw_rad);
+
+        // full matrix multiplication, as used by BODY_FRD in AC_PrecLand
+        vec2 = Tbn_yaw * vec2;
+
+        EXPECT_LE((vec - vec2).length(), 1e-5);  
+    }
+}
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 TEST(RotationsTest, TestFailedGetLinux)
 {