From 7d319f8059a8edf09debaf6fbf4f48b97480c6ff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 27 Nov 2020 17:21:12 +0900 Subject: [PATCH] AP_DAL: add wheelencoder_enabled --- libraries/AP_DAL/AP_DAL.cpp | 2 ++ libraries/AP_DAL/AP_DAL.h | 4 ++++ libraries/AP_DAL/LogStructure.h | 1 + 3 files changed, 7 insertions(+) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 638835a768..c074e5d57a 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #if APM_BUILD_TYPE(APM_BUILD_Replay) #include @@ -65,6 +66,7 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype) _RFRN.available_memory = hal.util->available_memory(); _RFRN.ahrs_trim = ahrs.get_trim(); _RFRN.opticalflow_enabled = AP::opticalflow() && AP::opticalflow()->enabled(); + _RFRN.wheelencoder_enabled = AP::wheelencoder() && (AP::wheelencoder()->num_sensors() > 0); WRITE_REPLAY_BLOCK_IFCHANGED(RFRN, _RFRN, old); // update body conversion diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index e3bd452f5d..780dfa6937 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -192,6 +192,10 @@ public: return _RFRN.opticalflow_enabled; } + bool wheelencoder_enabled(void) const { + return _RFRN.wheelencoder_enabled; + } + // log optical flow data void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 01d8821030..6f60628ac3 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -66,6 +66,7 @@ struct log_RFRN { uint8_t fly_forward:1; uint8_t ahrs_airspeed_sensor_enabled:1; uint8_t opticalflow_enabled:1; + uint8_t wheelencoder_enabled:1; uint8_t _end; };