Line data    Source code

       1             : #include "AP_Camera_Backend.h"
       2             : #include <AP_Mount/AP_Mount.h>
       3             : #include <AP_Logger/AP_Logger_config.h>
       4             : 
       5             : #if AP_CAMERA_ENABLED && HAL_LOGGING_ENABLED
       6             : 
       7             : #include <AP_Logger/AP_Logger.h>
       8             : #include <AP_GPS/AP_GPS.h>
       9             : #include <AP_AHRS/AP_AHRS.h>
      10             : 
      11             : // Write a Camera packet.  Also writes a Mount packet if available
      12          66 : void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
      13             : {
      14             :     // exit immediately if no logger
      15          66 :     AP_Logger *logger = AP_Logger::get_singleton();
      16          66 :     if (logger == nullptr) {
      17           0 :         return;
      18             :     }
      19             : 
      20             :     // exit immediately if should not log camera messages
      21          66 :     if (!logger->should_log(_frontend.get_log_camera_bit())) {
      22           0 :         return;
      23             :     }
      24             : 
      25          66 :     const AP_AHRS &ahrs = AP::ahrs();
      26             : 
      27          66 :     Location current_loc;
      28          66 :     if (!ahrs.get_location(current_loc)) {
      29             :         // completely ignore this failure!  AHRS will provide its best guess.
      30             :     }
      31             : 
      32          66 :     int32_t altitude_cm = 0;
      33          66 :     int32_t altitude_rel_cm = 0;
      34          66 :     if (current_loc.initialised()) {
      35             :         // ignore failures to get altitude
      36          65 :         IGNORE_RETURN(current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_cm));
      37          65 :         IGNORE_RETURN(current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel_cm));
      38             :     }
      39             : 
      40          66 :     int32_t altitude_gps_cm = 0;
      41          66 :     const AP_GPS &gps = AP::gps();
      42          66 :     if (gps.status() >= AP_GPS_FixType::FIX_3D) {
      43          65 :         if (!gps.location().get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_gps_cm)) {
      44             :             // ignore this problem...
      45             :         }
      46             :     }
      47             : 
      48             :     // if timestamp is zero set to current system time
      49          66 :     if (timestamp_us == 0) {
      50          66 :         timestamp_us = AP_HAL::micros64();
      51             :     }
      52             : 
      53          66 :     const struct log_Camera pkt{
      54             :         LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
      55             :         time_us     : timestamp_us,
      56          66 :         instance    : _instance,
      57          66 :         image_number: image_index,
      58          66 :         gps_time    : gps.time_week_ms(),
      59          66 :         gps_week    : gps.time_week(),
      60          66 :         latitude    : current_loc.lat,
      61          66 :         longitude   : current_loc.lng,
      62             :         altitude    : altitude_cm,
      63             :         altitude_rel: altitude_rel_cm,
      64             :         altitude_gps: altitude_gps_cm,
      65          66 :         roll        : ahrs.get_roll_deg(),
      66          66 :         pitch       : ahrs.get_pitch_deg(),
      67          66 :         yaw         : ahrs.get_yaw_deg()
      68          66 :     };
      69          66 :     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
      70             : 
      71             : #if HAL_MOUNT_ENABLED
      72          66 :     auto *mount = AP_Mount::get_singleton();
      73          66 :     if (mount!= nullptr) {
      74          66 :         mount->write_log(get_mount_instance(), timestamp_us);
      75             :     }
      76             : #endif
      77             : }
      78             : 
      79             : // Write a Camera packet
      80          66 : void AP_Camera_Backend::Write_Camera(uint64_t timestamp_us)
      81             : {
      82          66 :     Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);
      83          66 : }
      84             : 
      85             : // Write a Trigger packet
      86           0 : void AP_Camera_Backend::Write_Trigger()
      87             : {
      88           0 :     Write_CameraInfo(LOG_TRIGGER_MSG, 0);
      89           0 : }
      90             : 
      91             : #endif  // AP_CAMERA_ENABLED && HAL_LOGGING_ENABLED