From 6f59c4ae53f0fc76293b840207b5b0ddd1e9a09c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 29 Jan 2016 09:51:52 +1100 Subject: [PATCH] DataFlash: added GPSAlt to CAM and TRIG log messages useful for people with RTK GPS --- libraries/DataFlash/DataFlash.h | 3 +- libraries/DataFlash/LogFile.cpp | 44 ++++++++++++------------------ libraries/DataFlash/LogStructure.h | 23 ++++------------ 3 files changed, 24 insertions(+), 46 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index f8812ae283..8c7d5e1d97 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -109,6 +109,7 @@ public: bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd); void Log_Write_Radio(const mavlink_radio_t &packet); void Log_Write_Message(const char *message); + void Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc); void Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc); void Log_Write_Trigger(const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc); void Log_Write_ESC(void); @@ -179,4 +180,4 @@ private: const char *_firmware_string; }; -#endif \ No newline at end of file +#endif diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 6f99843b04..735f57fd97 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1538,9 +1538,9 @@ void DataFlash_Class::Log_Write_Radio(const mavlink_radio_t &packet) } // Write a Camera packet -void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc) +void DataFlash_Class::Log_Write_CameraInfo(enum LogMessages msg, const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc) { - int32_t altitude, altitude_rel; + int32_t altitude, altitude_rel, altitude_gps; if (current_loc.flags.relative_alt) { altitude = current_loc.alt+ahrs.get_home().alt; altitude_rel = current_loc.alt; @@ -1548,9 +1548,14 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c altitude = current_loc.alt; altitude_rel = current_loc.alt - ahrs.get_home().alt; } + if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + altitude_gps = gps.location().alt; + } else { + altitude_gps = 0; + } struct log_Camera pkt = { - LOG_PACKET_HEADER_INIT(LOG_CAMERA_MSG), + LOG_PACKET_HEADER_INIT(msg), time_us : AP_HAL::micros64(), gps_time : gps.time_week_ms(), gps_week : gps.time_week(), @@ -1558,6 +1563,7 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c 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 @@ -1565,32 +1571,16 @@ void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, c WriteCriticalBlock(&pkt, sizeof(pkt)); } +// Write a Camera packet +void DataFlash_Class::Log_Write_Camera(const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc) +{ + Log_Write_CameraInfo(LOG_CAMERA_MSG, ahrs, gps, current_loc); +} + // Write a Trigger packet void DataFlash_Class::Log_Write_Trigger(const AP_AHRS &ahrs, const AP_GPS &gps, const Location ¤t_loc) { - int32_t altitude, altitude_rel; - if (current_loc.flags.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; - } - - struct log_Trigger pkt = { - LOG_PACKET_HEADER_INIT(LOG_TRIGGER_MSG), - time_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, - roll : (int16_t)ahrs.roll_sensor, - pitch : (int16_t)ahrs.pitch_sensor, - yaw : (uint16_t)ahrs.yaw_sensor - }; - WriteBlock(&pkt, sizeof(pkt)); + Log_Write_CameraInfo(LOG_TRIGGER_MSG, ahrs, gps, current_loc); } // Write an attitude packet @@ -1804,4 +1794,4 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor) rpm2 : rpm_sensor.get_rpm(1) }; WriteBlock(&pkt, sizeof(pkt)); -} \ No newline at end of file +} diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index 1bee674ca7..ce4b9b177a 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -385,20 +385,7 @@ struct PACKED log_Camera { int32_t longitude; int32_t altitude; int32_t altitude_rel; - int16_t roll; - int16_t pitch; - uint16_t yaw; -}; - -struct PACKED log_Trigger { - 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; @@ -735,7 +722,9 @@ Format characters in the format string for binary log messages { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed" }, \ { LOG_CAMERA_MSG, sizeof(log_Camera), \ - "CAM", "QIHLLeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" }, \ + "CAM", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw" }, \ + { LOG_TRIGGER_MSG, sizeof(log_Camera), \ + "TRIG", "QIHLLeeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,GPSAlt,Roll,Pitch,Yaw" }, \ { LOG_ARSP_MSG, sizeof(log_AIRSPEED), \ "ARSP", "Qffcff", "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset" }, \ { LOG_CURRENT_MSG, sizeof(log_Current), \ @@ -749,9 +738,7 @@ Format characters in the format string for binary log messages { LOG_RFND_MSG, sizeof(log_RFND), \ "RFND", "QCC", "TimeUS,Dist1,Dist2" }, \ { LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \ - "DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }, \ - { LOG_TRIGGER_MSG, sizeof(log_Trigger), \ - "TRIG", "QIHLLeeccC","TimeUS,GPSTime,GPSWeek,Lat,Lng,Alt,RelAlt,Roll,Pitch,Yaw" } + "DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \