forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
AP_Camera_Logging.cpp
89 lines (76 loc) · 2.48 KB
/
AP_Camera_Logging.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
#include "AP_Camera_Backend.h"
#include <AP_Mount/AP_Mount.h>
#if AP_CAMERA_ENABLED
#include <AP_Logger/AP_Logger.h>
#include <AP_GPS/AP_GPS.h>
// Write a Camera packet. Also writes a Mount packet if available
void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
{
// exit immediately if no logger
AP_Logger *logger = AP_Logger::get_singleton();
if (logger == nullptr) {
return;
}
// exit immediately if should not log camera messages
if (!logger->should_log(_frontend.get_log_camera_bit())) {
return;
}
const AP_AHRS &ahrs = AP::ahrs();
Location current_loc;
if (!ahrs.get_location(current_loc)) {
// completely ignore this failure! AHRS will provide its best guess.
}
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;
}
// if timestamp is zero set to current system time
if (timestamp_us == 0) {
timestamp_us = AP_HAL::micros64();
}
const struct log_Camera pkt{
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
time_us : timestamp_us,
instance : _instance,
image_number: image_index,
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));
#if HAL_MOUNT_ENABLED
auto *mount = AP_Mount::get_singleton();
if (mount!= nullptr) {
// assuming camera instance matches mount instance
mount->write_log(_instance, timestamp_us);
}
#endif
}
// Write a Camera packet
void AP_Camera_Backend::Write_Camera(uint64_t timestamp_us)
{
Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);
}
// Write a Trigger packet
void AP_Camera_Backend::Write_Trigger()
{
Write_CameraInfo(LOG_TRIGGER_MSG, 0);
}
#endif