AP_Camera: Privatize AP_Camera

libraries/AP_Camera/AP_Camera.cpp

@@ -461,9 +461,9 @@ void AP_Camera::log_picture()
     }
 
     if (!using_feedback_pin()) {
-        logger->Write_Camera(current_loc);
+        Write_Camera();
     } else {
-        logger->Write_Trigger(current_loc);
+        Write_Trigger();
     }
 }
 
@@ -499,7 +499,7 @@ void AP_Camera::update_trigger()
             if (logger->should_log(log_camera_bit)) {
                 uint32_t tdiff = AP_HAL::micros() - timestamp32;
                 uint64_t timestamp = AP_HAL::micros64();
-                logger->Write_Camera(current_loc, timestamp - tdiff);
+                Write_Camera(timestamp - tdiff);
             }
         }
     }

libraries/AP_Camera/AP_Camera.h

@@ -4,6 +4,7 @@
 
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Logger/AP_Logger.h>
 
 #define AP_CAMERA_TRIGGER_DEFAULT_DURATION  10      // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second)
 
@@ -123,6 +124,11 @@ private:
 
     void log_picture();
 
+    // Logging Function
+    void Write_Camera(uint64_t timestamp_us=0);
+    void Write_Trigger(void);
+    void Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us=0);
+
     uint32_t log_camera_bit;
     const struct Location &current_loc;
 

libraries/AP_Camera/AP_Camera_Logging.cpp

@@ -0,0 +1,51 @@
+#include "AP_Camera.h"
+#include <AP_Logger/AP_Logger.h>
+
+// Write a Camera packet
+void AP_Camera::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
+{
+    const AP_AHRS &ahrs = AP::ahrs();
+
+    int32_t altitude, altitude_rel, altitude_gps;
+    if (current_loc.relative_alt) {
+        altitude = current_loc.alt+ahrs.get_home().alt;
+        altitude_rel = current_loc.alt;
+    } else {
+        altitude = current_loc.alt;
+        altitude_rel = current_loc.alt - ahrs.get_home().alt;
+    }
+    const AP_GPS &gps = AP::gps();
+    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        altitude_gps = gps.location().alt;
+    } else {
+        altitude_gps = 0;
+    }
+
+    const struct log_Camera pkt{
+        LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
+        time_us     : timestamp_us?timestamp_us:AP_HAL::micros64(),
+        gps_time    : gps.time_week_ms(),
+        gps_week    : gps.time_week(),
+        latitude    : current_loc.lat,
+        longitude   : current_loc.lng,
+        altitude    : altitude,
+        altitude_rel: altitude_rel,
+        altitude_gps: altitude_gps,
+        roll        : (int16_t)ahrs.roll_sensor,
+        pitch       : (int16_t)ahrs.pitch_sensor,
+        yaw         : (uint16_t)ahrs.yaw_sensor
+    };
+    AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
+}
+
+// Write a Camera packet
+void AP_Camera::Write_Camera(uint64_t timestamp_us)
+{
+    Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);
+}
+
+// Write a Trigger packet
+void AP_Camera::Write_Trigger(void)
+{
+    Write_CameraInfo(LOG_TRIGGER_MSG, 0);
+}

libraries/AP_Camera/LogStructure.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_CAMERA \
+    LOG_CAMERA_MSG, \
+    LOG_TRIGGER_MSG
+
+// @LoggerMessage: CAM,TRIG
+// @Description: Camera shutter information
+// @Field: TimeUS: Time since system startup
+// @Field: GPSTime: milliseconds since start of GPS week
+// @Field: GPSWeek: weeks since 5 Jan 1980
+// @Field: Lat: current latitude
+// @Field: Lng: current longitude
+// @Field: Alt: current altitude
+// @Field: RelAlt: current altitude relative to home
+// @Field: GPSAlt: altitude as reported by GPS
+// @Field: Roll: current vehicle roll
+// @Field: Pitch: current vehicle pitch
+// @Field: Yaw: current vehicle yaw
+struct PACKED log_Camera {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint32_t gps_time;
+    uint16_t gps_week;
+    int32_t  latitude;
+    int32_t  longitude;
+    int32_t  altitude;
+    int32_t  altitude_rel;
+    int32_t  altitude_gps;
+    int16_t  roll;
+    int16_t  pitch;
+    uint16_t yaw;
+};
+
+#define LOG_STRUCTURE_FROM_CAMERA \
+    { LOG_CAMERA_MSG, sizeof(log_Camera), \
+      "CAM", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw", "s--DUmmmddd", "F--GGBBBBBB" }, \
+    { LOG_TRIGGER_MSG, sizeof(log_Camera), \
+      "TRIG", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw", "s--DUmmmddd", "F--GGBBBBBB" },