AP_AHRS: added set_ekf_type() and DCM logging

libraries/AP_AHRS/AP_AHRS.h

@@ -421,6 +421,11 @@ public:
         return _ekf_type;
     }
 
+    // set the selected ekf type, for RC aux control
+    void set_ekf_type(uint8_t ahrs_type) {
+        _ekf_type.set(ahrs_type);
+    }
+    
     // these are only out here so vehicles can reference them for parameters
 #if HAL_NAVEKF2_AVAILABLE
     NavEKF2 EKF2;
@@ -700,6 +705,7 @@ private:
         EXTERNAL = 11,
 #endif
     };
+
     EKFType active_EKF_type(void) const { return state.active_EKF; }
 
     bool always_use_EKF() const {

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -101,6 +101,22 @@ AP_AHRS_DCM::update()
 
     // remember the last origin for fallback support
     IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
+
+#if HAL_LOGGING_ENABLED
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_log_ms >= 100) {
+        // log DCM at 10Hz
+        last_log_ms = now_ms;
+        AP::logger().WriteStreaming("DCM", "TimeUS,Roll,Pitch,Yaw",
+                                    "sddd",
+                                    "F000",
+                                    "Qfff",
+                                    AP_HAL::micros64(),
+                                    degrees(roll),
+                                    degrees(pitch),
+                                    wrap_360(degrees(yaw)));
+    }
+#endif // HAL_LOGGING_ENABLED
 }
 
 void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -286,6 +286,8 @@ private:
     // pre-calculated trig cache:
     float _sin_yaw;
     float _cos_yaw;
+
+    uint32_t last_log_ms;
 };
 
 #endif  // AP_AHRS_DCM_ENABLED