Line data Source code
1 : /*
2 : Simulator for MAVLink Gimbal Protocol v2 peripherals
3 : */
4 :
5 : #include "SIM_config.h"
6 :
7 : #if AP_SIM_MAVLINKGIMBALV2_ENABLED
8 :
9 : #include "SIM_MAVLinkGimbalv2.h"
10 : #include "SIM_Aircraft.h"
11 : #include <AP_HAL/AP_HAL.h>
12 : #include <stdio.h>
13 :
14 : namespace SITL {
15 :
16 5 : void MAVLinkGimbalv2::set_instance(uint8_t instance)
17 : {
18 5 : Mount::set_instance(instance);
19 : // select the component ID matching this gimbal instance:
20 : // instance 0 → MAV_COMP_ID_GIMBAL (154)
21 : // instance 1 → MAV_COMP_ID_GIMBAL2 (171), 2 → 172, …
22 5 : _compid = (instance == 0) ? MAV_COMP_ID_GIMBAL
23 : : MAV_COMP_ID_GIMBAL2 + (instance - 1);
24 :
25 : // configure joint limits from subclass declarations (roll kept at ±40°)
26 5 : _gimbal.set_joint_limits(
27 5 : Vector3f(radians(-40.0f), get_pitch_min_rad(), get_yaw_min_rad()),
28 5 : Vector3f(radians( 40.0f), get_pitch_max_rad(), get_yaw_max_rad())
29 : );
30 5 : }
31 :
32 3326 : void MAVLinkGimbalv2::send_mavlink_message(const mavlink_message_t &msg)
33 : {
34 : uint8_t buf[MAVLINK_MAX_PACKET_LEN];
35 3326 : const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
36 3326 : if (len > 0) {
37 3326 : write_to_autopilot((char*)buf, len);
38 : }
39 3326 : }
40 :
41 332820 : void MAVLinkGimbalv2::update_input()
42 : {
43 : uint8_t buf[128];
44 332820 : const ssize_t nread = read_from_autopilot((char*)buf, sizeof(buf));
45 1828071 : for (ssize_t i = 0; i < nread; i++) {
46 : mavlink_message_t msg;
47 : mavlink_status_t status;
48 2990502 : if (mavlink_frame_char_buffer(&mav.rxmsg, &mav.status,
49 1495251 : buf[i], &msg, &status) == MAVLINK_FRAMING_OK) {
50 27617 : handle_message(msg);
51 : }
52 : }
53 332820 : }
54 :
55 27617 : void MAVLinkGimbalv2::handle_message(const mavlink_message_t &msg)
56 : {
57 27617 : switch (msg.msgid) {
58 2055 : case MAVLINK_MSG_ID_HEARTBEAT: {
59 2055 : if (!_seen_autopilot_heartbeat) {
60 5 : _seen_autopilot_heartbeat = true;
61 5 : _vehicle_system_id = msg.sysid;
62 5 : _vehicle_component_id = msg.compid;
63 5 : ::printf("MAVLinkGimbalv2[%u]: using sysid=%u compid=%u\n",
64 5 : (unsigned)_instance,
65 5 : (unsigned)_vehicle_system_id,
66 5 : (unsigned)_compid);
67 : }
68 2055 : break;
69 : }
70 37 : case MAVLINK_MSG_ID_COMMAND_LONG: {
71 : mavlink_command_long_t cmd;
72 37 : mavlink_msg_command_long_decode(&msg, &cmd);
73 : // only handle messages addressed to this gimbal
74 37 : if (cmd.target_system != _vehicle_system_id ||
75 37 : cmd.target_component != _compid) {
76 : break;
77 : }
78 4 : if (cmd.command == MAV_CMD_REQUEST_MESSAGE) {
79 4 : const uint32_t requested_msgid = (uint32_t)cmd.param1;
80 4 : if (requested_msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION) {
81 4 : send_gimbal_device_information();
82 4 : send_command_ack(msg.sysid, msg.compid,
83 : MAV_CMD_REQUEST_MESSAGE, MAV_RESULT_ACCEPTED);
84 : }
85 : }
86 4 : break;
87 : }
88 0 : case MAVLINK_MSG_ID_COMMAND_INT: {
89 : mavlink_command_int_t cmd;
90 0 : mavlink_msg_command_int_decode(&msg, &cmd);
91 0 : if (cmd.target_system != _vehicle_system_id ||
92 0 : cmd.target_component != _compid) {
93 : break;
94 : }
95 0 : if (cmd.command == MAV_CMD_DO_SET_ROI_LOCATION) {
96 0 : _roi.loc.lat = cmd.x;
97 0 : _roi.loc.lng = cmd.y;
98 0 : _roi.loc.set_alt_m(cmd.z, (Location::AltFrame)cmd.frame);
99 0 : _roi.valid = true;
100 0 : } else if (cmd.command == MAV_CMD_DO_SET_ROI_NONE) {
101 0 : _roi.valid = false;
102 : }
103 0 : break;
104 : }
105 11600 : case MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE: {
106 : mavlink_gimbal_device_set_attitude_t cmd;
107 11600 : mavlink_msg_gimbal_device_set_attitude_decode(&msg, &cmd);
108 11600 : if (cmd.target_system != _vehicle_system_id ||
109 11600 : cmd.target_component != _compid) {
110 : break;
111 : }
112 11600 : const uint16_t flags = cmd.flags;
113 11600 : if (flags & GIMBAL_DEVICE_FLAGS_RETRACT) {
114 0 : _target.valid = false;
115 0 : _roi.valid = false;
116 0 : break;
117 : }
118 11600 : const bool yaw_ef = (flags & GIMBAL_DEVICE_FLAGS_YAW_LOCK) != 0;
119 11600 : const bool rates_valid = !isnan(cmd.angular_velocity_x);
120 11600 : const bool q_valid = !isnan(cmd.q[0]);
121 11600 : if (rates_valid || q_valid) {
122 : // any explicit attitude/rate command cancels ROI tracking
123 11600 : _roi.valid = false;
124 : }
125 11600 : if (rates_valid) {
126 0 : _target.valid = true;
127 0 : _target.is_rate = true;
128 0 : _target.yaw_is_ef = yaw_ef;
129 0 : _target.rates_rads = Vector3f(cmd.angular_velocity_x,
130 : cmd.angular_velocity_y,
131 : cmd.angular_velocity_z);
132 11600 : } else if (q_valid) {
133 11600 : _target.valid = true;
134 11600 : _target.is_rate = false;
135 11600 : _target.yaw_is_ef = yaw_ef;
136 11600 : _target.attitude = Quaternion(cmd.q[0], cmd.q[1], cmd.q[2], cmd.q[3]);
137 : }
138 11600 : break;
139 : }
140 13925 : default:
141 13925 : break;
142 : }
143 27617 : }
144 :
145 277 : void MAVLinkGimbalv2::send_heartbeat()
146 : {
147 277 : if (!_seen_autopilot_heartbeat) {
148 5 : return;
149 : }
150 :
151 272 : mavlink_heartbeat_t hb {};
152 272 : hb.type = MAV_TYPE_GIMBAL;
153 272 : hb.autopilot = MAV_AUTOPILOT_INVALID;
154 272 : hb.base_mode = 0;
155 272 : hb.system_status = MAV_STATE_ACTIVE;
156 272 : hb.mavlink_version = 3;
157 :
158 : mavlink_message_t msg;
159 272 : mavlink_msg_heartbeat_encode_status(
160 272 : _vehicle_system_id, _compid,
161 : &mav.status, &msg, &hb);
162 272 : send_mavlink_message(msg);
163 : }
164 :
165 4 : void MAVLinkGimbalv2::send_gimbal_device_information()
166 : {
167 4 : mavlink_gimbal_device_information_t info {};
168 4 : info.time_boot_ms = AP_HAL::millis();
169 4 : info.firmware_version = get_firmware_version();
170 4 : info.hardware_version = 0;
171 4 : info.roll_min = NAN;
172 4 : info.roll_max = NAN;
173 4 : info.pitch_min = get_pitch_min_rad();
174 4 : info.pitch_max = get_pitch_max_rad();
175 4 : info.yaw_min = get_yaw_min_rad();
176 4 : info.yaw_max = get_yaw_max_rad();
177 4 : info.cap_flags = get_cap_flags();
178 4 : strncpy_noterm(info.vendor_name, get_vendor_name(), sizeof(info.vendor_name));
179 4 : strncpy_noterm(info.model_name, get_model_name(), sizeof(info.model_name));
180 :
181 : mavlink_message_t msg;
182 4 : mavlink_msg_gimbal_device_information_encode_status(
183 4 : _vehicle_system_id, _compid,
184 : &mav.status, &msg, &info);
185 4 : send_mavlink_message(msg);
186 4 : }
187 :
188 : // update gimbal physics and compute demanded rates from the current target
189 332820 : void MAVLinkGimbalv2::update_gimbal(const Aircraft &aircraft)
190 : {
191 332820 : _vehicle_dcm = aircraft.get_dcm();
192 :
193 332820 : Matrix3f gimbal_dcm;
194 332820 : _gimbal.get_dcm(gimbal_dcm);
195 :
196 : // if tracking an ROI, continuously recompute the earth-frame target
197 : // (replicates AP_Mount_Backend::get_angle_target_to_location())
198 332820 : if (_roi.valid) {
199 0 : const Location &veh = aircraft.get_location();
200 0 : const float gps_x = Location::diff_longitude(_roi.loc.lng, veh.lng)
201 0 : * cosf(radians((veh.lat + _roi.loc.lat) * 0.00000005f)) * 0.01113195f;
202 0 : const float gps_y = (_roi.loc.lat - veh.lat) * 0.01113195f;
203 0 : int32_t target_alt_cm = 0, veh_alt_cm = 0;
204 0 : if (_roi.loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm) &&
205 0 : veh.get_alt_cm(Location::AltFrame::ABOVE_HOME, veh_alt_cm)) {
206 0 : const float gps_z = (float)(target_alt_cm - veh_alt_cm); // cm
207 0 : const float horiz_cm = 100.0f * norm(gps_x, gps_y);
208 0 : float pitch_ef = atan2f(gps_z, horiz_cm);
209 0 : pitch_ef = constrain_float(pitch_ef, get_pitch_min_rad(), get_pitch_max_rad());
210 0 : const float yaw_ef = atan2f(gps_x, gps_y);
211 0 : _target.valid = true;
212 0 : _target.is_rate = false;
213 0 : _target.yaw_is_ef = true;
214 0 : _target.attitude.from_euler(0.0f, pitch_ef, yaw_ef);
215 : }
216 : }
217 :
218 332820 : if (_target.valid) {
219 278506 : Vector3f demanded_rates;
220 278506 : if (_target.is_rate) {
221 0 : demanded_rates = _target.rates_rads;
222 0 : if (_target.yaw_is_ef) {
223 : // transform earth-frame rates to gimbal body frame
224 0 : demanded_rates = gimbal_dcm.transposed() * demanded_rates;
225 : }
226 : // add vehicle body rates so that rate=0 tracks the vehicle body
227 : // (body-relative stabilisation); matches neutral-mode behaviour
228 0 : demanded_rates += gimbal_dcm.transposed() * _vehicle_dcm * aircraft.get_gyro();
229 0 : _gimbal.set_demanded_rates(demanded_rates);
230 278506 : } else if (_target.yaw_is_ef) {
231 : // earth-frame angle control: P-controller
232 : // clamp target pitch to hardware joint limits before computing error
233 : float t_r, t_p, t_y;
234 0 : _target.attitude.to_euler(t_r, t_p, t_y);
235 0 : t_p = constrain_float(t_p, get_pitch_min_rad(), get_pitch_max_rad());
236 0 : Quaternion target_clamped;
237 0 : target_clamped.from_euler(t_r, t_p, t_y);
238 0 : Quaternion q_current;
239 0 : q_current.from_rotation_matrix(gimbal_dcm);
240 0 : Quaternion q_error = q_current.inverse() * target_clamped;
241 0 : q_error.normalize();
242 0 : Vector3f av(q_error.q2, q_error.q3, q_error.q4);
243 0 : if (q_error.q1 < 0.0f) {
244 0 : av = -av;
245 : }
246 0 : const float attitude_gain = 2.0f; // rad/s per radian of error
247 0 : demanded_rates = av * (2.0f * attitude_gain);
248 0 : _gimbal.set_demanded_rates(demanded_rates);
249 : } else {
250 : // body-frame angle control: snap gimbal DCM to the commanded
251 : // body-relative orientation directly
252 278506 : Matrix3f body_dcm;
253 278506 : _target.attitude.rotation_matrix(body_dcm);
254 278506 : _gimbal.set_dcm(_vehicle_dcm * body_dcm);
255 278506 : _gimbal.set_demanded_rates(Vector3f{});
256 : }
257 : } else {
258 : // neutral: track vehicle body using vehicle angular rate feedforward
259 54314 : const Vector3f vehicle_rate_gimbal = gimbal_dcm.transposed() * _vehicle_dcm * aircraft.get_gyro();
260 54314 : _gimbal.set_demanded_rates(vehicle_rate_gimbal);
261 : }
262 332820 : _gimbal.update(aircraft);
263 332820 : }
264 :
265 2770 : void MAVLinkGimbalv2::send_attitude_status()
266 : {
267 2770 : if (!_seen_autopilot_heartbeat) {
268 50 : return;
269 : }
270 :
271 : // report actual gimbal attitude from physics model
272 2720 : Matrix3f gimbal_dcm;
273 2720 : _gimbal.get_dcm(gimbal_dcm);
274 :
275 : uint16_t flags;
276 2720 : Quaternion q;
277 2720 : if (_target.valid && _target.yaw_is_ef) {
278 : // earth-frame target: report actual gimbal DCM so convergence is visible
279 0 : flags = GIMBAL_DEVICE_FLAGS_ROLL_LOCK |
280 : GIMBAL_DEVICE_FLAGS_PITCH_LOCK |
281 : GIMBAL_DEVICE_FLAGS_YAW_LOCK;
282 0 : q.from_rotation_matrix(gimbal_dcm);
283 2720 : } else if (_target.valid && !_target.yaw_is_ef && !_target.is_rate) {
284 : // body-frame angle target: report the commanded body-relative attitude
285 : // directly, matching how servo mounts report commanded angles
286 2320 : flags = 0;
287 2320 : q = _target.attitude;
288 : } else {
289 : // neutral or rate control: report body-relative attitude from DCM
290 400 : flags = 0;
291 400 : const Matrix3f body_to_gimbal = _vehicle_dcm.transposed() * gimbal_dcm;
292 400 : q.from_rotation_matrix(body_to_gimbal);
293 : }
294 :
295 2720 : mavlink_gimbal_device_attitude_status_t status {};
296 2720 : status.target_system = _vehicle_system_id;
297 2720 : status.target_component = _vehicle_component_id;
298 2720 : status.time_boot_ms = AP_HAL::millis();
299 2720 : status.flags = flags;
300 2720 : status.q[0] = q.q1; // w
301 2720 : status.q[1] = q.q2; // x
302 2720 : status.q[2] = q.q3; // y
303 2720 : status.q[3] = q.q4; // z
304 2720 : status.angular_velocity_x = 0.0f;
305 2720 : status.angular_velocity_y = 0.0f;
306 2720 : status.angular_velocity_z = 0.0f;
307 2720 : status.failure_flags = 0;
308 :
309 : mavlink_message_t msg;
310 2720 : mavlink_msg_gimbal_device_attitude_status_encode_status(
311 2720 : _vehicle_system_id, _compid,
312 : &mav.status, &msg, &status);
313 2720 : send_mavlink_message(msg);
314 : }
315 :
316 4 : void MAVLinkGimbalv2::send_command_ack(uint8_t target_sysid, uint8_t target_compid,
317 : MAV_CMD command, MAV_RESULT result)
318 : {
319 4 : mavlink_command_ack_t ack {};
320 4 : ack.command = (uint16_t)command;
321 4 : ack.result = (uint8_t)result;
322 4 : ack.progress = 255;
323 4 : ack.result_param2 = 0;
324 4 : ack.target_system = target_sysid;
325 4 : ack.target_component = target_compid;
326 :
327 : mavlink_message_t msg;
328 4 : mavlink_msg_command_ack_encode_status(
329 4 : _vehicle_system_id, _compid,
330 : &mav.status, &msg, &ack);
331 4 : send_mavlink_message(msg);
332 4 : }
333 :
334 332820 : void MAVLinkGimbalv2::update(const Aircraft &aircraft)
335 : {
336 332820 : if (!init_sitl_pointer()) {
337 0 : return;
338 : }
339 :
340 332820 : update_input();
341 332820 : update_gimbal(aircraft);
342 :
343 332820 : const uint32_t now_ms = AP_HAL::millis();
344 :
345 332820 : if (now_ms - _last_heartbeat_ms >= 1000) {
346 277 : _last_heartbeat_ms = now_ms;
347 277 : send_heartbeat();
348 : }
349 :
350 332820 : if (now_ms - _last_attitude_status_ms >= 100) {
351 2770 : _last_attitude_status_ms = now_ms;
352 2770 : send_attitude_status();
353 : }
354 : }
355 :
356 : } // namespace SITL
357 :
358 : #endif // AP_SIM_MAVLINKGIMBALV2_ENABLED
|
