From d6dc8864db582951634586978d0275c34a23ca87 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Fri, 22 Jan 2021 14:50:46 -0500 Subject: [PATCH] AP_Camera: Privatize AP_Camera --- libraries/AP_Camera/AP_Camera.cpp | 6 +-- libraries/AP_Camera/AP_Camera.h | 6 +++ libraries/AP_Camera/AP_Camera_Logging.cpp | 51 +++++++++++++++++++++++ libraries/AP_Camera/LogStructure.h | 41 ++++++++++++++++++ 4 files changed, 101 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Camera/AP_Camera_Logging.cpp create mode 100644 libraries/AP_Camera/LogStructure.h diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index c9247b0467..3aae01ec9b 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/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); } } } diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index ab6558bdd7..82ea7d6cff 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -4,6 +4,7 @@ #include #include +#include #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 ¤t_loc; diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp new file mode 100644 index 0000000000..7b27d9c9a3 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -0,0 +1,51 @@ +#include "AP_Camera.h" +#include + +// 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(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); +} \ No newline at end of file diff --git a/libraries/AP_Camera/LogStructure.h b/libraries/AP_Camera/LogStructure.h new file mode 100644 index 0000000000..90fb14504a --- /dev/null +++ b/libraries/AP_Camera/LogStructure.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +#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" },