From 66778dbf63e6f0bc2d3209990ae436a393d3df2c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 4 Feb 2019 22:00:40 -0700 Subject: [PATCH] AP_WheelEncoder: move wheelEncoder logging to library --- libraries/AP_WheelEncoder/AP_WheelEncoder.cpp | 20 +++++++++++++++++++ libraries/AP_WheelEncoder/AP_WheelEncoder.h | 3 +++ 2 files changed, 23 insertions(+) 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; }