AC_PrecLand: move precision landing logging up into AC_PrecLand

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -210,6 +210,12 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
         _backend->update();
         run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
     }
+
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_log_ms > 40) {  // 25Hz
+        last_log_ms = now;
+        Write_Precland();
+    }
 }
 
 bool AC_PrecLand::target_acquired()
@@ -454,3 +460,38 @@ void AC_PrecLand::run_output_prediction()
     _target_pos_rel_out_NE.x += land_ofs_ned_m.x;
     _target_pos_rel_out_NE.y += land_ofs_ned_m.y;
 }
+
+// Write a precision landing entry
+void AC_PrecLand::Write_Precland()
+{
+    // exit immediately if not enabled
+    if (!enabled()) {
+        return;
+    }
+
+    Vector3f target_pos_meas;
+    Vector2f target_pos_rel;
+    Vector2f target_vel_rel;
+    get_target_position_relative_cm(target_pos_rel);
+    get_target_velocity_relative_cms(target_vel_rel);
+    get_target_position_measurement_cm(target_pos_meas);
+
+    const struct log_Precland pkt {
+        LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG),
+        time_us         : AP_HAL::micros64(),
+        healthy         : healthy(),
+        target_acquired : target_acquired(),
+        pos_x           : target_pos_rel.x,
+        pos_y           : target_pos_rel.y,
+        vel_x           : target_vel_rel.x,
+        vel_y           : target_vel_rel.y,
+        meas_x          : target_pos_meas.x,
+        meas_y          : target_pos_meas.y,
+        meas_z          : target_pos_meas.z,
+        last_meas       : last_backend_los_meas_ms(),
+        ekf_outcount    : ekf_outlier_count(),
+        estimator       : estimator_type()
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+

libraries/AC_PrecLand/AC_PrecLand.h

@@ -156,4 +156,8 @@ private:
         bool    healthy;
     } _backend_state;
     AC_PrecLand_Backend         *_backend;  // pointers to backend precision landing driver
+
+    // write out PREC message to log:
+    void Write_Precland();
+    uint32_t last_log_ms;  // last time we logged
 };

libraries/AC_PrecLand/LogStructure.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_PRECLAND \
+    LOG_PRECLAND_MSG
+
+// @LoggerMessage: PL
+// @Description: Precision Landing messages
+// @Field: TimeUS: Time since system startup
+// @Field: Heal: True if Precision Landing is healthy
+// @Field: TAcq: True if landing target is detected
+// @Field: pX: Target position relative to vehicle, X-Axis (0 if target not found)
+// @Field: pY: Target position relative to vehicle, Y-Axis (0 if target not found)
+// @Field: vX: Target velocity relative to vehicle, X-Axis (0 if target not found)
+// @Field: vY: Target velocity relative to vehicle, Y-Axis (0 if target not found)
+// @Field: mX: Target's relative to origin position as 3-D Vector, X-Axis
+// @Field: mY: Target's relative to origin position as 3-D Vector, Y-Axis
+// @Field: mZ: Target's relative to origin position as 3-D Vector, Z-Axis
+// @Field: LastMeasMS: Time when target was last detected
+// @Field: EKFOutl: EKF's outlier count
+// @Field: Est: Type of estimator used
+
+// precision landing logging
+struct PACKED log_Precland {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t healthy;
+    uint8_t target_acquired;
+    float pos_x;
+    float pos_y;
+    float vel_x;
+    float vel_y;
+    float meas_x;
+    float meas_y;
+    float meas_z;
+    uint32_t last_meas;
+    uint32_t ekf_outcount;
+    uint8_t estimator;
+};
+
+#define LOG_STRUCTURE_FROM_PRECLAND                                     \
+    { LOG_PRECLAND_MSG, sizeof(log_Precland),                           \
+      "PL",    "QBBfffffffIIB",    "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasMS,EKFOutl,Est", "s--mmnnmmms--","F--BBBBBBBC--" },