2023-02-10 20:27:39 -04:00
|
|
|
#include "AP_Camera_Backend.h"
|
2023-07-21 03:35:42 -03:00
|
|
|
#include <AP_Mount/AP_Mount.h>
|
2022-06-02 05:28:26 -03:00
|
|
|
|
|
|
|
#if AP_CAMERA_ENABLED
|
|
|
|
|
2021-01-22 15:50:46 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2022-08-16 02:07:47 -03:00
|
|
|
#include <AP_GPS/AP_GPS.h>
|
2021-01-22 15:50:46 -04:00
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// Write a Camera packet. Also writes a Mount packet if available
|
2023-02-10 20:27:39 -04:00
|
|
|
void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
|
2021-01-22 15:50:46 -04:00
|
|
|
{
|
2023-02-10 20:27:39 -04:00
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
|
2021-01-22 15:50:46 -04:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
|
|
|
|
2019-03-26 08:06:18 -03:00
|
|
|
Location current_loc;
|
2022-01-20 19:42:40 -04:00
|
|
|
if (!ahrs.get_location(current_loc)) {
|
2019-03-26 08:06:18 -03:00
|
|
|
// completely ignore this failure! AHRS will provide its best guess.
|
|
|
|
}
|
|
|
|
|
2021-01-22 15:50:46 -04:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// if timestamp is zero set to current system time
|
|
|
|
if (timestamp_us == 0) {
|
|
|
|
timestamp_us = AP_HAL::micros64();
|
|
|
|
}
|
|
|
|
|
2021-01-22 15:50:46 -04:00
|
|
|
const struct log_Camera pkt{
|
|
|
|
LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
|
2023-07-21 03:35:42 -03:00
|
|
|
time_us : timestamp_us,
|
2023-02-10 20:27:39 -04:00
|
|
|
instance : _instance,
|
2023-02-22 07:38:36 -04:00
|
|
|
image_number: image_index,
|
2021-01-22 15:50:46 -04:00
|
|
|
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));
|
2023-07-21 03:35:42 -03:00
|
|
|
|
|
|
|
#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
|
2021-01-22 15:50:46 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Write a Camera packet
|
2023-02-10 20:27:39 -04:00
|
|
|
void AP_Camera_Backend::Write_Camera(uint64_t timestamp_us)
|
2021-01-22 15:50:46 -04:00
|
|
|
{
|
|
|
|
Write_CameraInfo(LOG_CAMERA_MSG, timestamp_us);
|
|
|
|
}
|
|
|
|
|
|
|
|
// Write a Trigger packet
|
2023-02-10 20:27:39 -04:00
|
|
|
void AP_Camera_Backend::Write_Trigger()
|
2021-01-22 15:50:46 -04:00
|
|
|
{
|
|
|
|
Write_CameraInfo(LOG_TRIGGER_MSG, 0);
|
2022-06-02 05:28:26 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|