From 275c2849cf38d32c46e868e7f3e216c8fee5aee7 Mon Sep 17 00:00:00 2001
From: Josh Henderson <hendjoshsr71@gmail.com>
Date: Fri, 22 Jan 2021 23:26:33 -0500
Subject: [PATCH] AP_Baro: Privatize Logging

---
 libraries/AP_Baro/AP_Baro.cpp         |  2 +-
 libraries/AP_Baro/AP_Baro.h           |  4 +++
 libraries/AP_Baro/AP_Baro_Logging.cpp | 30 ++++++++++++++++++++++
 libraries/AP_Baro/LogStructure.h      | 36 +++++++++++++++++++++++++++
 4 files changed, 71 insertions(+), 1 deletion(-)
 create mode 100644 libraries/AP_Baro/AP_Baro_Logging.cpp
 create mode 100644 libraries/AP_Baro/LogStructure.h

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 789371f681..db5ac95d63 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -895,7 +895,7 @@ void AP_Baro::update(void)
     // logging
 #ifndef HAL_NO_LOGGING
     if (should_log()) {
-        AP::logger().Write_Baro();
+        Write_Baro();
     }
 #endif
 }
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index caab63c901..730636b75a 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -311,6 +311,10 @@ private:
     */
     float wind_pressure_correction(uint8_t instance);
 #endif
+
+    // Logging function
+    void Write_Baro(void);
+    void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
     
 };
 
diff --git a/libraries/AP_Baro/AP_Baro_Logging.cpp b/libraries/AP_Baro/AP_Baro_Logging.cpp
new file mode 100644
index 0000000000..6e84d033e0
--- /dev/null
+++ b/libraries/AP_Baro/AP_Baro_Logging.cpp
@@ -0,0 +1,30 @@
+#include "AP_Baro.h"
+#include <AP_Logger/AP_Logger.h>
+
+void AP_Baro::Write_Baro_instance(uint64_t time_us, uint8_t baro_instance)
+{
+    const struct log_BARO pkt{
+        LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
+        time_us       : time_us,
+        instance      : baro_instance,
+        altitude      : get_altitude(baro_instance),
+        pressure      : get_pressure(baro_instance),
+        temperature   : (int16_t)(get_temperature(baro_instance) * 100 + 0.5f),
+        climbrate     : get_climb_rate(),
+        sample_time_ms: get_last_update(baro_instance),
+        drift_offset  : get_baro_drift_offset(),
+        ground_temp   : get_ground_temperature(),
+        healthy       : (uint8_t)healthy(baro_instance)
+    };
+    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+}
+
+// Write a BARO packet
+void AP_Baro::Write_Baro(void)
+{
+    const uint64_t time_us = AP_HAL::micros64();
+
+    for (uint8_t i=0; i< _num_sensors; i++) {
+        Write_Baro_instance(time_us, i);
+    }
+}
diff --git a/libraries/AP_Baro/LogStructure.h b/libraries/AP_Baro/LogStructure.h
new file mode 100644
index 0000000000..f24b04f5b6
--- /dev/null
+++ b/libraries/AP_Baro/LogStructure.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_BARO \
+    LOG_BARO_MSG
+
+// @LoggerMessage: BARO
+// @Description: Gathered Barometer data
+// @Field: TimeUS: Time since system startup
+// @Field: I: barometer sensor instance number
+// @Field: Alt: calculated altitude
+// @Field: Press: measured atmospheric pressure
+// @Field: Temp: measured atmospheric temperature
+// @Field: CRt: derived climb rate from primary barometer
+// @Field: SMS: time last sample was taken
+// @Field: Offset: raw adjustment of barometer altitude, zeroed on calibration, possibly set by GCS
+// @Field: GndTemp: temperature on ground, specified by parameter or measured while on ground
+// @Field: Health: true if barometer is considered healthy
+struct PACKED log_BARO {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    float   altitude;
+    float   pressure;
+    int16_t temperature;
+    float   climbrate;
+    uint32_t sample_time_ms;
+    float   drift_offset;
+    float   ground_temp;
+    uint8_t healthy;
+};
+
+#define LOG_STRUCTURE_FROM_BARO \
+    { LOG_BARO_MSG, sizeof(log_BARO), \
+      "BARO",  "QBffcfIffB", "TimeUS,I,Alt,Press,Temp,CRt,SMS,Offset,GndTemp,Health", "s#mPOnsmO-", "F-00B0C?0-" },