From 004dddbe18dfe60d0ff0d597c7bd4937bdf139fb Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Fri, 22 Jan 2021 14:51:48 -0500 Subject: [PATCH 1/2] AP_Logger: Privatize AP_Camera Logging --- libraries/AP_Logger/AP_Logger.h | 3 -- libraries/AP_Logger/LogFile.cpp | 49 ------------------------------ libraries/AP_Logger/LogStructure.h | 38 ++--------------------- 3 files changed, 3 insertions(+), 87 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index bf72d64b8efdd..2f1b708c3c31a 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -314,9 +314,6 @@ class AP_Logger void Write_Radio(const mavlink_radio_t &packet); void Write_Message(const char *message); void Write_MessageF(const char *fmt, ...); - void Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Camera(const Location ¤t_loc, uint64_t timestamp_us=0); - void Write_Trigger(const Location ¤t_loc); void Write_ESC(uint8_t id, uint64_t time_us, int32_t rpm, uint16_t voltage, uint16_t current, int16_t esc_temp, uint16_t current_tot, int16_t motor_temp, float error_rate = 0.0f); void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct); void Write_ESCStatus(uint64_t time_us, uint8_t id, uint32_t error_count, float voltage, float current, float temperature, int32_t rpm, uint8_t power_pct); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 4ae7570f33e84..dfc3d3723b7c8 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -459,55 +459,6 @@ void AP_Logger::Write_Radio(const mavlink_radio_t &packet) WriteBlock(&pkt, sizeof(pkt)); } -// Write a Camera packet -void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location ¤t_loc, 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 - }; - WriteCriticalBlock(&pkt, sizeof(pkt)); -} - -// Write a Camera packet -void AP_Logger::Write_Camera(const Location ¤t_loc, uint64_t timestamp_us) -{ - Write_CameraInfo(LOG_CAMERA_MSG, current_loc, timestamp_us); -} - -// Write a Trigger packet -void AP_Logger::Write_Trigger(const Location ¤t_loc) -{ - Write_CameraInfo(LOG_TRIGGER_MSG, current_loc, 0); -} - void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance) { const Compass &compass = AP::compass(); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 0e1fd30c4a0c5..68f9fd64089ae 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -123,6 +123,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include +#include // structure used to define logging format struct LogStructure { @@ -480,21 +481,6 @@ struct PACKED log_Radio { uint16_t fixed; }; -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; -}; - struct PACKED log_PID { LOG_PACKET_HEADER; uint64_t time_us; @@ -1058,20 +1044,6 @@ struct PACKED log_PSC { // @Field: PosY: Calculated beacon position, y-axis // @Field: PosZ: Calculated beacon position, z-axis -// @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 - // @LoggerMessage: CESC // @Description: CAN ESC data // @Field: TimeUS: Time since system startup @@ -1748,10 +1720,7 @@ struct PACKED log_PSC { "MAVC", "QBBBHBBffffiifBB","TimeUS,TS,TC,Fr,Cmd,Cur,AC,P1,P2,P3,P4,X,Y,Z,Res,WL", "s---------------", "F---------------" }, \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------" }, \ - { 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" }, \ +LOG_STRUCTURE_FROM_CAMERA \ { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBfB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hfp,Pri", "s#nPOPP----", "F-00B00----" }, \ LOG_STRUCTURE_FROM_BATTMONITOR \ { LOG_MAG_MSG, sizeof(log_MAG), \ @@ -1905,7 +1874,7 @@ enum LogMessages : uint8_t { LOG_MAVLINK_COMMAND_MSG, LOG_RADIO_MSG, LOG_ATRP_MSG, - LOG_CAMERA_MSG, + LOG_IDS_FROM_CAMERA, LOG_TERRAIN_MSG, LOG_GPS_UBX1_MSG, LOG_GPS_UBX2_MSG, @@ -1951,7 +1920,6 @@ enum LogMessages : uint8_t { LOG_MSG_SBPRAWH, LOG_MSG_SBPRAWM, LOG_MSG_SBPEVENT, - LOG_TRIGGER_MSG, LOG_RALLY_MSG, LOG_VISUALODOM_MSG, From e93e978eb05e44b83544dd8cc26bde33c5fa8e5b Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Fri, 22 Jan 2021 14:50:46 -0500 Subject: [PATCH 2/2] 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 c9247b04679f4..3aae01ec9b49c 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 ab6558bdd7c66..82ea7d6cffb69 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 @@ class AP_Camera { 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 0000000000000..7b27d9c9a35ac --- /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 0000000000000..90fb14504a580 --- /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" },