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_Mount.cpp
Views: 1798
1
#include "AP_Camera_Mount.h"
2
3
#if AP_CAMERA_MOUNT_ENABLED
4
#include <AP_Mount/AP_Mount.h>
5
6
extern const AP_HAL::HAL& hal;
7
8
// entry point to actually take a picture. returns true on success
9
bool AP_Camera_Mount::trigger_pic()
10
{
11
AP_Mount* mount = AP::mount();
12
if (mount != nullptr) {
13
return mount->take_picture(get_mount_instance());
14
}
15
return false;
16
}
17
18
// start/stop recording video. returns true on success
19
// start_recording should be true to start recording, false to stop recording
20
bool AP_Camera_Mount::record_video(bool start_recording)
21
{
22
AP_Mount* mount = AP::mount();
23
if (mount != nullptr) {
24
return mount->record_video(get_mount_instance(), start_recording);
25
}
26
return false;
27
}
28
29
// set zoom specified as a rate or percentage
30
bool AP_Camera_Mount::set_zoom(ZoomType zoom_type, float zoom_value)
31
{
32
AP_Mount* mount = AP::mount();
33
if (mount != nullptr) {
34
return mount->set_zoom(get_mount_instance(), zoom_type, zoom_value);
35
}
36
return false;
37
}
38
39
// set focus specified as rate, percentage or auto
40
// focus in = -1, focus hold = 0, focus out = 1
41
SetFocusResult AP_Camera_Mount::set_focus(FocusType focus_type, float focus_value)
42
{
43
AP_Mount* mount = AP::mount();
44
if (mount != nullptr) {
45
return mount->set_focus(get_mount_instance(), focus_type, focus_value);
46
}
47
return SetFocusResult::FAILED;
48
}
49
50
// set tracking to none, point or rectangle (see TrackingType enum)
51
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
52
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
53
bool AP_Camera_Mount::set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
54
{
55
AP_Mount* mount = AP::mount();
56
if (mount != nullptr) {
57
return mount->set_tracking(get_mount_instance(), tracking_type, p1, p2);
58
}
59
return false;
60
}
61
62
63
// set camera lens as a value from 0 to 5
64
bool AP_Camera_Mount::set_lens(uint8_t lens)
65
{
66
AP_Mount* mount = AP::mount();
67
if (mount != nullptr) {
68
return mount->set_lens(get_mount_instance(), lens);
69
}
70
return false;
71
}
72
73
#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
74
// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
75
bool AP_Camera_Mount::set_camera_source(AP_Camera::CameraSource primary_source, AP_Camera::CameraSource secondary_source)
76
{
77
AP_Mount* mount = AP::mount();
78
if (mount != nullptr) {
79
return mount->set_camera_source(get_mount_instance(), (uint8_t)primary_source, (uint8_t)secondary_source);
80
}
81
return false;
82
}
83
#endif
84
85
// send camera information message to GCS
86
void AP_Camera_Mount::send_camera_information(mavlink_channel_t chan) const
87
{
88
AP_Mount* mount = AP::mount();
89
if (mount != nullptr) {
90
return mount->send_camera_information(get_mount_instance(), chan);
91
}
92
}
93
94
// send camera settings message to GCS
95
void AP_Camera_Mount::send_camera_settings(mavlink_channel_t chan) const
96
{
97
AP_Mount* mount = AP::mount();
98
if (mount != nullptr) {
99
return mount->send_camera_settings(get_mount_instance(), chan);
100
}
101
}
102
103
// send camera capture status message to GCS
104
void AP_Camera_Mount::send_camera_capture_status(mavlink_channel_t chan) const
105
{
106
AP_Mount* mount = AP::mount();
107
if (mount != nullptr) {
108
return mount->send_camera_capture_status(get_mount_instance(), chan);
109
}
110
}
111
112
#if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
113
// send camera thermal range message to GCS
114
void AP_Camera_Mount::send_camera_thermal_range(mavlink_channel_t chan) const
115
{
116
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
117
AP_Mount* mount = AP::mount();
118
if (mount != nullptr) {
119
mount->send_camera_thermal_range(get_mount_instance(), chan);
120
}
121
#endif
122
}
123
#endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
124
125
#if AP_CAMERA_SCRIPTING_ENABLED
126
// change camera settings not normally used by autopilot
127
bool AP_Camera_Mount::change_setting(CameraSetting setting, float value)
128
{
129
AP_Mount* mount = AP::mount();
130
if (mount != nullptr) {
131
return mount->change_setting(get_mount_instance(), setting, value);
132
}
133
return false;
134
}
135
#endif
136
137
#endif // AP_CAMERA_MOUNT_ENABLED
138
139