diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 0f5c985eeb..25881d2f11 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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 { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5dd7f681ba..b26bdeb472 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 690ef22810..0d3dedc5b5 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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