From bbcf0a096875bee354849f001ad3a91035139b71 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 4 Mar 2022 12:31:25 +1100
Subject: [PATCH] AP_Beacon: move logging of Beacon into Beacon library

---
 libraries/AP_Beacon/AP_Beacon.cpp  | 27 ++++++++++++++++++++++
 libraries/AP_Beacon/AP_Beacon.h    |  3 +++
 libraries/AP_Beacon/LogStructure.h | 37 ++++++++++++++++++++++++++++++
 3 files changed, 67 insertions(+)
 create mode 100644 libraries/AP_Beacon/LogStructure.h

diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp
index 68a3718cfa..81ca563a77 100644
--- a/libraries/AP_Beacon/AP_Beacon.cpp
+++ b/libraries/AP_Beacon/AP_Beacon.cpp
@@ -21,6 +21,7 @@
 #include "AP_Beacon_SITL.h"
 
 #include <AP_Common/Location.h>
+#include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -388,6 +389,32 @@ bool AP_Beacon::device_ready(void) const
     return ((_driver != nullptr) && (_type != AP_BeaconType_None));
 }
 
+// Write beacon sensor (position) data
+void AP_Beacon::log()
+{
+    if (!enabled()) {
+        return;
+    }
+    // position
+    Vector3f pos;
+    float accuracy = 0.0f;
+    get_vehicle_position_ned(pos, accuracy);
+
+    const struct log_Beacon pkt_beacon{
+       LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
+       time_us         : AP_HAL::micros64(),
+       health          : (uint8_t)healthy(),
+       count           : (uint8_t)count(),
+       dist0           : beacon_distance(0),
+       dist1           : beacon_distance(1),
+       dist2           : beacon_distance(2),
+       dist3           : beacon_distance(3),
+       posx            : pos.x,
+       posy            : pos.y,
+       posz            : pos.z
+    };
+    AP::logger().WriteBlock(&pkt_beacon, sizeof(pkt_beacon));
+}
 
 // singleton instance
 AP_Beacon *AP_Beacon::_singleton;
diff --git a/libraries/AP_Beacon/AP_Beacon.h b/libraries/AP_Beacon/AP_Beacon.h
index 3bf766c7ee..4d0cdb023d 100644
--- a/libraries/AP_Beacon/AP_Beacon.h
+++ b/libraries/AP_Beacon/AP_Beacon.h
@@ -104,6 +104,9 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    // a method for vehicles to call to make onboard log messages:
+    void log();
+
 private:
 
     static AP_Beacon *_singleton;
diff --git a/libraries/AP_Beacon/LogStructure.h b/libraries/AP_Beacon/LogStructure.h
new file mode 100644
index 0000000000..f0faa8f355
--- /dev/null
+++ b/libraries/AP_Beacon/LogStructure.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_BEACON \
+    LOG_BEACON_MSG
+
+// @LoggerMessage: BCN
+// @Description: Beacon information
+// @Field: TimeUS: Time since system startup
+// @Field: Health: True if beacon sensor is healthy
+// @Field: Cnt: Number of beacons being used
+// @Field: D0: Distance to first beacon
+// @Field: D1: Distance to second beacon
+// @Field: D2: Distance to third beacon
+// @Field: D3: Distance to fourth beacon
+// @Field: PosX: Calculated beacon position, x-axis
+// @Field: PosY: Calculated beacon position, y-axis
+// @Field: PosZ: Calculated beacon position, z-axis
+
+struct PACKED log_Beacon {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t health;
+    uint8_t count;
+    float dist0;
+    float dist1;
+    float dist2;
+    float dist3;
+    float posx;
+    float posy;
+    float posz;
+};
+
+#define LOG_STRUCTURE_FROM_BEACON \
+    { LOG_BEACON_MSG, sizeof(log_Beacon), \
+        "BCN", "QBBfffffff",  "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000", true },