Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: add MNT log msg for mount's actual and target angles logging

Browse Source
This commit is contained in:
Asif Khan 2023-07-21 12:05:42 +05:30 committed by Andrew Tridgell
parent 7b83ccfdd1
commit a5d4133a2c
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Camera/AP_Camera_Logging.cpp
View File

@ -1,11 +1,12 @@
#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
// 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
@ -41,9 +42,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam
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 ? timestamp_us : AP_HAL::micros64(),
time_us : timestamp_us,
instance : _instance,
image_number: image_index,
gps_time : gps.time_week_ms(),
@ -58,6 +64,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam
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