AP_RangeFinder: handle RFND logging

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -40,6 +40,8 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
+#include <AP_Logger/AP_Logger.h>
+
 extern const AP_HAL::HAL &hal;
 
 // table of user settable parameters
@@ -338,6 +340,8 @@ void RangeFinder::update(void)
             drivers[i]->update_pre_arm_check();
         }
     }
+
+    Log_RFND();
 }
 
 bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend)
@@ -670,4 +674,44 @@ MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotati
     return backend->get_mav_distance_sensor_type();
 }
 
+// Write an RFND (rangefinder) packet
+void RangeFinder::Log_RFND()
+{
+    if (_log_rfnd_bit == uint32_t(-1)) {
+        return;
+    }
+
+    AP_Logger &logger = AP::logger();
+    if (!logger.should_log(_log_rfnd_bit)) {
+        return;
+    }
+
+    const AP_RangeFinder_Backend *s0 = get_backend(0);
+    const AP_RangeFinder_Backend *s1 = get_backend(1);
+    if (s0 == nullptr && s1 == nullptr) {
+        return;
+    }
+
+    struct log_RFND pkt = {
+        LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)),
+        time_us       : AP_HAL::micros64(),
+        dist1         : s0 ? s0->distance_cm() : (uint16_t)0,
+        status1       : s0 ? (uint8_t)s0->status() : (uint8_t)0,
+        orient1       : s0 ? s0->orientation() : ROTATION_NONE,
+        dist2         : s1 ? s1->distance_cm() : (uint16_t)0,
+        status2       : s1 ? (uint8_t)s1->status() : (uint8_t)0,
+        orient2       : s1 ? s1->orientation() : ROTATION_NONE,
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
 RangeFinder *RangeFinder::_singleton;
+
+namespace AP {
+
+RangeFinder *rangefinder()
+{
+    return RangeFinder::get_singleton();
+}
+
+}

libraries/AP_RangeFinder/RangeFinder.h

@@ -104,6 +104,8 @@ public:
     // parameters for each instance
     static const struct AP_Param::GroupInfo var_info[];
 
+    void set_log_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
+
     // Return the number of range finder instances
     uint8_t num_sensors(void) const {
         return num_instances;
@@ -141,6 +143,9 @@ public:
     const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
     uint32_t last_reading_ms(enum Rotation orientation) const;
 
+    // indicate which bit in LOG_BITMASK indicates RFND should be logged
+    void set_rfnd_bit(uint32_t log_rfnd_bit) { _log_rfnd_bit = log_rfnd_bit; }
+
     /*
       set an externally estimated terrain height. Used to enable power
       saving (where available) at high altitudes.
@@ -177,4 +182,11 @@ private:
     void update_instance(uint8_t instance);  
 
     bool _add_backend(AP_RangeFinder_Backend *driver);
+
+    uint32_t _log_rfnd_bit = -1;
+    void Log_RFND();
+};
+
+namespace AP {
+    RangeFinder *rangefinder();
 };