CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_Camera/AP_Camera_Logging.cpp
Views: 1798
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
void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)

13
{

14
    // exit immediately if no logger

15
    AP_Logger *logger = AP_Logger::get_singleton();

16
    if (logger == nullptr) {

17
        return;

18
    }

19


20
    // exit immediately if should not log camera messages

21
    if (!logger->should_log(_frontend.get_log_camera_bit())) {

22
        return;

23
    }

24


25
    const AP_AHRS &ahrs = AP::ahrs();

26


27
    Location current_loc;

28
    if (!ahrs.get_location(current_loc)) {

29
        // completely ignore this failure!  AHRS will provide its best guess.

30
    }

31


32
    int32_t altitude_cm = 0;

33
    if (!current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_cm)) {

34
        // ignore this problem...

35
    }

36
    int32_t altitude_rel_cm = 0;

37
    if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel_cm)) {

38
        // ignore this problem...

39
    }

40


41


42
    int32_t altitude_gps_cm = 0;

43
    const AP_GPS &gps = AP::gps();

44
    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {

45
        if (!gps.location().get_alt_cm(Location::AltFrame::ABSOLUTE, altitude_gps_cm)) {

46
            // ignore this problem...

47
        }

48
    }

49


50
    // if timestamp is zero set to current system time

51
    if (timestamp_us == 0) {

52
        timestamp_us = AP_HAL::micros64();

53
    }

54


55
    const struct log_Camera pkt{

56
        LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),

57
        time_us     : timestamp_us,

58
        instance    : _instance,

59
        image_number: image_index,

60
        gps_time    : gps.time_week_ms(),

61
        gps_week    : gps.time_week(),

62
        latitude    : current_loc.lat,

63
        longitude   : current_loc.lng,

64
        altitude    : altitude_cm,

65
        altitude_rel: altitude_rel_cm,

66
        altitude_gps: altitude_gps_cm,

67
        roll        : (int16_t)ahrs.roll_sensor,

68
        pitch       : (int16_t)ahrs.pitch_sensor,

69
        yaw         : (uint16_t)ahrs.yaw_sensor

70
    };

71
    AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));

72


73
#if HAL_MOUNT_ENABLED

74
    auto *mount = AP_Mount::get_singleton();

75
    if (mount!= nullptr) {

76
        mount->write_log(get_mount_instance(), timestamp_us);

77
    }

78
#endif

79
}

80


81
// Write a Camera packet

82
void AP_Camera_Backend::Write_Camera(uint64_t timestamp_us)

83
{

84
    Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);

85
}

86


87
// Write a Trigger packet

88
void AP_Camera_Backend::Write_Trigger()

89
{

90
    Write_CameraInfo(LOG_TRIGGER_MSG, 0);

91
}

92


93
#endif  // AP_CAMERA_ENABLED && HAL_LOGGING_ENABLED

94


95