diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index bc73b20102f..08c9486df58 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -15,6 +15,7 @@ #include "AP_WheelEncoder.h" #include "WheelEncoder_Quadrature.h" +#include extern const AP_HAL::HAL& hal; @@ -182,6 +183,25 @@ void AP_WheelEncoder::update(void) } } +// log wheel encoder information +void AP_WheelEncoder::Log_Write() +{ + // return immediately if no wheel encoders are enabled + if (!enabled(0) && !enabled(1)) { + return; + } + + struct log_WheelEncoder pkt = { + LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG), + time_us : AP_HAL::micros64(), + distance_0 : get_distance(0), + quality_0 : (uint8_t)get_signal_quality(0), + distance_1 : get_distance(1), + quality_1 : (uint8_t)get_signal_quality(1), + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + // check if an instance is healthy bool AP_WheelEncoder::healthy(uint8_t instance) const { diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.h b/libraries/AP_WheelEncoder/AP_WheelEncoder.h index 7102bba808b..847fc305464 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.h +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.h @@ -62,6 +62,9 @@ public: // update state of all sensors. Should be called from main loop void update(void); + // log data to logger + void Log_Write(); + // return the number of wheel encoder sensor instances uint8_t num_sensors(void) const { return num_instances; }