Line data Source code
1 : #include "GCS_MAVLink_Tracker.h"
2 : #include "Tracker.h"
3 :
4 2032 : MAV_TYPE GCS_Tracker::frame_type() const
5 : {
6 2032 : return MAV_TYPE_ANTENNA_TRACKER;
7 : }
8 :
9 2032 : uint8_t GCS_MAVLINK_Tracker::base_mode() const
10 : {
11 2032 : uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
12 : // work out the base_mode. This value is not very useful
13 : // for APM, but we calculate it as best we can so a generic
14 : // MAVLink enabled ground station can work out something about
15 : // what the MAV is up to. The actual bit values are highly
16 : // ambiguous for most of the APM flight modes. In practice, you
17 : // only get useful information from the custom_mode, which maps to
18 : // the APM flight mode and has a well defined meaning in the
19 : // ArduPlane documentation
20 2032 : switch (tracker.mode->number()) {
21 38 : case Mode::Number::MANUAL:
22 38 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
23 38 : break;
24 :
25 26 : case Mode::Number::STOP:
26 26 : break;
27 :
28 1712 : case Mode::Number::SCAN:
29 : case Mode::Number::SERVOTEST:
30 : case Mode::Number::AUTO:
31 : case Mode::Number::GUIDED:
32 1712 : _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
33 : MAV_MODE_FLAG_STABILIZE_ENABLED;
34 : // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
35 : // APM does in any mode, as that is defined as "system finds its own goal
36 : // positions", which APM does not currently do
37 1712 : break;
38 :
39 256 : case Mode::Number::INITIALISING:
40 256 : break;
41 : }
42 :
43 : // we are armed if safety switch is not disarmed
44 2032 : if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED &&
45 3776 : tracker.mode != &tracker.mode_initialising &&
46 1744 : hal.util->get_soft_armed()) {
47 1610 : _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
48 : }
49 :
50 2032 : return _base_mode;
51 : }
52 :
53 2032 : uint32_t GCS_Tracker::custom_mode() const
54 : {
55 2032 : return (uint32_t)tracker.mode->number();
56 : }
57 :
58 2032 : MAV_STATE GCS_MAVLINK_Tracker::vehicle_system_status() const
59 : {
60 2032 : if (tracker.mode == &tracker.mode_initialising) {
61 256 : return MAV_STATE_CALIBRATING;
62 : }
63 1776 : return MAV_STATE_ACTIVE;
64 : }
65 :
66 0 : void GCS_MAVLINK_Tracker::send_attitude_target()
67 : {
68 0 : Quaternion quat;
69 0 : bool use_yaw_rate = false;
70 0 : float yaw_rate_rads = 0.0;
71 0 : bool use_pitch_rate = false;
72 0 : float pitch_rate_rads = 0.0;
73 0 : float quat_out[4] {0.0, 0.0, 0.0, 0.0};
74 0 : uint16_t typemask = ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE |
75 : ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
76 :
77 0 : if (tracker.mode->number() == Mode::Number::GUIDED) {
78 0 : quat = tracker.mode_guided.get_attitude_target_quat();
79 0 : use_yaw_rate = tracker.mode_guided.get_attitude_target_use_yaw_rate();
80 0 : use_pitch_rate = tracker.mode_guided.get_attitude_target_use_pitch_rate();
81 0 : quat_out[0] = quat.q1;
82 0 : quat_out[1] = quat.q2;
83 0 : quat_out[2] = quat.q3;
84 0 : quat_out[3] = quat.q4;
85 0 : if (!use_yaw_rate) {
86 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
87 : } else {
88 0 : yaw_rate_rads = tracker.mode_guided.get_attitude_target_yaw_rate_rads();
89 : }
90 0 : if (!use_pitch_rate) {
91 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
92 : } else {
93 0 : pitch_rate_rads = tracker.mode_guided.get_attitude_target_pitch_rate_rads();
94 : }
95 :
96 0 : } else if (tracker.mode->number() == Mode::Number::AUTO) {
97 0 : float yaw = tracker.mode_auto.get_auto_target_yaw_deg();
98 0 : float pitch = tracker.mode_auto.get_auto_target_pitch_deg();
99 0 : quat.from_euler(0, radians(pitch), radians(yaw));
100 0 : quat_out[0] = quat.q1;
101 0 : quat_out[1] = quat.q2;
102 0 : quat_out[2] = quat.q3;
103 0 : quat_out[3] = quat.q4;
104 :
105 0 : typemask |=
106 : ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE |
107 : ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE |
108 : ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE |
109 : ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
110 :
111 0 : } else if (tracker.mode->number() == Mode::Number::SCAN) {
112 0 : struct Tracker::NavStatus &nav_status = tracker.nav_status;
113 0 : Parameters &g = tracker.g;
114 :
115 0 : if (!nav_status.manual_control_yaw) {
116 0 : yaw_rate_rads = radians(g.scan_speed_yaw);
117 : } else {
118 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
119 : }
120 :
121 0 : if (!nav_status.manual_control_pitch) {
122 0 : pitch_rate_rads = radians(g.scan_speed_pitch);
123 : } else {
124 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
125 : }
126 :
127 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;
128 :
129 0 : } else if (tracker.mode->number() == Mode::Number::STOP) {
130 0 : typemask |= ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;
131 :
132 : } else {
133 0 : return;
134 : }
135 :
136 0 : mavlink_msg_attitude_target_send(
137 : chan,
138 : AP_HAL::millis(), // time since boot (ms)
139 : typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle
140 : quat_out, // Attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length
141 : 0, // roll rate (rad/s)
142 : pitch_rate_rads, // pitch rate (rad/s)
143 : yaw_rate_rads, // yaw rate (rad/s)
144 : 0); // Collective thrust
145 : }
146 :
147 1760 : void GCS_MAVLINK_Tracker::send_nav_controller_output() const
148 : {
149 1760 : float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps;
150 :
151 1760 : mavlink_msg_nav_controller_output_send(
152 1760 : chan,
153 : 0,
154 : tracker.nav_status.pitch,
155 1760 : tracker.nav_status.bearing,
156 1760 : tracker.nav_status.bearing,
157 1760 : MIN(tracker.nav_status.distance, UINT16_MAX),
158 : alt_diff,
159 : 0,
160 : 0);
161 1760 : }
162 :
163 40 : void GCS_MAVLINK_Tracker::handle_set_attitude_target(const mavlink_message_t &msg)
164 : {
165 : // decode packet
166 : mavlink_set_attitude_target_t packet;
167 40 : mavlink_msg_set_attitude_target_decode(&msg, &packet);
168 :
169 : // exit if vehicle is not in Guided mode
170 40 : if (tracker.mode != &tracker.mode_guided) {
171 0 : return;
172 : }
173 :
174 : // sanity checks:
175 40 : if (!is_zero(packet.body_roll_rate)) {
176 0 : return;
177 : }
178 40 : if (!(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE)) {
179 : // not told to ignore body roll rate
180 0 : return;
181 : }
182 40 : if (!(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE)) {
183 : // not told to ignore throttle
184 0 : return;
185 : }
186 40 : if (packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) {
187 : // told to ignore attitude (we don't allow continuous motion yet)
188 0 : return;
189 : }
190 40 : if ((packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE) &&
191 0 : (packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE)) {
192 : // told to ignore both pitch and yaw rates - nothing to do?!
193 0 : return;
194 : }
195 :
196 40 : const bool use_yaw_rate = !(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE);
197 40 : const bool use_pitch_rate = !(packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE);
198 :
199 40 : tracker.mode_guided.set_angle(
200 80 : Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
201 : use_yaw_rate,
202 : packet.body_yaw_rate,
203 : use_pitch_rate,
204 : packet.body_pitch_rate);
205 : }
206 :
207 : /*
208 : send PID tuning message
209 : */
210 1760 : void GCS_MAVLINK_Tracker::send_pid_tuning()
211 : {
212 1760 : const Parameters &g = tracker.g;
213 :
214 : // Pitch PID
215 1760 : if (g.gcs_pid_mask & 1) {
216 4 : const AP_PIDInfo *pid_info = &g.pidPitch2Srv.get_pid_info();
217 4 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
218 4 : pid_info->target,
219 4 : pid_info->actual,
220 4 : pid_info->FF,
221 4 : pid_info->P,
222 4 : pid_info->I,
223 4 : pid_info->D,
224 4 : pid_info->slew_rate,
225 4 : pid_info->Dmod);
226 4 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
227 0 : return;
228 : }
229 : }
230 :
231 : // Yaw PID
232 1760 : if (g.gcs_pid_mask & 2) {
233 0 : const AP_PIDInfo *pid_info = &g.pidYaw2Srv.get_pid_info();
234 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
235 0 : pid_info->target,
236 0 : pid_info->actual,
237 0 : pid_info->FF,
238 0 : pid_info->P,
239 0 : pid_info->I,
240 0 : pid_info->D,
241 0 : pid_info->slew_rate,
242 0 : pid_info->Dmod);
243 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
244 0 : return;
245 : }
246 : }
247 : }
248 :
249 : /*
250 : We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and
251 : MAVLINK_MSG_ID_SCALED_PRESSUREs
252 : */
253 8994 : void GCS_MAVLINK_Tracker::packetReceived(const mavlink_status_t &status,
254 : const mavlink_message_t &msg)
255 : {
256 : // return immediately if sysid doesn't match our target sysid
257 8994 : if ((tracker.g.sysid_target != 0) && (tracker.g.sysid_target != msg.sysid)) {
258 0 : GCS_MAVLINK::packetReceived(status, msg);
259 0 : return;
260 : }
261 :
262 8994 : switch (msg.msgid) {
263 1403 : case MAVLINK_MSG_ID_HEARTBEAT:
264 : {
265 1403 : mavlink_check_target(msg);
266 1403 : break;
267 : }
268 :
269 0 : case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
270 : {
271 : // decode
272 : mavlink_global_position_int_t packet;
273 0 : mavlink_msg_global_position_int_decode(&msg, &packet);
274 0 : tracker.tracking_update_position(packet);
275 0 : break;
276 : }
277 :
278 0 : case MAVLINK_MSG_ID_SCALED_PRESSURE:
279 : {
280 : // decode
281 : mavlink_scaled_pressure_t packet;
282 0 : mavlink_msg_scaled_pressure_decode(&msg, &packet);
283 0 : tracker.tracking_update_pressure(packet);
284 0 : break;
285 : }
286 : }
287 8994 : GCS_MAVLINK::packetReceived(status, msg);
288 : }
289 :
290 312676 : bool GCS_MAVLINK_Tracker::try_send_message(enum ap_message id)
291 : {
292 312676 : switch(id) {
293 1760 : case MSG_WIND: // other vehicles do something custom with wind:
294 1760 : return true;
295 310916 : default:
296 310916 : return GCS_MAVLINK::try_send_message(id);
297 : }
298 : return true;
299 : }
300 :
301 : // locks onto a particular target sysid and sets it's position data stream to at least 1hz
302 1403 : void GCS_MAVLINK_Tracker::mavlink_check_target(const mavlink_message_t &msg)
303 : {
304 : // exit immediately if the target has already been set
305 1403 : if (tracker.target_set) {
306 1403 : return;
307 : }
308 :
309 : // decode
310 : mavlink_heartbeat_t packet;
311 1403 : mavlink_msg_heartbeat_decode(&msg, &packet);
312 :
313 : // exit immediately if this is not a vehicle we would track
314 1403 : if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
315 1403 : (packet.type == MAV_TYPE_GCS) ||
316 0 : (packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
317 0 : (packet.type == MAV_TYPE_GIMBAL)) {
318 1403 : return;
319 : }
320 :
321 : // set our sysid to the target, this ensures we lock onto a single vehicle
322 0 : if (tracker.g.sysid_target == 0) {
323 0 : tracker.g.sysid_target.set(msg.sysid);
324 : }
325 :
326 : // send data stream request to target on all channels
327 : // Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
328 0 : tracker.gcs().request_datastream_position(msg.sysid, msg.compid);
329 0 : tracker.gcs().request_datastream_airpressure(msg.sysid, msg.compid);
330 :
331 : // flag target has been set
332 0 : tracker.target_set = true;
333 : }
334 :
335 0 : MAV_RESULT GCS_MAVLINK_Tracker::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
336 : {
337 0 : MAV_RESULT ret = GCS_MAVLINK::_handle_command_preflight_calibration_baro(msg);
338 0 : if (ret == MAV_RESULT_ACCEPTED) {
339 : // zero the altitude difference on next baro update
340 0 : tracker.nav_status.need_altitude_calibration = true;
341 : }
342 0 : return ret;
343 : }
344 :
345 31 : MAV_RESULT GCS_MAVLINK_Tracker::handle_command_component_arm_disarm(const mavlink_command_int_t &packet)
346 : {
347 31 : if (is_equal(packet.param1,1.0f)) {
348 0 : tracker.arm_servos();
349 0 : return MAV_RESULT_ACCEPTED;
350 : }
351 31 : if (is_zero(packet.param1)) {
352 31 : tracker.disarm_servos();
353 31 : return MAV_RESULT_ACCEPTED;
354 : }
355 0 : return MAV_RESULT_UNSUPPORTED;
356 : }
357 :
358 141 : MAV_RESULT GCS_MAVLINK_Tracker::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
359 : {
360 141 : switch(packet.command) {
361 :
362 8 : case MAV_CMD_DO_SET_SERVO:
363 : // ensure we are in servo test mode
364 8 : tracker.set_mode(tracker.mode_servotest, ModeReason::SERVOTEST);
365 :
366 8 : if (!tracker.mode_servotest.set_servo(packet.param1, packet.param2)) {
367 0 : return MAV_RESULT_FAILED;
368 : }
369 8 : return MAV_RESULT_ACCEPTED;
370 :
371 : // mavproxy/mavutil sends this when auto command is entered
372 2 : case MAV_CMD_MISSION_START:
373 2 : tracker.set_mode(tracker.mode_auto, ModeReason::GCS_COMMAND);
374 2 : return MAV_RESULT_ACCEPTED;
375 :
376 131 : default:
377 131 : return GCS_MAVLINK::handle_command_int_packet(packet, msg);
378 : }
379 : }
380 :
381 8941 : void GCS_MAVLINK_Tracker::handle_message(const mavlink_message_t &msg)
382 : {
383 8941 : switch (msg.msgid) {
384 :
385 40 : case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
386 40 : handle_set_attitude_target(msg);
387 40 : break;
388 :
389 : #if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED
390 : // When mavproxy 'wp sethome'
391 0 : case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
392 0 : handle_message_mission_write_partial_list(msg);
393 0 : break;
394 :
395 : // XXX receive a WP from GCS and store in EEPROM if it is HOME
396 0 : case MAVLINK_MSG_ID_MISSION_ITEM:
397 0 : handle_message_mission_item(msg);
398 0 : break;
399 : #endif
400 :
401 0 : case MAVLINK_MSG_ID_MANUAL_CONTROL:
402 0 : handle_message_manual_control(msg);
403 0 : break;
404 :
405 0 : case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
406 0 : handle_message_global_position_int(msg);
407 0 : break;
408 :
409 0 : case MAVLINK_MSG_ID_SCALED_PRESSURE:
410 0 : handle_message_scaled_pressure(msg);
411 0 : break;
412 : }
413 :
414 8941 : GCS_MAVLINK::handle_message(msg);
415 8914 : }
416 :
417 :
418 : #if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED
419 0 : void GCS_MAVLINK_Tracker::handle_message_mission_write_partial_list(const mavlink_message_t &msg)
420 : {
421 : // decode
422 : mavlink_mission_write_partial_list_t packet;
423 0 : mavlink_msg_mission_write_partial_list_decode(&msg, &packet);
424 0 : if (packet.start_index == 0)
425 : {
426 : // New home at wp index 0. Ask for it
427 0 : waypoint_receiving = true;
428 0 : send_message(MSG_NEXT_MISSION_REQUEST_WAYPOINTS);
429 : }
430 0 : }
431 :
432 0 : void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &msg)
433 : {
434 : mavlink_mission_item_t packet;
435 0 : MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED;
436 :
437 0 : mavlink_msg_mission_item_decode(&msg, &packet);
438 :
439 0 : Location tell_command;
440 :
441 0 : switch (packet.frame)
442 : {
443 0 : case MAV_FRAME_MISSION:
444 : case MAV_FRAME_GLOBAL:
445 : {
446 0 : tell_command = Location{
447 0 : int32_t(1.0e7f*packet.x), // in as DD converted to * t7
448 0 : int32_t(1.0e7f*packet.y), // in as DD converted to * t7
449 0 : int32_t(packet.z*1.0e2f), // in as m converted to cm
450 : Location::AltFrame::ABSOLUTE
451 : };
452 0 : break;
453 : }
454 :
455 : #ifdef MAV_FRAME_LOCAL_NED
456 : case MAV_FRAME_LOCAL_NED: // local (relative to home position)
457 : {
458 : tell_command = Location{
459 : int32_t(1.0e7f*degrees(packet.x/(RADIUS_OF_EARTH*cosf(radians(home.lat/1.0e7f)))) + home.lat),
460 : int32_t(1.0e7f*degrees(packet.y/RADIUS_OF_EARTH) + home.lng),
461 : int32_t(-packet.z*1.0e2f),
462 : Location::AltFrame::ABOVE_HOME
463 : };
464 : break;
465 : }
466 : #endif
467 :
468 : #ifdef MAV_FRAME_LOCAL
469 : case MAV_FRAME_LOCAL: // local (relative to home position)
470 : {
471 : tell_command = {
472 : int32_t(1.0e7f*degrees(packet.x/(RADIUS_OF_EARTH*cosf(radians(home.lat/1.0e7f)))) + home.lat),
473 : int32_t(1.0e7f*degrees(packet.y/RADIUS_OF_EARTH) + home.lng),
474 : int32_t(packet.z*1.0e2f),
475 : Location::AltFrame::ABOVE_HOME
476 : };
477 : break;
478 : }
479 : #endif
480 :
481 0 : case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
482 : {
483 0 : tell_command = {
484 0 : int32_t(1.0e7f * packet.x), // in as DD converted to * t7
485 0 : int32_t(1.0e7f * packet.y), // in as DD converted to * t7
486 0 : int32_t(packet.z * 1.0e2f),
487 : Location::AltFrame::ABOVE_HOME
488 : };
489 0 : break;
490 : }
491 :
492 0 : default:
493 0 : result = MAV_MISSION_UNSUPPORTED_FRAME;
494 0 : break;
495 : }
496 :
497 0 : if (result != MAV_MISSION_ACCEPTED) goto mission_failed;
498 :
499 : // Check if receiving waypoints (mission upload expected)
500 0 : if (!waypoint_receiving) {
501 0 : result = MAV_MISSION_ERROR;
502 0 : goto mission_failed;
503 : }
504 :
505 : // check if this is the HOME wp
506 0 : if (packet.seq == 0) {
507 0 : if (!tracker.set_home(tell_command, false)) {
508 0 : result = MAV_MISSION_ERROR;
509 0 : goto mission_failed;
510 : }
511 0 : send_text(MAV_SEVERITY_INFO,"New HOME received");
512 0 : waypoint_receiving = false;
513 : }
514 :
515 0 : mission_failed:
516 : // send ACK (including in success case)
517 0 : mavlink_msg_mission_ack_send(
518 : chan,
519 0 : msg.sysid,
520 0 : msg.compid,
521 : result,
522 : MAV_MISSION_TYPE_MISSION);
523 0 : }
524 : #endif
525 :
526 0 : void GCS_MAVLINK_Tracker::handle_message_manual_control(const mavlink_message_t &msg)
527 : {
528 : mavlink_manual_control_t packet;
529 0 : mavlink_msg_manual_control_decode(&msg, &packet);
530 0 : tracker.tracking_manual_control(packet);
531 0 : }
532 :
533 0 : void GCS_MAVLINK_Tracker::handle_message_global_position_int(const mavlink_message_t &msg)
534 : {
535 : // decode
536 : mavlink_global_position_int_t packet;
537 0 : mavlink_msg_global_position_int_decode(&msg, &packet);
538 0 : tracker.tracking_update_position(packet);
539 0 : }
540 :
541 0 : void GCS_MAVLINK_Tracker::handle_message_scaled_pressure(const mavlink_message_t &msg)
542 : {
543 : mavlink_scaled_pressure_t packet;
544 0 : mavlink_msg_scaled_pressure_decode(&msg, &packet);
545 0 : tracker.tracking_update_pressure(packet);
546 0 : }
547 :
548 : // send position tracker is using
549 1760 : void GCS_MAVLINK_Tracker::send_global_position_int()
550 : {
551 1760 : if (!tracker.stationary) {
552 1463 : GCS_MAVLINK::send_global_position_int();
553 1463 : return;
554 : }
555 :
556 297 : mavlink_msg_global_position_int_send(
557 : chan,
558 : AP_HAL::millis(),
559 : tracker.current_loc.lat, // in 1E7 degrees
560 : tracker.current_loc.lng, // in 1E7 degrees
561 297 : tracker.current_loc.alt * 10, // millimeters above ground/sea level
562 : 0, // millimeters above home
563 : 0, // X speed cm/s (+ve North)
564 : 0, // Y speed cm/s (+ve East)
565 : 0, // Z speed cm/s (+ve Down)
566 297 : tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
567 : }
568 :
569 : // Send the mode with the given index (not mode number!) return the total number of modes
570 : // Index starts at 1
571 0 : uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const
572 : {
573 0 : const Mode* modes[] {
574 : &tracker.mode_manual,
575 : &tracker.mode_stop,
576 : &tracker.mode_scan,
577 : &tracker.mode_guided,
578 : &tracker.mode_servotest,
579 : &tracker.mode_auto,
580 : &tracker.mode_initialising,
581 : };
582 :
583 0 : const uint8_t mode_count = ARRAY_SIZE(modes);
584 :
585 : // Convert to zero indexed
586 0 : const uint8_t index_zero = index - 1;
587 0 : if (index_zero >= mode_count) {
588 : // Mode does not exist!?
589 0 : return mode_count;
590 : }
591 :
592 : // Ask the mode for its name and number
593 0 : const char* name = modes[index_zero]->name();
594 0 : const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
595 :
596 0 : mavlink_msg_available_modes_send(
597 0 : chan,
598 : mode_count,
599 : index,
600 : MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
601 : mode_number,
602 : 0, // MAV_MODE_PROPERTY bitmask
603 : name
604 : );
605 :
606 0 : return mode_count;
607 : }
|
