Line data Source code
1 : #include "Rover.h"
2 :
3 : #include "GCS_MAVLink_Rover.h"
4 :
5 : #include <AP_RPM/AP_RPM_config.h>
6 : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
7 : #include <AP_EFI/AP_EFI_config.h>
8 : #include <AC_Avoidance/AP_OADatabase.h>
9 :
10 15726 : MAV_TYPE GCS_Rover::frame_type() const
11 : {
12 15726 : if (rover.is_boat()) {
13 1056 : return MAV_TYPE_SURFACE_BOAT;
14 : }
15 14670 : return MAV_TYPE_GROUND_ROVER;
16 : }
17 :
18 15726 : uint8_t GCS_MAVLINK_Rover::base_mode() const
19 : {
20 15726 : uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
21 :
22 : // work out the base_mode. This value is not very useful
23 : // for APM, but we calculate it as best we can so a generic
24 : // MAVLink enabled ground station can work out something about
25 : // what the MAV is up to. The actual bit values are highly
26 : // ambiguous for most of the APM flight modes. In practice, you
27 : // only get useful information from the custom_mode, which maps to
28 : // the APM flight mode and has a well defined meaning in the
29 : // ArduPlane documentation
30 15726 : if (rover.control_mode->has_manual_input()) {
31 10386 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
32 : }
33 :
34 15726 : if (rover.control_mode->is_autopilot_mode()) {
35 5236 : _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
36 : }
37 :
38 15726 : if (rover.g2.stick_mixing > 0 && rover.control_mode != &rover.mode_initializing) {
39 : // all modes except INITIALISING have some form of manual
40 : // override if stick mixing is enabled
41 24 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
42 : }
43 :
44 : // we are armed if we are not initialising
45 15726 : if (rover.control_mode != &rover.mode_initializing && rover.arming.is_armed()) {
46 5274 : _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
47 : }
48 :
49 : // indicate we have set a custom mode
50 15726 : _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
51 :
52 15726 : return _base_mode;
53 : }
54 :
55 15726 : uint32_t GCS_Rover::custom_mode() const
56 : {
57 15726 : return (uint32_t)rover.control_mode->mode_number();
58 : }
59 :
60 14999 : MAV_STATE GCS_MAVLINK_Rover::vehicle_system_status() const
61 : {
62 14999 : if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
63 153 : return MAV_STATE_CRITICAL;
64 : }
65 14846 : if (rover.control_mode == &rover.mode_initializing) {
66 1192 : return MAV_STATE_CALIBRATING;
67 : }
68 13654 : if (rover.control_mode == &rover.mode_hold) {
69 101 : return MAV_STATE_STANDBY;
70 : }
71 :
72 13553 : return MAV_STATE_ACTIVE;
73 : }
74 :
75 124902 : bool GCS_MAVLINK_Rover::get_target_location(Location &target) const
76 : {
77 124902 : return rover.control_mode->get_desired_location(target);
78 : }
79 :
80 124899 : void GCS_MAVLINK_Rover::send_nav_controller_output() const
81 : {
82 124899 : if (!rover.control_mode->is_autopilot_mode()) {
83 73936 : return;
84 : }
85 :
86 50963 : const Mode *control_mode = rover.control_mode;
87 :
88 101926 : mavlink_msg_nav_controller_output_send(
89 50963 : chan,
90 : 0, // roll
91 : degrees(rover.g2.attitude_control.get_desired_pitch()),
92 50963 : control_mode->nav_bearing(),
93 50963 : control_mode->wp_bearing(),
94 101926 : MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
95 : 0,
96 : control_mode->speed_error(),
97 50963 : control_mode->crosstrack_error_m());
98 : }
99 :
100 125466 : int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
101 : {
102 125466 : return rover.g2.motors.get_throttle();
103 : }
104 :
105 : #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
106 2 : void GCS_MAVLINK_Rover::send_rangefinder() const
107 : {
108 2 : float distance = 0;
109 2 : float voltage = 0;
110 2 : bool got_one = false;
111 :
112 : // report smaller distance of all rangefinders
113 4 : for (uint8_t i=0; i<rover.rangefinder.num_sensors(); i++) {
114 2 : AP_RangeFinder_Backend *s = rover.rangefinder.get_backend(i);
115 2 : if (s == nullptr) {
116 0 : continue;
117 : }
118 2 : if (!got_one ||
119 0 : s->distance() < distance) {
120 2 : distance = s->distance();
121 2 : voltage = s->voltage_mv();
122 2 : got_one = true;
123 : }
124 : }
125 2 : if (!got_one) {
126 : // no relevant data found
127 0 : return;
128 : }
129 :
130 2 : mavlink_msg_rangefinder_send(
131 2 : chan,
132 : distance,
133 : voltage);
134 : }
135 : #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
136 :
137 : #if AP_RANGEFINDER_ENABLED
138 124943 : void GCS_MAVLINK_Rover::send_water_depth()
139 : {
140 124943 : if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
141 123570 : return;
142 : }
143 :
144 : // only send for boats:
145 124943 : if (!rover.is_boat()) {
146 114901 : return;
147 : }
148 :
149 10042 : RangeFinder *rangefinder = RangeFinder::get_singleton();
150 :
151 10042 : if (rangefinder == nullptr) {
152 0 : return;
153 : }
154 :
155 : // depth can only be measured by a downward-facing rangefinder:
156 10042 : if (!rangefinder->has_orientation(ROTATION_PITCH_270)) {
157 8669 : return;
158 : }
159 :
160 : // get position
161 1373 : const AP_AHRS &ahrs = AP::ahrs();
162 1373 : Location loc;
163 1373 : IGNORE_RETURN(ahrs.get_location(loc));
164 :
165 1373 : const auto num_sensors = rangefinder->num_sensors();
166 4121 : for (uint8_t i=0; i<num_sensors; i++) {
167 4121 : last_WATER_DEPTH_index += 1;
168 4121 : if (last_WATER_DEPTH_index >= num_sensors) {
169 686 : last_WATER_DEPTH_index = 0;
170 : }
171 :
172 4121 : const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index);
173 :
174 4121 : if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
175 2748 : continue;
176 : }
177 :
178 : // get temperature
179 : float temp_C;
180 1373 : if (!s->get_temp(temp_C)) {
181 687 : temp_C = 0.0f;
182 : }
183 :
184 1373 : const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
185 :
186 2746 : mavlink_msg_water_depth_send(
187 : chan,
188 : AP_HAL::millis(), // time since system boot TODO: take time of measurement
189 1373 : last_WATER_DEPTH_index, // rangefinder instance
190 : sensor_healthy, // sensor healthy
191 : loc.lat, // latitude of vehicle
192 : loc.lng, // longitude of vehicle
193 1373 : loc.alt * 0.01f, // altitude of vehicle (MSL)
194 : ahrs.get_roll_rad(), // roll in radians
195 : ahrs.get_pitch_rad(), // pitch in radians
196 : ahrs.get_yaw_rad(), // yaw in radians
197 : s->distance(), // distance in meters
198 : temp_C); // temperature in degC
199 :
200 1373 : break; // only send one WATER_DEPTH message per loop
201 : }
202 :
203 : }
204 : #endif // AP_RANGEFINDER_ENABLED
205 :
206 : /*
207 : send PID tuning message
208 : */
209 124899 : void GCS_MAVLINK_Rover::send_pid_tuning()
210 : {
211 124899 : Parameters &g = rover.g;
212 124899 : ParametersG2 &g2 = rover.g2;
213 :
214 : const AP_PIDInfo *pid_info;
215 :
216 : // steering PID
217 124899 : if (g.gcs_pid_mask & 1) {
218 17 : pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
219 17 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
220 17 : degrees(pid_info->target),
221 17 : degrees(pid_info->actual),
222 17 : pid_info->FF,
223 17 : pid_info->P,
224 17 : pid_info->I,
225 17 : pid_info->D,
226 17 : pid_info->slew_rate,
227 17 : pid_info->Dmod);
228 17 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
229 0 : return;
230 : }
231 : }
232 :
233 : // speed to throttle PID
234 124899 : if (g.gcs_pid_mask & 2) {
235 0 : pid_info = &g2.attitude_control.get_throttle_speed_pid_info();
236 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
237 0 : pid_info->target,
238 0 : pid_info->actual,
239 0 : pid_info->FF,
240 0 : pid_info->P,
241 0 : pid_info->I,
242 0 : pid_info->D,
243 0 : pid_info->slew_rate,
244 0 : pid_info->Dmod);
245 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
246 0 : return;
247 : }
248 : }
249 :
250 : // pitch to throttle pid
251 124899 : if (g.gcs_pid_mask & 4) {
252 0 : pid_info = &g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info();
253 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
254 0 : degrees(pid_info->target),
255 0 : degrees(pid_info->actual),
256 0 : pid_info->FF,
257 0 : pid_info->P,
258 0 : pid_info->I,
259 0 : pid_info->D,
260 0 : pid_info->slew_rate,
261 0 : pid_info->Dmod);
262 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
263 0 : return;
264 : }
265 : }
266 :
267 : // left wheel rate control pid
268 124899 : if (g.gcs_pid_mask & 8) {
269 0 : pid_info = &g2.wheel_rate_control.get_pid(0).get_pid_info();
270 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_WHEEL_LEFT,
271 0 : pid_info->target,
272 0 : pid_info->actual,
273 0 : pid_info->FF,
274 0 : pid_info->P,
275 0 : pid_info->I,
276 0 : pid_info->D,
277 0 : pid_info->slew_rate,
278 0 : pid_info->Dmod);
279 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
280 0 : return;
281 : }
282 : }
283 :
284 : // right wheel rate control pid
285 124899 : if (g.gcs_pid_mask & 16) {
286 0 : pid_info = &g2.wheel_rate_control.get_pid(1).get_pid_info();
287 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_WHEEL_RIGHT,
288 0 : pid_info->target,
289 0 : pid_info->actual,
290 0 : pid_info->FF,
291 0 : pid_info->P,
292 0 : pid_info->I,
293 0 : pid_info->D,
294 0 : pid_info->slew_rate,
295 0 : pid_info->Dmod);
296 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
297 0 : return;
298 : }
299 : }
300 :
301 : // sailboat heel to mainsail pid
302 124899 : if (g.gcs_pid_mask & 32) {
303 0 : pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info();
304 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_SAIL_HEEL,
305 0 : pid_info->target,
306 0 : pid_info->actual,
307 0 : pid_info->FF,
308 0 : pid_info->P,
309 0 : pid_info->I,
310 0 : pid_info->D,
311 0 : pid_info->slew_rate,
312 0 : pid_info->Dmod);
313 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
314 0 : return;
315 : }
316 : }
317 :
318 : // Position Controller Velocity North PID
319 124899 : if (g.gcs_pid_mask & 64) {
320 0 : pid_info = &g2.pos_control.get_vel_pid().get_pid_info_x();
321 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_VEL_NORTH,
322 0 : pid_info->target,
323 0 : pid_info->actual,
324 0 : pid_info->FF,
325 0 : pid_info->P,
326 0 : pid_info->I,
327 0 : pid_info->D,
328 0 : pid_info->slew_rate,
329 0 : pid_info->Dmod);
330 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
331 0 : return;
332 : }
333 : }
334 :
335 : // Position Controller Velocity East PID
336 124899 : if (g.gcs_pid_mask & 128) {
337 0 : pid_info = &g2.pos_control.get_vel_pid().get_pid_info_y();
338 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_VEL_EAST,
339 0 : pid_info->target,
340 0 : pid_info->actual,
341 0 : pid_info->FF,
342 0 : pid_info->P,
343 0 : pid_info->I,
344 0 : pid_info->D,
345 0 : pid_info->slew_rate,
346 0 : pid_info->Dmod);
347 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
348 0 : return;
349 : }
350 : }
351 : }
352 :
353 124899 : void Rover::send_wheel_encoder_distance(const mavlink_channel_t chan)
354 : {
355 : // send wheel encoder data using wheel_distance message
356 124899 : if (g2.wheel_encoder.num_sensors() > 0) {
357 815 : double distances[MAVLINK_MSG_WHEEL_DISTANCE_FIELD_DISTANCE_LEN] {};
358 1886 : for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
359 1071 : distances[i] = wheel_encoder_last_distance_m[i];
360 : }
361 815 : mavlink_msg_wheel_distance_send(chan, AP_HAL::micros64(), g2.wheel_encoder.num_sensors(), distances);
362 : }
363 124899 : }
364 :
365 258845 : bool GCS_Rover::vehicle_initialised() const
366 : {
367 258845 : return rover.control_mode != &rover.mode_initializing;
368 : }
369 :
370 : // try to send a message, return false if it won't fit in the serial tx buffer
371 7955264 : bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
372 : {
373 7955264 : switch (id) {
374 :
375 124899 : case MSG_WHEEL_DISTANCE:
376 124899 : CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
377 124899 : rover.send_wheel_encoder_distance(chan);
378 124899 : break;
379 :
380 124899 : case MSG_WIND:
381 124899 : CHECK_PAYLOAD_SIZE(WIND);
382 124899 : rover.g2.windvane.send_wind(chan);
383 124899 : break;
384 :
385 : #if AP_OADATABASE_ENABLED
386 124162 : case MSG_ADSB_VEHICLE: {
387 124162 : AP_OADatabase *oadb = AP::oadatabase();
388 124162 : if (oadb != nullptr) {
389 124162 : CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
390 124162 : uint16_t interval_ms = 0;
391 124162 : if (get_ap_message_interval(id, interval_ms)) {
392 124162 : oadb->send_adsb_vehicle(chan, interval_ms);
393 : }
394 : }
395 124162 : break;
396 : }
397 : #endif
398 :
399 : #if AP_RANGEFINDER_ENABLED
400 124943 : case MSG_WATER_DEPTH:
401 124943 : CHECK_PAYLOAD_SIZE(WATER_DEPTH);
402 124943 : send_water_depth();
403 124943 : break;
404 : #endif // AP_RANGEFINDER_ENABLED
405 :
406 7456361 : default:
407 7456361 : return GCS_MAVLINK::try_send_message(id);
408 : }
409 498903 : return true;
410 : }
411 5 : bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
412 : {
413 5 : if (!rover.control_mode->in_guided_mode()) {
414 : // only accept position updates when in GUIDED mode
415 0 : return false;
416 : }
417 :
418 : // make any new wp uploaded instant (in case we are already in Guided mode)
419 5 : return rover.mode_guided.set_desired_location(cmd.content.location);
420 : }
421 :
422 1 : MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
423 : {
424 1 : switch (packet.y) {
425 0 : case 1:
426 0 : if (rover.g2.windvane.start_direction_calibration()) {
427 0 : return MAV_RESULT_ACCEPTED;
428 : } else {
429 0 : return MAV_RESULT_FAILED;
430 : }
431 :
432 0 : case 2:
433 0 : if (rover.g2.windvane.start_speed_calibration()) {
434 0 : return MAV_RESULT_ACCEPTED;
435 : } else {
436 0 : return MAV_RESULT_FAILED;
437 : }
438 :
439 1 : default:
440 1 : break;
441 :
442 : }
443 :
444 1 : return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg);
445 : }
446 :
447 25177 : MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
448 : {
449 25177 : switch (packet.command) {
450 :
451 8 : case MAV_CMD_DO_CHANGE_SPEED:
452 : // param1 : type
453 : // param2 : new speed in m/s
454 8 : switch (SPEED_TYPE(packet.param1)) {
455 0 : case SPEED_TYPE_CLIMB_SPEED:
456 : case SPEED_TYPE_DESCENT_SPEED:
457 : case SPEED_TYPE_ENUM_END:
458 0 : return MAV_RESULT_DENIED;
459 :
460 8 : case SPEED_TYPE_AIRSPEED: // Airspeed is treated as ground speed for GCS compatibility
461 : case SPEED_TYPE_GROUNDSPEED:
462 8 : break;
463 : }
464 8 : if (!rover.control_mode->set_desired_speed(packet.param2)) {
465 0 : return MAV_RESULT_FAILED;
466 : }
467 8 : return MAV_RESULT_ACCEPTED;
468 :
469 7 : case MAV_CMD_DO_REPOSITION:
470 7 : return handle_command_int_do_reposition(packet);
471 :
472 6 : case MAV_CMD_DO_SET_REVERSE:
473 : // param1 : Direction (0=Forward, 1=Reverse)
474 6 : rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
475 6 : return MAV_RESULT_ACCEPTED;
476 :
477 2 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
478 2 : if (rover.set_mode(rover.mode_rtl, ModeReason::GCS_COMMAND)) {
479 2 : return MAV_RESULT_ACCEPTED;
480 : }
481 0 : return MAV_RESULT_FAILED;
482 :
483 1 : case MAV_CMD_DO_MOTOR_TEST:
484 : // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
485 : // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
486 : // param3 : throttle (range depends upon param2)
487 : // param4 : timeout (in seconds)
488 1 : return rover.mavlink_motor_test_start(*this,
489 1 : (AP_MotorsUGV::motor_test_order)packet.param1,
490 1 : static_cast<uint8_t>(packet.param2),
491 1 : static_cast<int16_t>(packet.param3),
492 1 : packet.param4);
493 :
494 4 : case MAV_CMD_MISSION_START:
495 4 : if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
496 : // first-item/last item not supported
497 0 : return MAV_RESULT_DENIED;
498 : }
499 4 : if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) {
500 4 : return MAV_RESULT_ACCEPTED;
501 : }
502 0 : return MAV_RESULT_FAILED;
503 :
504 25149 : default:
505 25149 : return GCS_MAVLINK::handle_command_int_packet(packet, msg);
506 : }
507 : }
508 :
509 7 : MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
510 : {
511 7 : const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
512 7 : if (!rover.control_mode->in_guided_mode() && !change_modes) {
513 0 : return MAV_RESULT_DENIED;
514 : }
515 :
516 7 : if (packet.x == 0 && packet.y == 0) {
517 0 : return MAV_RESULT_DENIED;
518 : }
519 :
520 7 : Location requested_location {};
521 7 : if (!location_from_command_t(packet, requested_location)) {
522 0 : return MAV_RESULT_DENIED;
523 : }
524 :
525 7 : if (!rover.control_mode->in_guided_mode()) {
526 0 : if (!rover.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) {
527 0 : return MAV_RESULT_FAILED;
528 : }
529 : }
530 :
531 7 : if (is_positive(packet.param1)) {
532 0 : if (!rover.control_mode->set_desired_speed(packet.param1)) {
533 0 : return MAV_RESULT_FAILED;
534 : }
535 : }
536 :
537 : // set the destination
538 7 : if (!rover.mode_guided.set_desired_location(requested_location)) {
539 1 : return MAV_RESULT_FAILED;
540 : }
541 :
542 6 : return MAV_RESULT_ACCEPTED;
543 : }
544 :
545 280649 : void GCS_MAVLINK_Rover::handle_message(const mavlink_message_t &msg)
546 : {
547 280649 : switch (msg.msgid) {
548 :
549 441 : case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
550 441 : handle_set_attitude_target(msg);
551 441 : break;
552 :
553 309 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
554 309 : handle_set_position_target_local_ned(msg);
555 309 : break;
556 :
557 1358 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
558 1358 : handle_set_position_target_global_int(msg);
559 1358 : break;
560 :
561 278541 : default:
562 278541 : GCS_MAVLINK::handle_message(msg);
563 278303 : break;
564 : }
565 280411 : }
566 :
567 45 : void GCS_MAVLINK_Rover::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
568 : {
569 45 : manual_override(rover.channel_steer, packet.y, 1000, 2000, tnow);
570 45 : manual_override(rover.channel_throttle, packet.z, 1000, 2000, tnow);
571 45 : }
572 :
573 441 : void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg)
574 : {
575 : // decode packet
576 : mavlink_set_attitude_target_t packet;
577 441 : mavlink_msg_set_attitude_target_decode(&msg, &packet);
578 :
579 : // exit if vehicle is not in Guided mode
580 441 : if (!rover.control_mode->in_guided_mode()) {
581 0 : return;
582 : }
583 :
584 : // ensure type_mask specifies to use thrust
585 441 : if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
586 0 : return;
587 : }
588 :
589 : // convert thrust to ground speed
590 441 : packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
591 441 : const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
592 :
593 : // if the body_yaw_rate field is ignored, convert quaternion to heading
594 441 : if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
595 : // convert quaternion to heading
596 390 : float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
597 390 : rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
598 : } else {
599 : // use body_yaw_rate field
600 51 : rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
601 : }
602 : }
603 :
604 : // if we receive a message where the user has not masked out
605 : // acceleration from the input packet we send a curt message
606 : // informing them:
607 0 : void GCS_MAVLINK_Rover::send_acc_ignore_must_be_set_message(const char *msgname)
608 : {
609 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Ignoring %s; set ACC_IGNORE in mask", msgname);
610 0 : }
611 :
612 309 : void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_message_t &msg)
613 : {
614 : // decode packet
615 : mavlink_set_position_target_local_ned_t packet;
616 309 : mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
617 :
618 : // exit if vehicle is not in Guided mode
619 309 : if (!rover.control_mode->in_guided_mode()) {
620 309 : return;
621 : }
622 :
623 : // need ekf origin
624 309 : Location ekf_origin;
625 309 : if (!rover.ahrs.get_origin(ekf_origin)) {
626 0 : return;
627 : }
628 :
629 : // check for supported coordinate frames
630 309 : switch (packet.coordinate_frame) {
631 309 : case MAV_FRAME_LOCAL_NED:
632 : case MAV_FRAME_LOCAL_OFFSET_NED:
633 : case MAV_FRAME_BODY_NED:
634 : case MAV_FRAME_BODY_OFFSET_NED:
635 309 : break;
636 :
637 0 : default:
638 0 : return;
639 : }
640 :
641 309 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
642 309 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
643 309 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
644 309 : bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
645 309 : bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
646 :
647 : // prepare target position
648 309 : Location target_loc = rover.current_loc;
649 309 : if (!pos_ignore) {
650 309 : switch (packet.coordinate_frame) {
651 0 : case MAV_FRAME_BODY_NED:
652 : case MAV_FRAME_BODY_OFFSET_NED: {
653 : // rotate from body-frame to NE frame
654 0 : const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
655 0 : const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
656 : // add offset to current location
657 0 : target_loc.offset(ne_x, ne_y);
658 : }
659 0 : break;
660 :
661 0 : case MAV_FRAME_LOCAL_OFFSET_NED:
662 : // add offset to current location
663 0 : target_loc.offset(packet.x, packet.y);
664 0 : break;
665 :
666 309 : case MAV_FRAME_LOCAL_NED:
667 : default:
668 : // MAV_FRAME_LOCAL_NED is interpreted as an offset from EKF origin
669 309 : target_loc = ekf_origin;
670 309 : target_loc.offset(packet.x, packet.y);
671 309 : break;
672 : }
673 : }
674 :
675 309 : float target_speed = 0.0f;
676 309 : float target_yaw_cd = 0.0f;
677 :
678 : // consume velocity and convert to target speed and heading
679 309 : if (!vel_ignore) {
680 0 : const float speed_max = rover.control_mode->get_speed_default();
681 : // convert vector length into a speed
682 0 : target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
683 : // convert vector direction to target yaw
684 0 : target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
685 :
686 : // rotate target yaw if provided in body-frame
687 0 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
688 0 : target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
689 : }
690 : }
691 :
692 : // consume yaw heading
693 309 : if (!yaw_ignore) {
694 0 : target_yaw_cd = degrees(packet.yaw) * 100.0f;
695 : // rotate target yaw if provided in body-frame
696 0 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
697 0 : target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
698 : }
699 : }
700 : // consume yaw rate
701 309 : float target_turn_rate_cds = 0.0f;
702 309 : if (!yaw_rate_ignore) {
703 0 : target_turn_rate_cds = degrees(packet.yaw_rate) * 100.0f;
704 : }
705 :
706 : // handling case when both velocity and either yaw or yaw-rate are provided
707 : // by default, we consider that the rover will drive forward
708 309 : float speed_dir = 1.0f;
709 309 : if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
710 : // Note: we are using the x-axis velocity to determine direction even though
711 : // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
712 0 : if (is_negative(packet.vx)) {
713 0 : speed_dir = -1.0f;
714 : }
715 : }
716 :
717 309 : if (!acc_ignore) {
718 : // ignore any command where acceleration is not ignored
719 0 : send_acc_ignore_must_be_set_message("SET_POSITION_TARGET_LOCAL_NED");
720 0 : return;
721 : }
722 :
723 : // set guided mode targets
724 309 : if (!pos_ignore) {
725 : // consume position target
726 309 : if (!rover.mode_guided.set_desired_location(target_loc)) {
727 : // GCS will need to monitor desired location to
728 : // see if they are having an effect.
729 : }
730 309 : return;
731 : }
732 :
733 0 : if (!vel_ignore && yaw_ignore && yaw_rate_ignore) {
734 : // consume velocity
735 0 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
736 0 : } else if (!vel_ignore && yaw_ignore && !yaw_rate_ignore) {
737 : // consume velocity and turn rate
738 0 : rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
739 0 : } else if (!vel_ignore && !yaw_ignore && yaw_rate_ignore) {
740 : // consume velocity and heading
741 0 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
742 0 : } else if (vel_ignore && !yaw_ignore && yaw_rate_ignore) {
743 : // consume just target heading (probably only skid steering vehicles can do this)
744 0 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
745 0 : } else if (vel_ignore && yaw_ignore && !yaw_rate_ignore) {
746 : // consume just turn rate (probably only skid steering vehicles can do this)
747 0 : rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
748 : }
749 : }
750 :
751 1358 : void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_message_t &msg)
752 : {
753 : // decode packet
754 : mavlink_set_position_target_global_int_t packet;
755 1358 : mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
756 :
757 : // exit if vehicle is not in Guided mode
758 1358 : if (!rover.control_mode->in_guided_mode()) {
759 46 : return;
760 : }
761 : // check for supported coordinate frames
762 1358 : switch (packet.coordinate_frame) {
763 1358 : case MAV_FRAME_GLOBAL:
764 : case MAV_FRAME_GLOBAL_INT:
765 : case MAV_FRAME_GLOBAL_RELATIVE_ALT:
766 : case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
767 : case MAV_FRAME_GLOBAL_TERRAIN_ALT:
768 : case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
769 1358 : break;
770 :
771 0 : default:
772 0 : return;
773 : }
774 :
775 1358 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
776 1358 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
777 1358 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
778 1358 : bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
779 1358 : bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
780 :
781 : // prepare target position
782 1358 : Location target_loc = rover.current_loc;
783 1358 : if (!pos_ignore) {
784 : // sanity check location
785 46 : if (!check_latlng(packet.lat_int, packet.lon_int)) {
786 : // result = MAV_RESULT_FAILED;
787 0 : return;
788 : }
789 46 : target_loc.lat = packet.lat_int;
790 46 : target_loc.lng = packet.lon_int;
791 : }
792 :
793 1358 : float target_speed = 0.0f;
794 1358 : float target_yaw_cd = 0.0f;
795 :
796 : // consume velocity and convert to target speed and heading
797 1358 : if (!vel_ignore) {
798 1312 : const float speed_max = rover.control_mode->get_speed_default();
799 : // convert vector length into a speed
800 1312 : target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
801 : // convert vector direction to target yaw
802 1312 : target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
803 : }
804 :
805 : // consume yaw heading
806 1358 : if (!yaw_ignore) {
807 0 : target_yaw_cd = degrees(packet.yaw) * 100.0f;
808 : }
809 :
810 : // consume yaw rate
811 1358 : float target_turn_rate_cds = 0.0f;
812 1358 : if (!yaw_rate_ignore) {
813 0 : target_turn_rate_cds = degrees(packet.yaw_rate) * 100.0f;
814 : }
815 :
816 : // handling case when both velocity and either yaw or yaw-rate are provided
817 : // by default, we consider that the rover will drive forward
818 1358 : float speed_dir = 1.0f;
819 1358 : if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
820 : // Note: we are using the x-axis velocity to determine direction even though
821 : // the frame is provided in MAV_FRAME_GLOBAL_xxx
822 0 : if (is_negative(packet.vx)) {
823 0 : speed_dir = -1.0f;
824 : }
825 : }
826 :
827 1358 : if (!acc_ignore) {
828 : // ignore any command where acceleration is not ignored
829 0 : send_acc_ignore_must_be_set_message("SET_POSITION_TARGET_GLOBAL_INT");
830 0 : return;
831 : }
832 :
833 : // set guided mode targets
834 1358 : if (!pos_ignore) {
835 : // consume position target
836 46 : if (!rover.mode_guided.set_desired_location(target_loc)) {
837 : // GCS will just need to look at desired location
838 : // outputs to see if it having an effect.
839 : }
840 46 : return;
841 : }
842 :
843 1312 : if (!vel_ignore && yaw_ignore && yaw_rate_ignore) {
844 : // consume velocity
845 1312 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
846 0 : } else if (!vel_ignore && yaw_ignore && !yaw_rate_ignore) {
847 : // consume velocity and turn rate
848 0 : rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
849 0 : } else if (!vel_ignore && !yaw_ignore && yaw_rate_ignore) {
850 : // consume velocity
851 0 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
852 0 : } else if (vel_ignore && !yaw_ignore && yaw_rate_ignore) {
853 : // consume just target heading (probably only skid steering vehicles can do this)
854 0 : rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
855 0 : } else if (vel_ignore && yaw_ignore && !yaw_rate_ignore) {
856 : // consume just turn rate(probably only skid steering vehicles can do this)
857 0 : rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
858 : }
859 : }
860 :
861 : /*
862 : handle a LANDING_TARGET command. The timestamp has been jitter corrected
863 : */
864 0 : void GCS_MAVLINK_Rover::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
865 : {
866 : #if AC_PRECLAND_ENABLED
867 0 : rover.precland.handle_msg(packet, timestamp_ms);
868 : #endif
869 0 : }
870 :
871 9 : uint64_t GCS_MAVLINK_Rover::capabilities() const
872 : {
873 : return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
874 : MAV_PROTOCOL_CAPABILITY_MISSION_INT |
875 : MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
876 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
877 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
878 : MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
879 9 : GCS_MAVLINK::capabilities());
880 : }
881 :
882 : #if HAL_HIGH_LATENCY2_ENABLED
883 727 : uint8_t GCS_MAVLINK_Rover::high_latency_tgt_heading() const
884 : {
885 727 : const Mode *control_mode = rover.control_mode;
886 727 : if (rover.control_mode->is_autopilot_mode()) {
887 : // need to convert -180->180 to 0->360/2
888 0 : return wrap_360(control_mode->wp_bearing()) / 2;
889 : }
890 727 : return 0;
891 : }
892 :
893 727 : uint16_t GCS_MAVLINK_Rover::high_latency_tgt_dist_dam() const
894 : {
895 727 : const Mode *control_mode = rover.control_mode;
896 727 : if (rover.control_mode->is_autopilot_mode()) {
897 0 : return MIN(static_cast<uint16_t>(control_mode->get_distance_to_destination() * 0.1), UINT16_MAX);
898 : }
899 727 : return 0;
900 : }
901 :
902 727 : uint8_t GCS_MAVLINK_Rover::high_latency_tgt_airspeed() const
903 : {
904 727 : const Mode *control_mode = rover.control_mode;
905 727 : if (rover.control_mode->is_autopilot_mode()) {
906 : // return units are m/s*5
907 0 : return MIN((vfr_hud_airspeed() - control_mode->speed_error()) * 5, UINT8_MAX);
908 : }
909 727 : return 0;
910 : }
911 :
912 727 : uint8_t GCS_MAVLINK_Rover::high_latency_wind_speed() const
913 : {
914 727 : if (rover.g2.windvane.enabled()) {
915 : // return units are m/s*5
916 0 : return MIN(rover.g2.windvane.get_true_wind_speed() * 5, UINT8_MAX);
917 : }
918 727 : return 0;
919 : }
920 :
921 727 : uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
922 : {
923 727 : if (rover.g2.windvane.enabled()) {
924 : // return units are deg/2
925 0 : return wrap_360(degrees(rover.g2.windvane.get_true_wind_direction_rad())) / 2;
926 : }
927 727 : return 0;
928 : }
929 : #endif // HAL_HIGH_LATENCY2_ENABLED
930 :
931 : // Send the mode with the given index (not mode number!) return the total number of modes
932 : // Index starts at 1
933 0 : uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const
934 : {
935 0 : const Mode* modes[] {
936 : &rover.mode_manual,
937 : &rover.mode_acro,
938 : &rover.mode_steering,
939 : &rover.mode_hold,
940 : &rover.mode_loiter,
941 : #if MODE_FOLLOW_ENABLED
942 : &rover.mode_follow,
943 : #endif
944 : &rover.mode_simple,
945 : &rover.g2.mode_circle,
946 : &rover.mode_auto,
947 : &rover.mode_rtl,
948 : &rover.mode_smartrtl,
949 : &rover.mode_guided,
950 : &rover.mode_initializing,
951 : #if MODE_DOCK_ENABLED
952 0 : (Mode *)rover.g2.mode_dock_ptr,
953 : #endif
954 0 : };
955 :
956 0 : const uint8_t mode_count = ARRAY_SIZE(modes);
957 :
958 : // Convert to zero indexed
959 0 : const uint8_t index_zero = index - 1;
960 0 : if (index_zero >= mode_count) {
961 : // Mode does not exist!?
962 0 : return mode_count;
963 : }
964 :
965 : // Ask the mode for its name and number
966 0 : const char* name = modes[index_zero]->name();
967 0 : const Mode::Number mode_number = modes[index_zero]->mode_number();
968 :
969 : // the check here must be the same as the one in `get_available_mode_enabled_mask`
970 0 : const bool user_selectable = modes[index_zero]->enabled() && rover.gcs_mode_enabled(mode_number);
971 :
972 0 : mavlink_msg_available_modes_send(
973 0 : chan,
974 : mode_count,
975 : index,
976 : MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
977 : (uint8_t)mode_number,
978 : // MAV_MODE_PROPERTY bitmask,
979 : user_selectable ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
980 : name
981 : );
982 :
983 0 : return mode_count;
984 : }
|
