AC_AttitudeControl: add helper functions to retrieve rotation matrices

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -647,6 +647,43 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
     }
 }
 
+void AC_AttitudeControl::get_rotation_vehicle_to_ned(Matrix3f& m)
+{
+    m = _ahrs.get_dcm_matrix();
+}
+
+void AC_AttitudeControl::get_rotation_ned_to_vehicle(Matrix3f& m)
+{
+    get_rotation_vehicle_to_ned(m);
+    m = m.transposed();
+}
+
+void AC_AttitudeControl::get_rotation_reference_to_ned(Matrix3f& m)
+{
+    m.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
+}
+
+void AC_AttitudeControl::get_rotation_ned_to_reference(Matrix3f& m)
+{
+    get_rotation_reference_to_ned(m);
+    m = m.transposed();
+}
+
+void AC_AttitudeControl::get_rotation_vehicle_to_reference(Matrix3f& m)
+{
+    Matrix3f Tvn;
+    Matrix3f Tnr;
+    get_rotation_vehicle_to_ned(Tvn);
+    get_rotation_ned_to_reference(Tnr);
+    m = Tnr * Tvn;
+}
+
+void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
+{
+    get_rotation_vehicle_to_reference(m);
+    m = m.transposed();
+}
+
 float AC_AttitudeControl::max_rate_step_bf_roll()
 {
     float alpha = _pid_rate_roll.get_filt_alpha();

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -214,6 +214,24 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
+    // Retrieve a rotation matrix from the vehicle body frame to NED earth frame
+    void get_rotation_vehicle_to_ned(Matrix3f& m);
+
+    // Retrieve a rotation matrix from NED earth frame to the vehicle body frame
+    void get_rotation_ned_to_vehicle(Matrix3f& m);
+
+    // Retrieve a rotation matrix from reference (setpoint) body frame to NED earth frame
+    void get_rotation_reference_to_ned(Matrix3f& m);
+
+    // Retrieve a rotation matrix from NED earth frame to reference (setpoint) body frame
+    void get_rotation_ned_to_reference(Matrix3f& m);
+
+    // Retrieve a rotation matrix from vehicle body frame to reference (setpoint) body frame
+    void get_rotation_vehicle_to_reference(Matrix3f& m);
+
+    // Retrieve a rotation matrix from reference (setpoint) body frame to vehicle body frame
+    void get_rotation_reference_to_vehicle(Matrix3f& m);
+
     // Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
     void update_att_target_and_error_roll(float euler_roll_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad);