Line data Source code
1 : /*
2 : Simulator mixin for MAVLink Camera Protocol v2 peripherals.
3 : */
4 :
5 : #include "SIM_config.h"
6 :
7 : #if AP_SIM_MAVLINKCAMV2_ENABLED
8 :
9 : #include "SIM_MAVLinkCamV2.h"
10 : #include <AP_HAL/AP_HAL.h>
11 : #include <stdio.h>
12 :
13 : namespace SITL {
14 :
15 5 : void MAVLinkCamV2::set_camera_instance(uint8_t instance)
16 : {
17 5 : _camera_compid = MIN(MAV_COMP_ID_CAMERA + instance, (uint8_t)MAV_COMP_ID_CAMERA6);
18 5 : }
19 :
20 332820 : void MAVLinkCamV2::update(const class Aircraft &aircraft)
21 : {
22 332820 : if (camera_vehicle_sysid() == 0) {
23 6114 : return;
24 : }
25 326706 : const uint32_t now_ms = AP_HAL::millis();
26 326706 : if (now_ms - _last_camera_heartbeat_ms >= 1000) {
27 277 : _last_camera_heartbeat_ms = now_ms;
28 277 : send_camera_heartbeat();
29 : }
30 : }
31 :
32 27617 : void MAVLinkCamV2::handle_message(const mavlink_message_t &msg)
33 : {
34 27617 : if (msg.msgid != MAVLINK_MSG_ID_COMMAND_LONG) {
35 27584 : return;
36 : }
37 : mavlink_command_long_t cmd;
38 37 : mavlink_msg_command_long_decode(&msg, &cmd);
39 74 : if (cmd.target_system != camera_vehicle_sysid() ||
40 37 : cmd.target_component != _camera_compid) {
41 4 : return;
42 : }
43 33 : switch ((MAV_CMD)cmd.command) {
44 4 : case MAV_CMD_REQUEST_MESSAGE:
45 4 : if ((uint32_t)cmd.param1 == MAVLINK_MSG_ID_CAMERA_INFORMATION) {
46 4 : send_camera_information(msg.sysid, msg.compid);
47 4 : send_camera_command_ack(msg.sysid, msg.compid,
48 : MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED);
49 0 : } else if ((uint32_t)cmd.param1 == MAVLINK_MSG_ID_CAMERA_SETTINGS) {
50 0 : send_camera_settings();
51 0 : send_camera_command_ack(msg.sysid, msg.compid,
52 : MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED);
53 : } else {
54 0 : send_camera_command_ack(msg.sysid, msg.compid,
55 : MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_DENIED);
56 : }
57 4 : break;
58 17 : case MAV_CMD_IMAGE_START_CAPTURE:
59 17 : _camera.trigger_shutter();
60 17 : ::printf("MAVLinkCamV2[compid=%u]: image captured (shot %u)\n",
61 17 : (unsigned)_camera_compid, (unsigned)_camera.shot_count());
62 17 : send_camera_command_ack(msg.sysid, msg.compid,
63 : MAV_CMD_IMAGE_START_CAPTURE, MAV_RESULT_ACCEPTED);
64 17 : break;
65 6 : case MAV_CMD_SET_CAMERA_ZOOM:
66 6 : if ((uint32_t)cmd.param1 == ZOOM_TYPE_RANGE) {
67 6 : _camera.set_zoom_pct(cmd.param2);
68 6 : ::printf("MAVLinkCamV2[compid=%u]: zoom set to %.1f%%\n",
69 6 : (unsigned)_camera_compid, (double)cmd.param2);
70 6 : send_camera_command_ack(msg.sysid, msg.compid,
71 : MAV_CMD_SET_CAMERA_ZOOM, MAV_RESULT_ACCEPTED);
72 6 : send_camera_settings();
73 : } else {
74 0 : send_camera_command_ack(msg.sysid, msg.compid,
75 : MAV_CMD_SET_CAMERA_ZOOM, MAV_RESULT_DENIED);
76 : }
77 6 : break;
78 6 : case MAV_CMD_SET_CAMERA_FOCUS:
79 6 : if ((uint32_t)cmd.param1 == FOCUS_TYPE_RANGE) {
80 6 : _camera.set_focus_pct(cmd.param2);
81 6 : ::printf("MAVLinkCamV2[compid=%u]: focus set to %.1f%%\n",
82 6 : (unsigned)_camera_compid, (double)cmd.param2);
83 6 : send_camera_command_ack(msg.sysid, msg.compid,
84 : MAV_CMD_SET_CAMERA_FOCUS, MAV_RESULT_ACCEPTED);
85 6 : send_camera_settings();
86 : } else {
87 0 : send_camera_command_ack(msg.sysid, msg.compid,
88 : MAV_CMD_SET_CAMERA_FOCUS, MAV_RESULT_DENIED);
89 : }
90 6 : break;
91 0 : default:
92 0 : send_camera_command_ack(msg.sysid, msg.compid, (MAV_CMD)cmd.command,
93 : MAV_RESULT_UNSUPPORTED);
94 0 : break;
95 : }
96 : }
97 :
98 277 : void MAVLinkCamV2::send_camera_heartbeat()
99 : {
100 277 : mavlink_heartbeat_t hb {};
101 277 : hb.type = MAV_TYPE_CAMERA;
102 277 : hb.autopilot = MAV_AUTOPILOT_INVALID;
103 277 : hb.system_status = MAV_STATE_ACTIVE;
104 277 : hb.mavlink_version = 3;
105 :
106 : mavlink_message_t msg;
107 277 : mavlink_msg_heartbeat_encode_status(
108 277 : camera_vehicle_sysid(), _camera_compid,
109 277 : &camera_mav_status(), &msg, &hb);
110 277 : camera_send_mavlink_message(msg);
111 277 : }
112 :
113 4 : void MAVLinkCamV2::send_camera_information(uint8_t target_sysid, uint8_t target_compid)
114 : {
115 4 : mavlink_camera_information_t info {};
116 4 : info.time_boot_ms = AP_HAL::millis();
117 4 : info.firmware_version = get_camera_firmware_version();
118 4 : info.focal_length = NAN;
119 4 : info.sensor_size_h = NAN;
120 4 : info.sensor_size_v = NAN;
121 4 : info.flags = get_camera_cap_flags();
122 4 : strncpy_noterm((char *)info.vendor_name, get_camera_vendor_name(), sizeof(info.vendor_name));
123 4 : strncpy_noterm((char *)info.model_name, get_camera_model_name(), sizeof(info.model_name));
124 :
125 : mavlink_message_t msg;
126 4 : mavlink_msg_camera_information_encode_status(
127 4 : camera_vehicle_sysid(), _camera_compid,
128 4 : &camera_mav_status(), &msg, &info);
129 4 : camera_send_mavlink_message(msg);
130 4 : }
131 :
132 12 : void MAVLinkCamV2::send_camera_settings()
133 : {
134 12 : mavlink_camera_settings_t settings {};
135 12 : settings.time_boot_ms = AP_HAL::millis();
136 12 : settings.mode_id = CAMERA_MODE_IMAGE;
137 12 : settings.zoomLevel = _camera.zoom_pct();
138 12 : settings.focusLevel = _camera.focus_pct();
139 :
140 : mavlink_message_t msg;
141 12 : mavlink_msg_camera_settings_encode_status(
142 12 : camera_vehicle_sysid(), _camera_compid,
143 12 : &camera_mav_status(), &msg, &settings);
144 12 : camera_send_mavlink_message(msg);
145 12 : }
146 :
147 33 : void MAVLinkCamV2::send_camera_command_ack(uint8_t target_sysid, uint8_t target_compid,
148 : MAV_CMD cmd, MAV_RESULT result)
149 : {
150 33 : mavlink_command_ack_t ack {};
151 33 : ack.command = (uint16_t)cmd;
152 33 : ack.result = (uint8_t)result;
153 33 : ack.progress = 255;
154 33 : ack.target_system = target_sysid;
155 33 : ack.target_component = target_compid;
156 :
157 : mavlink_message_t msg;
158 33 : mavlink_msg_command_ack_encode_status(
159 33 : camera_vehicle_sysid(), _camera_compid,
160 33 : &camera_mav_status(), &msg, &ack);
161 33 : camera_send_mavlink_message(msg);
162 33 : }
163 :
164 : } // namespace SITL
165 :
166 : #endif // AP_SIM_MAVLINKCAMV2_ENABLED
|
