Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_MAVLINK_ENABLED
4 :
5 : #include "AP_Mount_MAVLink.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <GCS_MAVLink/GCS.h>
9 :
10 : extern const AP_HAL::HAL& hal;
11 :
12 : #define AP_MOUNT_MAVLINK_SEARCH_MS 60000 // search for gimbal for 1 minute after startup
13 : #define AP_MOUNT_MAVLINK_ATTITUDE_INTERVAL_US 20000 // send ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE at 50hz
14 :
15 : // update mount position
16 13247 : void AP_Mount_MAVLink::update()
17 : {
18 13247 : AP_Mount_Backend::update();
19 :
20 : // exit immediately if not initialised
21 13247 : if (!_initialised) {
22 1645 : find_gimbal();
23 1645 : return;
24 : }
25 :
26 11602 : update_mnt_target();
27 :
28 : // send target angles/rates/retract depending on the target type
29 11602 : send_target_to_gimbal();
30 : }
31 :
32 : // return true if healthy
33 108 : bool AP_Mount_MAVLink::healthy() const
34 : {
35 : // unhealthy until gimbal has been found and replied with device info
36 108 : if (_link == nullptr || !_got_device_info) {
37 16 : return false;
38 : }
39 :
40 : // unhealthy if attitude information NOT received within the last second
41 92 : if (AP_HAL::millis() - _last_attitude_status_ms > 1000) {
42 0 : return false;
43 : }
44 :
45 : // check failure flags
46 92 : uint32_t critical_failure_flags = GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR |
47 : GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR |
48 : GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR |
49 : GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR |
50 : GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR;
51 :
52 92 : if ((_gimbal_device_attitude_status.failure_flags & critical_failure_flags) > 0) {
53 0 : return false;
54 : }
55 :
56 : // if we get this far return mount is healthy
57 92 : return true;
58 : }
59 :
60 : // get attitude as a quaternion. returns true on success
61 3989 : bool AP_Mount_MAVLink::get_attitude_quaternion(Quaternion& att_quat)
62 : {
63 3989 : att_quat = _gimbal_device_attitude_status.q;
64 3989 : return true;
65 : }
66 :
67 : // search for gimbal in GCS_MAVLink routing table
68 1645 : void AP_Mount_MAVLink::find_gimbal()
69 : {
70 : // do not look for gimbal for first 10 seconds so user may see banner
71 1645 : uint32_t now_ms = AP_HAL::millis();
72 1645 : if (now_ms < 10000) {
73 1637 : return;
74 : }
75 :
76 : // search for gimbal for 60 seconds or until armed
77 8 : if ((now_ms > AP_MOUNT_MAVLINK_SEARCH_MS) && hal.util->get_soft_armed()) {
78 0 : return;
79 : }
80 :
81 : // search for a mavlink enabled gimbal
82 8 : if (_link == nullptr) {
83 : // we expect that instance 0 has compid = MAV_COMP_ID_GIMBAL, instance 1 has compid = MAV_COMP_ID_GIMBAL2, etc
84 4 : uint8_t compid = (_instance == 0) ? MAV_COMP_ID_GIMBAL : MAV_COMP_ID_GIMBAL2 + (_instance - 1);
85 4 : _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_GIMBAL, compid, _sysid);
86 4 : if (_link == nullptr) {
87 : // have not yet found a gimbal so return
88 0 : return;
89 : }
90 :
91 4 : _compid = compid;
92 : }
93 :
94 : // request GIMBAL_DEVICE_INFORMATION
95 8 : if (!_got_device_info) {
96 4 : if (now_ms - _last_devinfo_req_ms > 1000) {
97 4 : _last_devinfo_req_ms = now_ms;
98 4 : request_gimbal_device_information();
99 : }
100 4 : return;
101 : }
102 :
103 : // start sending autopilot attitude to gimbal
104 4 : if (start_sending_attitude_to_gimbal()) {
105 4 : _initialised = true;
106 : }
107 : }
108 :
109 : // handle GIMBAL_DEVICE_INFORMATION message
110 8 : void AP_Mount_MAVLink::handle_gimbal_device_information(const mavlink_message_t &msg)
111 : {
112 : // exit immediately if this is not our message
113 8 : if (msg.sysid != _sysid || msg.compid != _compid) {
114 4 : return;
115 : }
116 :
117 : mavlink_gimbal_device_information_t info;
118 4 : mavlink_msg_gimbal_device_information_decode(&msg, &info);
119 :
120 : // set parameter defaults from gimbal information
121 4 : if (!isnan(info.roll_min) && !isnan(info.roll_max)) {
122 0 : _params.roll_angle_min.set_default(degrees(info.roll_min));
123 0 : _params.roll_angle_max.set_default(degrees(info.roll_max));
124 : }
125 4 : if (!isnan(info.pitch_min) && !isnan(info.pitch_max)) {
126 4 : _params.pitch_angle_min.set_default(degrees(info.pitch_min));
127 4 : _params.pitch_angle_max.set_default(degrees(info.pitch_max));
128 : }
129 4 : if (!isnan(info.yaw_min) && !isnan(info.yaw_max)) {
130 4 : _params.yaw_angle_min.set_default(degrees(info.yaw_min));
131 4 : _params.yaw_angle_max.set_default(degrees(info.yaw_max));
132 : }
133 :
134 4 : const uint8_t fw_ver_major = info.firmware_version & 0x000000FF;
135 4 : const uint8_t fw_ver_minor = (info.firmware_version & 0x0000FF00) >> 8;
136 4 : const uint8_t fw_ver_revision = (info.firmware_version & 0x00FF0000) >> 16;
137 4 : const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24;
138 :
139 4 : strncpy(vendor_name, info.vendor_name, ARRAY_SIZE(vendor_name));
140 4 : strncpy(model_name, info.model_name, ARRAY_SIZE(model_name));
141 :
142 : // prefer the 32-bit extension, fall back to the 16-bit field:
143 4 : device_capapability_flags = (info.cap_flags2 != 0) ? info.cap_flags2 : info.cap_flags;
144 :
145 : // display gimbal info to user
146 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u",
147 : info.vendor_name,
148 : info.model_name,
149 : (unsigned)fw_ver_major,
150 : (unsigned)fw_ver_minor,
151 : (unsigned)fw_ver_revision,
152 : (unsigned)fw_ver_build);
153 :
154 4 : _got_device_info = true;
155 : }
156 :
157 : // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
158 5400 : void AP_Mount_MAVLink::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
159 : {
160 : // exit immediately if this is not our message
161 5400 : if (msg.sysid != _sysid || msg.compid != _compid) {
162 3080 : return;
163 : }
164 :
165 : // take copy of message so it can be forwarded onto GCS later
166 2320 : mavlink_msg_gimbal_device_attitude_status_decode(&msg, &_gimbal_device_attitude_status);
167 2320 : _last_attitude_status_ms = AP_HAL::millis();
168 : }
169 :
170 : // request GIMBAL_DEVICE_INFORMATION message
171 4 : void AP_Mount_MAVLink::request_gimbal_device_information() const
172 : {
173 4 : if (_link == nullptr) {
174 0 : return;
175 : }
176 :
177 4 : const mavlink_command_long_t pkt {
178 : MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, // param1
179 : 0, // param2
180 : 0, // param3
181 : 0, // param4
182 : 0, // param5
183 : 0, // param6
184 : 0, // param7
185 : MAV_CMD_REQUEST_MESSAGE,
186 4 : _sysid,
187 4 : _compid,
188 : 0 // confirmation
189 4 : };
190 :
191 4 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
192 : }
193 :
194 : // start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
195 4 : bool AP_Mount_MAVLink::start_sending_attitude_to_gimbal()
196 : {
197 : // better safe than sorry:
198 4 : if (_link == nullptr) {
199 0 : return false;
200 : }
201 : // send AUTOPILOT_STATE_FOR_GIMBAL_DEVICE
202 4 : const MAV_RESULT res = _link->set_message_interval(MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_MAVLINK_ATTITUDE_INTERVAL_US);
203 :
204 : // return true on success
205 4 : return (res == MAV_RESULT_ACCEPTED);
206 : }
207 :
208 : // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to command gimbal to retract (aka relax)
209 0 : void AP_Mount_MAVLink::send_target_retracted()
210 : {
211 : // update the target angles. These may be absolutely bogus, of
212 : // course, but may be useful in logs to see what the gimbal was
213 : // doing. This is also preserving existing behaviour in a change...
214 0 : mnt_target.angle_rad.set(Vector3f{0,0,0}, false);
215 :
216 0 : const mavlink_gimbal_device_set_attitude_t pkt {
217 : {NAN, NAN, NAN, NAN}, // attitude
218 : 0, // angular velocity x
219 : 0, // angular velocity y
220 : 0, // angular velocity z
221 : GIMBAL_DEVICE_FLAGS_RETRACT, // flags
222 0 : _sysid,
223 0 : _compid
224 0 : };
225 :
226 0 : _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
227 0 : }
228 :
229 : // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate
230 0 : void AP_Mount_MAVLink::send_target_rates(const MountRateTarget &rate_rads)
231 : {
232 0 : const float roll_rads = rate_rads.roll;
233 0 : const float pitch_rads = rate_rads.pitch;
234 0 : const float yaw_rads = rate_rads.yaw;
235 0 : const bool earth_frame = rate_rads.yaw_is_ef;
236 :
237 : // prepare flags
238 0 : const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
239 :
240 0 : const mavlink_gimbal_device_set_attitude_t pkt {
241 : {NAN, NAN, NAN, NAN}, // attitude
242 : roll_rads, // angular velocity x
243 : pitch_rads, // angular velocity y
244 : yaw_rads, // angular velocity z
245 : flags,
246 0 : _sysid,
247 0 : _compid
248 0 : };
249 :
250 0 : _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
251 0 : }
252 :
253 : // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude
254 11602 : void AP_Mount_MAVLink::send_target_angles(const MountAngleTarget &angle_rad)
255 : {
256 11602 : const float roll_rad = angle_rad.roll;
257 11602 : const float pitch_rad = angle_rad.pitch;
258 11602 : const float yaw_rad = angle_rad.yaw;
259 11602 : const bool earth_frame = angle_rad.yaw_is_ef;
260 :
261 : // exit immediately if not initialised
262 11602 : if (!_initialised) {
263 0 : return;
264 : }
265 :
266 : // prepare flags
267 11602 : const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
268 :
269 : // convert euler angles to quaternion
270 11602 : Quaternion q;
271 11602 : q.from_euler(roll_rad, pitch_rad, yaw_rad);
272 :
273 11602 : const mavlink_gimbal_device_set_attitude_t pkt {
274 11602 : {q.q1, q.q2, q.q3, q.q4},
275 : NAN, // angular velocity x
276 : NAN, // angular velocity y
277 : NAN, // angular velocity z
278 : flags,
279 11602 : _sysid,
280 11602 : _compid
281 11602 : };
282 :
283 11602 : _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt);
284 : }
285 :
286 : // Send MAV_CMD_DO_SET_ROI_LOCATION to gimbal
287 0 : void AP_Mount_MAVLink::send_target_location(const Location &roi_loc)
288 : {
289 0 : if (_link == nullptr) {
290 0 : return;
291 : }
292 :
293 0 : mavlink_command_int_t pkt {};
294 0 : pkt.target_system = _sysid;
295 0 : pkt.target_component = _compid;
296 :
297 0 : if (roi_loc.initialised()) {
298 0 : pkt.command = MAV_CMD_DO_SET_ROI_LOCATION;
299 0 : pkt.x = roi_loc.lat, // param5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7
300 0 : pkt.y = roi_loc.lng, // param6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7
301 0 : pkt.z = roi_loc.alt * 0.01f; // param7 / z position: global: altitude in meters (relative or absolute, depending on frame).
302 0 : pkt.frame = (uint8_t)roi_loc.get_alt_frame();
303 : } else {
304 0 : pkt.command = MAV_CMD_DO_SET_ROI_NONE;
305 : }
306 :
307 0 : _link->send_message(MAVLINK_MSG_ID_COMMAND_INT, (const char*)&pkt);
308 : }
309 :
310 : #endif // HAL_MOUNT_MAVLINK_ENABLED
|
