Line data Source code
1 : #include "Sub.h"
2 :
3 : #include "GCS_MAVLink_Sub.h"
4 : #include <AP_RPM/AP_RPM_config.h>
5 : #include <AP_RangeFinder/AP_RangeFinder.h>
6 : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
7 :
8 6094 : MAV_TYPE GCS_Sub::frame_type() const
9 : {
10 6094 : return MAV_TYPE_SUBMARINE;
11 : }
12 :
13 6094 : uint8_t GCS_MAVLINK_Sub::base_mode() const
14 : {
15 6094 : uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
16 :
17 : // work out the base_mode. This value is not very useful
18 : // for APM, but we calculate it as best we can so a generic
19 : // MAVLink enabled ground station can work out something about
20 : // what the MAV is up to. The actual bit values are highly
21 : // ambiguous for most of the APM flight modes. In practice, you
22 : // only get useful information from the custom_mode, which maps to
23 : // the APM flight mode and has a well defined meaning in the
24 : // ArduPlane documentation
25 6094 : switch (sub.control_mode) {
26 2035 : case Mode::Number::AUTO:
27 : case Mode::Number::GUIDED:
28 : case Mode::Number::CIRCLE:
29 : case Mode::Number::POSHOLD:
30 2035 : _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
31 : // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
32 : // APM does in any mode, as that is defined as "system finds its own goal
33 : // positions", which APM does not currently do
34 2035 : break;
35 4059 : default:
36 4059 : break;
37 : }
38 :
39 : // all modes except INITIALISING have some form of manual
40 : // override if stick mixing is enabled
41 6094 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
42 :
43 6094 : if (sub.motors.armed()) {
44 3137 : _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
45 : }
46 :
47 : // indicate we have set a custom mode
48 6094 : _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
49 :
50 6094 : return _base_mode;
51 : }
52 :
53 6094 : uint32_t GCS_Sub::custom_mode() const
54 : {
55 6094 : return (uint32_t)sub.control_mode;
56 : }
57 :
58 6094 : MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const
59 : {
60 : // set system as critical if any failsafe have triggered
61 6094 : if (sub.any_failsafe_triggered()) {
62 468 : return MAV_STATE_CRITICAL;
63 : }
64 :
65 5626 : if (sub.motors.armed()) {
66 3063 : return MAV_STATE_ACTIVE;
67 : }
68 2563 : if (!sub.ap.initialised) {
69 0 : return MAV_STATE_BOOT;
70 : }
71 :
72 2563 : return MAV_STATE_STANDBY;
73 : }
74 :
75 66 : void GCS_MAVLINK_Sub::send_banner()
76 : {
77 66 : GCS_MAVLINK::send_banner();
78 66 : send_text(MAV_SEVERITY_INFO, "Frame: %s", sub.motors.get_frame_string());
79 66 : }
80 :
81 52070 : void GCS_MAVLINK_Sub::send_nav_controller_output() const
82 : {
83 52070 : const Vector3f &targets = sub.attitude_control.get_att_target_euler_cd();
84 156210 : mavlink_msg_nav_controller_output_send(
85 52070 : chan,
86 52070 : targets.x * 1.0e-2f,
87 52070 : targets.y * 1.0e-2f,
88 52070 : targets.z * 1.0e-2f,
89 52070 : sub.wp_nav.get_wp_bearing_to_destination_cd() * 1.0e-2f,
90 52070 : MIN(sub.wp_nav.get_wp_distance_to_destination_cm() * 1.0e-2f, UINT16_MAX),
91 52070 : sub.pos_control.get_pos_error_U_cm() * 1.0e-2f,
92 : 0,
93 : 0);
94 52070 : }
95 :
96 : // returns a Location to which the vehicle is currently heading, or
97 : // would head in an autonomous mode
98 52071 : bool GCS_MAVLINK_Sub::get_target_location(Location &loc) const
99 : {
100 : // in Guided mode, only Guided_WP uses wp_nav; the other submodes keep
101 : // their own targets. Guided_WP breaks out to the wp_nav return below.
102 52071 : if (sub.flightmode->in_guided_mode()) {
103 17811 : switch (sub.guided_mode) {
104 15946 : case Guided_WP:
105 15946 : break;
106 1865 : case Guided_PosVelAccel: {
107 1865 : Vector3f pos_neu_cm;
108 1865 : if (!sub.mode_guided.get_posvelaccel_target_NEU_cm(pos_neu_cm)) {
109 0 : return false;
110 : }
111 1865 : loc = Location(pos_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
112 1865 : return true;
113 : }
114 0 : case Guided_Velocity:
115 : case Guided_Angle:
116 : // no fixed position target to report
117 0 : return false;
118 : }
119 : }
120 :
121 : // for non-guided modes (Auto, etc.) fall back to wp_nav as before
122 50206 : return sub.wp_nav.is_active() && sub.wp_nav.get_wp_destination_loc(loc);
123 : }
124 :
125 55950 : int16_t GCS_MAVLINK_Sub::vfr_hud_throttle() const
126 : {
127 55950 : return (int16_t)(sub.motors.get_throttle() * 100);
128 : }
129 :
130 55950 : float GCS_MAVLINK_Sub::vfr_hud_alt() const
131 : {
132 55950 : return sub.get_alt_msl();
133 : }
134 :
135 : // Work around to get temperature sensor data out
136 52070 : void GCS_MAVLINK_Sub::send_scaled_pressure3()
137 : {
138 : #if AP_TEMPERATURE_SENSOR_ENABLED
139 : float temperature;
140 52070 : if (!sub.temperature_sensor.get_temperature(temperature)) {
141 : // Fall back to original behaviour
142 72 : GCS_MAVLINK::send_scaled_pressure3();
143 72 : return;
144 : }
145 51998 : mavlink_msg_scaled_pressure3_send(
146 : chan,
147 : AP_HAL::millis(),
148 : 0,
149 : 0,
150 51998 : temperature * 100,
151 : 0); // TODO: use differential pressure temperature
152 : #else
153 : // Fall back to standard behaviour
154 : GCS_MAVLINK::send_scaled_pressure3();
155 : #endif
156 : }
157 :
158 52069 : bool GCS_MAVLINK_Sub::send_info()
159 : {
160 : // Just do this all at once, hopefully the hard-wire telemetry requirement means this is ok
161 : // Name is char[10]
162 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
163 104138 : send_named_float("CamTilt",
164 52069 : 1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_tilt) / 2.0f + 0.5f));
165 :
166 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
167 104138 : send_named_float("CamPan",
168 52069 : 1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_pan) / 2.0f + 0.5f));
169 :
170 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
171 52069 : send_named_float("TetherTrn",
172 52069 : (float)sub.quarter_turn_count / 4);
173 :
174 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
175 104138 : send_named_float("Lights1",
176 52069 : SRV_Channels::get_output_norm(SRV_Channel::k_lights1) / 2.0f + 0.5f);
177 :
178 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
179 104138 : send_named_float("Lights2",
180 52069 : SRV_Channels::get_output_norm(SRV_Channel::k_lights2) / 2.0f + 0.5f);
181 :
182 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
183 52069 : send_named_float("PilotGain", sub.gain);
184 :
185 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
186 52069 : send_named_float("InputHold", sub.input_hold_engaged);
187 :
188 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
189 52069 : send_named_float("RollPitch", sub.roll_pitch_flag);
190 :
191 52069 : CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT);
192 52069 : send_named_float("RFTarget", sub.mode_surftrak.get_rangefinder_target_cm() * 0.01f);
193 :
194 52069 : return true;
195 : }
196 :
197 : #if AP_RANGEFINDER_ENABLED
198 46 : void GCS_MAVLINK_Sub::send_water_depth()
199 : {
200 46 : if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
201 0 : return;
202 : }
203 :
204 46 : RangeFinder *rangefinder = RangeFinder::get_singleton();
205 46 : if (rangefinder == nullptr) {
206 0 : return;
207 : }
208 :
209 : // depth can only be measured by a downward-facing rangefinder:
210 46 : if (!rangefinder->has_orientation(ROTATION_PITCH_270)) {
211 0 : return;
212 : }
213 :
214 : // get position
215 46 : const AP_AHRS &ahrs = AP::ahrs();
216 46 : Location loc;
217 46 : IGNORE_RETURN(ahrs.get_location(loc));
218 :
219 46 : const auto num_sensors = rangefinder->num_sensors();
220 46 : for (uint8_t i=0; i<num_sensors; i++) {
221 46 : last_WATER_DEPTH_index += 1;
222 46 : if (last_WATER_DEPTH_index >= num_sensors) {
223 46 : last_WATER_DEPTH_index = 0;
224 : }
225 :
226 46 : const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index);
227 46 : if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
228 0 : continue;
229 : }
230 :
231 : // get temperature
232 : float temp_C;
233 46 : if (!s->get_temp(temp_C)) {
234 : // TODO: check known water temperature sources (temp sensor, external baro)
235 0 : temp_C = 0.0f;
236 : }
237 :
238 46 : const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
239 :
240 92 : mavlink_msg_water_depth_send(
241 : chan,
242 : AP_HAL::millis(), // time since system boot TODO: take time of measurement
243 46 : last_WATER_DEPTH_index, // rangefinder instance
244 : sensor_healthy, // sensor healthy
245 : loc.lat, // latitude of vehicle
246 : loc.lng, // longitude of vehicle
247 46 : loc.alt * 0.01f, // altitude of vehicle (MSL)
248 : ahrs.get_roll_rad(), // roll in radians
249 : ahrs.get_pitch_rad(), // pitch in radians
250 : ahrs.get_yaw_rad(), // yaw in radians
251 : s->distance(), // distance in meters
252 : temp_C); // temperature in degC
253 :
254 46 : break; // only send one WATER_DEPTH message per loop
255 : }
256 : }
257 : #endif // AP_RANGEFINDER_ENABLED
258 :
259 : /*
260 : send PID tuning message
261 : */
262 55949 : void GCS_MAVLINK_Sub::send_pid_tuning()
263 : {
264 55949 : const Parameters &g = sub.g;
265 55949 : AP_AHRS &ahrs = AP::ahrs();
266 55949 : AC_AttitudeControl_Sub &attitude_control = sub.attitude_control;
267 :
268 55949 : const Vector3f &gyro = ahrs.get_gyro();
269 55949 : if (g.gcs_pid_mask & 1) {
270 4 : const AP_PIDInfo &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
271 8 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
272 4 : pid_info.target*0.01f,
273 4 : degrees(gyro.x),
274 4 : pid_info.FF*0.01f,
275 4 : pid_info.P*0.01f,
276 4 : pid_info.I*0.01f,
277 4 : pid_info.D*0.01f,
278 4 : pid_info.slew_rate,
279 4 : pid_info.Dmod);
280 4 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
281 0 : return;
282 : }
283 : }
284 55949 : if (g.gcs_pid_mask & 2) {
285 0 : const AP_PIDInfo &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
286 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
287 0 : pid_info.target*0.01f,
288 0 : degrees(gyro.y),
289 0 : pid_info.FF*0.01f,
290 0 : pid_info.P*0.01f,
291 0 : pid_info.I*0.01f,
292 0 : pid_info.D*0.01f,
293 0 : pid_info.slew_rate,
294 0 : pid_info.Dmod);
295 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
296 0 : return;
297 : }
298 : }
299 55949 : if (g.gcs_pid_mask & 4) {
300 0 : const AP_PIDInfo &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
301 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
302 0 : pid_info.target*0.01f,
303 0 : degrees(gyro.z),
304 0 : pid_info.FF*0.01f,
305 0 : pid_info.P*0.01f,
306 0 : pid_info.I*0.01f,
307 0 : pid_info.D*0.01f,
308 0 : pid_info.slew_rate,
309 0 : pid_info.Dmod);
310 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
311 0 : return;
312 : }
313 : }
314 55949 : if (g.gcs_pid_mask & 8) {
315 0 : const AP_PIDInfo &pid_info = sub.pos_control.D_get_accel_pid().get_pid_info();
316 0 : mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
317 0 : pid_info.target*0.01f,
318 0 : -(ahrs.get_accel_ef().z + GRAVITY_MSS),
319 0 : pid_info.FF*0.01f,
320 0 : pid_info.P*0.01f,
321 0 : pid_info.I*0.01f,
322 0 : pid_info.D*0.01f,
323 0 : pid_info.slew_rate,
324 0 : pid_info.Dmod);
325 0 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
326 0 : return;
327 : }
328 : }
329 : }
330 :
331 108218 : bool GCS_Sub::vehicle_initialised() const {
332 108218 : return sub.ap.initialised;
333 : }
334 :
335 : // try to send a message, return false if it won't fit in the serial tx buffer
336 7219053 : bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
337 : {
338 7219053 : switch (id) {
339 :
340 52069 : case MSG_NAMED_FLOAT:
341 52069 : send_info();
342 52069 : break;
343 :
344 52523 : case MSG_WIND: // other vehicles do something custom with wind:
345 52523 : return true;
346 :
347 : #if AP_RANGEFINDER_ENABLED
348 46 : case MSG_WATER_DEPTH:
349 46 : CHECK_PAYLOAD_SIZE(WATER_DEPTH);
350 46 : send_water_depth();
351 46 : break;
352 : #endif // AP_RANGEFINDER_ENABLED
353 :
354 7114415 : default:
355 7114415 : return GCS_MAVLINK::try_send_message(id);
356 : }
357 :
358 52115 : return true;
359 : }
360 :
361 0 : bool GCS_MAVLINK_Sub::handle_guided_request(AP_Mission::Mission_Command &cmd)
362 : {
363 0 : return sub.do_guided(cmd);
364 : }
365 :
366 0 : MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
367 : {
368 0 : if (sub.motors.armed()) {
369 0 : gcs().send_text(MAV_SEVERITY_INFO, "Disarm before calibration.");
370 0 : return MAV_RESULT_FAILED;
371 : }
372 :
373 0 : if (!sub.control_check_barometer()) {
374 0 : return MAV_RESULT_FAILED;
375 : }
376 :
377 0 : AP::baro().calibrate(true);
378 0 : return MAV_RESULT_ACCEPTED;
379 : }
380 :
381 0 : MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
382 : {
383 0 : if (packet.y == 1) {
384 : // compassmot calibration
385 : //result = sub.mavlink_compassmot(chan);
386 0 : gcs().send_text(MAV_SEVERITY_INFO, "#CompassMot calibration not supported");
387 0 : return MAV_RESULT_UNSUPPORTED;
388 : }
389 :
390 0 : return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg);
391 : }
392 :
393 0 : MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc)
394 : {
395 0 : if (!roi_loc.check_latlng()) {
396 0 : return MAV_RESULT_FAILED;
397 : }
398 0 : sub.mode_auto.set_auto_yaw_roi(roi_loc);
399 0 : return MAV_RESULT_ACCEPTED;
400 : }
401 :
402 2 : MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
403 : {
404 2 : const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
405 2 : if (!sub.flightmode->in_guided_mode() && !change_modes) {
406 0 : return MAV_RESULT_DENIED;
407 : }
408 :
409 2 : Location request_location;
410 2 : if (!location_from_command_t(packet, request_location)) {
411 0 : return MAV_RESULT_DENIED;
412 : }
413 :
414 2 : if (request_location.sanitize(sub.current_loc)) {
415 : // if the location wasn't already sane don't load it
416 0 : return MAV_RESULT_DENIED; // failed as the location is not valid
417 : }
418 :
419 : // we need to do this first, as we don't want to change the flight mode unless we can also set the target
420 2 : if (!sub.mode_guided.guided_set_destination(request_location)) {
421 0 : return MAV_RESULT_FAILED;
422 : }
423 :
424 2 : if (!sub.flightmode->in_guided_mode()) {
425 0 : if (!sub.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) {
426 0 : return MAV_RESULT_FAILED;
427 : }
428 : // the position won't have been loaded if we had to change the flight mode, so load it again
429 0 : if (!sub.mode_guided.guided_set_destination(request_location)) {
430 0 : return MAV_RESULT_FAILED;
431 : }
432 : }
433 :
434 2 : return MAV_RESULT_ACCEPTED;
435 : }
436 :
437 34628 : MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
438 : {
439 34628 : switch(packet.command) {
440 :
441 12 : case MAV_CMD_CONDITION_YAW:
442 12 : return handle_MAV_CMD_CONDITION_YAW(packet);
443 :
444 6 : case MAV_CMD_DO_CHANGE_SPEED:
445 6 : return handle_MAV_CMD_DO_CHANGE_SPEED(packet);
446 :
447 0 : case MAV_CMD_DO_MOTOR_TEST:
448 0 : return handle_MAV_CMD_DO_MOTOR_TEST(packet);
449 :
450 2 : case MAV_CMD_DO_REPOSITION:
451 2 : return handle_command_int_do_reposition(packet);
452 :
453 4 : case MAV_CMD_MISSION_START:
454 4 : if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
455 : // first-item/last item not supported
456 0 : return MAV_RESULT_DENIED;
457 : }
458 4 : return handle_MAV_CMD_MISSION_START(packet);
459 :
460 2 : case MAV_CMD_NAV_LOITER_UNLIM:
461 2 : return handle_MAV_CMD_NAV_LOITER_UNLIM(packet);
462 :
463 2 : case MAV_CMD_NAV_LAND:
464 2 : return handle_MAV_CMD_NAV_LAND(packet);
465 :
466 : }
467 :
468 34600 : return GCS_MAVLINK::handle_command_int_packet(packet, msg);
469 : }
470 :
471 2 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet)
472 : {
473 2 : if (!sub.set_mode(Mode::Number::POSHOLD, ModeReason::GCS_COMMAND)) {
474 0 : return MAV_RESULT_FAILED;
475 : }
476 2 : return MAV_RESULT_ACCEPTED;
477 : }
478 :
479 2 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet)
480 : {
481 2 : if (!sub.set_mode(Mode::Number::SURFACE, ModeReason::GCS_COMMAND)) {
482 0 : return MAV_RESULT_FAILED;
483 : }
484 2 : return MAV_RESULT_ACCEPTED;
485 : }
486 :
487 12 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet)
488 : {
489 : // param1 : target angle [0-360]
490 : // param2 : speed during change [deg per second]
491 : // param3 : direction (-1:ccw, +1:cw)
492 : // param4 : relative offset (1) or absolute angle (0)
493 36 : if ((packet.param1 >= 0.0f) &&
494 26 : (packet.param1 <= 360.0f) &&
495 14 : (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
496 12 : sub.mode_auto.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, (uint8_t)packet.param4);
497 12 : return MAV_RESULT_ACCEPTED;
498 : }
499 0 : return MAV_RESULT_DENIED;
500 : }
501 :
502 6 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
503 : {
504 6 : if (!is_positive(packet.param2)) {
505 : // Target speed must be larger than zero
506 0 : return MAV_RESULT_DENIED;
507 : }
508 :
509 6 : switch (SPEED_TYPE(packet.param1)) {
510 0 : case SPEED_TYPE_CLIMB_SPEED:
511 : case SPEED_TYPE_DESCENT_SPEED:
512 : case SPEED_TYPE_ENUM_END:
513 0 : break;
514 :
515 6 : case SPEED_TYPE_AIRSPEED: // Airspeed is treated as ground speed for GCS compatibility
516 : case SPEED_TYPE_GROUNDSPEED:
517 6 : sub.wp_nav.set_speed_NE_cms(packet.param2 * 100.0);
518 6 : return MAV_RESULT_ACCEPTED;
519 : }
520 :
521 0 : return MAV_RESULT_DENIED;
522 : }
523 :
524 4 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet)
525 : {
526 4 : if (sub.motors.armed() && sub.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
527 4 : return MAV_RESULT_ACCEPTED;
528 : }
529 0 : return MAV_RESULT_FAILED;
530 : }
531 :
532 0 : MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet)
533 : {
534 : // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
535 : // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
536 : // param3 : throttle (range depends upon param2)
537 : // param4 : timeout (in seconds)
538 0 : if (!sub.handle_do_motor_test(packet)) {
539 0 : return MAV_RESULT_FAILED;
540 : }
541 0 : return MAV_RESULT_ACCEPTED;
542 : }
543 :
544 : // this is called on receipt of a MANUAL_CONTROL packet and is
545 : // expected to call manual_override to override RC input on desired
546 : // axes.
547 0 : void GCS_MAVLINK_Sub::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
548 : {
549 0 : sub.transform_manual_control_to_rc_override(
550 0 : packet.x,
551 0 : packet.y,
552 0 : packet.z,
553 0 : packet.r,
554 0 : packet.buttons,
555 0 : packet.buttons2,
556 0 : packet.enabled_extensions,
557 0 : packet.s,
558 0 : packet.t,
559 0 : packet.aux1,
560 0 : packet.aux2,
561 0 : packet.aux3,
562 0 : packet.aux4,
563 0 : packet.aux5,
564 0 : packet.aux6
565 : );
566 :
567 0 : sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
568 0 : }
569 :
570 138056 : void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
571 : {
572 138056 : switch (msg.msgid) {
573 :
574 0 : case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // MAV ID: 70
575 0 : if (!gcs().sysid_is_gcs(msg.sysid)) {
576 0 : break; // Only accept control from our gcs
577 : }
578 :
579 0 : sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
580 : // a RC override message is considered to be a 'heartbeat'
581 : // from the ground station for failsafe purposes
582 :
583 0 : handle_rc_channels_override(msg);
584 0 : break;
585 : }
586 :
587 :
588 282 : case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: { // MAV ID: 82
589 : // decode packet
590 : mavlink_set_attitude_target_t packet;
591 282 : mavlink_msg_set_attitude_target_decode(&msg, &packet);
592 :
593 : // ensure type_mask specifies to use attitude
594 : // the thrust can be used from the altitude hold
595 282 : if (packet.type_mask & (1<<6)) {
596 282 : sub.set_attitude_target_no_gps = {AP_HAL::millis(), packet};
597 : }
598 :
599 : // ensure type_mask specifies to use attitude and thrust
600 282 : if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
601 282 : break;
602 : }
603 :
604 : // convert thrust to climb rate
605 0 : packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
606 0 : float climb_rate_cms = 0.0f;
607 0 : if (is_equal(packet.thrust, 0.5f)) {
608 0 : climb_rate_cms = 0.0f;
609 0 : } else if (packet.thrust > 0.5f) {
610 : // climb at up to WP_SPD_UP
611 0 : climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up_cms();
612 : } else {
613 : // descend at up to WP_SPD_DN
614 0 : climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_down_cms();
615 : }
616 0 : sub.mode_guided.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
617 0 : break;
618 : }
619 :
620 2 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { // MAV ID: 84
621 : // decode packet
622 : mavlink_set_position_target_local_ned_t packet;
623 2 : mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
624 :
625 : // exit if vehicle is not in Guided mode or Auto-Guided mode
626 2 : if ((sub.control_mode != Mode::Number::GUIDED) && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)) {
627 : break;
628 : }
629 :
630 : // check for supported coordinate frames
631 2 : if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
632 0 : packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
633 0 : packet.coordinate_frame != MAV_FRAME_BODY_NED &&
634 0 : packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED &&
635 0 : packet.coordinate_frame != MAV_FRAME_BODY_FRD) {
636 0 : break;
637 : }
638 :
639 2 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
640 2 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
641 2 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
642 2 : bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
643 2 : bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
644 :
645 : // prepare position
646 2 : Vector3f pos_vector_neu_cm;
647 2 : if (!pos_ignore) {
648 : // convert to cm
649 2 : pos_vector_neu_cm = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
650 : // rotate from body-frame if necessary
651 2 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
652 2 : packet.coordinate_frame == MAV_FRAME_BODY_FRD ||
653 2 : packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
654 0 : sub.rotate_body_frame_to_NE(pos_vector_neu_cm.x, pos_vector_neu_cm.y);
655 : }
656 : // add body offset if necessary
657 2 : if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
658 2 : packet.coordinate_frame == MAV_FRAME_BODY_NED ||
659 2 : packet.coordinate_frame == MAV_FRAME_BODY_FRD ||
660 2 : packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
661 0 : Vector3f pos_neu_cm = (sub.pos_control.get_pos_estimate_NED_m() * 100.0f).tofloat();
662 0 : pos_neu_cm.z = -pos_neu_cm.z;
663 0 : pos_vector_neu_cm += pos_neu_cm;
664 : }
665 : }
666 :
667 : // prepare velocity
668 2 : Vector3f vel_vector_neu_cms;
669 2 : if (!vel_ignore) {
670 : // convert to cm
671 2 : vel_vector_neu_cms = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
672 : // rotate from body-frame if necessary
673 2 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
674 0 : sub.rotate_body_frame_to_NE(vel_vector_neu_cms.x, vel_vector_neu_cms.y);
675 : }
676 : }
677 :
678 : // prepare acceleration
679 2 : Vector3f acc_vector_neu_cmss;
680 2 : if (!acc_ignore) {
681 : // convert to cm/s/s
682 2 : acc_vector_neu_cmss = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
683 : // rotate from body-frame if necessary
684 2 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
685 0 : sub.rotate_body_frame_to_NE(acc_vector_neu_cmss.x, acc_vector_neu_cmss.y);
686 : }
687 : }
688 :
689 : // prepare yaw
690 2 : float yaw_cd = 0.0f;
691 2 : bool yaw_relative = false;
692 2 : float yaw_rate_cds = 0.0f;
693 2 : if (!yaw_ignore) {
694 0 : yaw_cd = degrees(packet.yaw) * 100.0f;
695 0 : yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
696 : }
697 2 : if (!yaw_rate_ignore) {
698 0 : yaw_rate_cds = degrees(packet.yaw_rate) * 100.0f;
699 : }
700 :
701 : // send request
702 2 : if (!pos_ignore && !vel_ignore) {
703 2 : sub.mode_guided.guided_set_posvelaccel(pos_vector_neu_cm, vel_vector_neu_cms, acc_vector_neu_cmss, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
704 0 : } else if (pos_ignore && !vel_ignore && acc_ignore) {
705 0 : sub.mode_guided.guided_set_velocity(vel_vector_neu_cms, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
706 0 : } else if (!pos_ignore && vel_ignore && acc_ignore) {
707 0 : sub.mode_guided.guided_set_destination(pos_vector_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
708 : }
709 :
710 2 : break;
711 : }
712 :
713 3 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: { // MAV ID: 86
714 : // decode packet
715 : mavlink_set_position_target_global_int_t packet;
716 3 : mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
717 :
718 : // exit if vehicle is not in Guided, Auto-Guided, or Depth Hold modes
719 3 : if ((sub.control_mode != Mode::Number::GUIDED)
720 0 : && !(sub.control_mode == Mode::Number::AUTO && sub.auto_mode == Auto_NavGuided)
721 0 : && !(sub.control_mode == Mode::Number::ALT_HOLD)) {
722 0 : break;
723 : }
724 :
725 3 : bool z_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_Z_IGNORE;
726 3 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
727 3 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
728 3 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
729 :
730 : /*
731 : * for future use:
732 : * bool force = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
733 : * bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
734 : * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
735 : */
736 :
737 3 : if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
738 0 : sub.pos_control.set_pos_desired_U_cm(packet.alt*100);
739 0 : break;
740 : }
741 :
742 3 : Location loc;
743 3 : if (!pos_ignore) {
744 : // sanity check location
745 3 : if (!check_latlng(packet.lat_int, packet.lon_int)) {
746 0 : break;
747 : }
748 : Location::AltFrame frame;
749 3 : if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
750 : // unknown coordinate frame
751 0 : break;
752 : }
753 3 : loc = {
754 : packet.lat_int,
755 : packet.lon_int,
756 3 : int32_t(packet.alt*100),
757 : frame,
758 : };
759 : }
760 :
761 3 : Vector3f acc_vector_neu_cmss;
762 3 : if (!acc_ignore) {
763 0 : acc_vector_neu_cmss = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
764 : }
765 :
766 3 : if (!pos_ignore && !vel_ignore) {
767 0 : Vector3f pos_neu_cm;
768 0 : if (!loc.get_vector_from_origin_NEU_cm(pos_neu_cm)) {
769 0 : break;
770 : }
771 0 : sub.mode_guided.guided_set_posvelaccel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), acc_vector_neu_cmss);
772 3 : } else if (pos_ignore && !vel_ignore && acc_ignore) {
773 0 : sub.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
774 3 : } else if (!pos_ignore && vel_ignore && acc_ignore) {
775 3 : sub.mode_guided.guided_set_destination(loc);
776 : }
777 :
778 3 : break;
779 : }
780 :
781 : // Remote leak sensor support (e.g. in a separate enclosure), via MAVLink status messages.
782 0 : case MAVLINK_MSG_ID_SYS_STATUS: {
783 : mavlink_sys_status_t packet;
784 0 : mavlink_msg_sys_status_decode(&msg, &packet);
785 0 : if ((msg.sysid == gcs().sysid_this_mav()) &&
786 0 : (packet.onboard_control_sensors_present & MAV_SYS_STATUS_EXTENSION_USED) &&
787 0 : (packet.onboard_control_sensors_enabled_extended & MAV_SYS_STATUS_SENSOR_LEAK) &&
788 0 : !(packet.onboard_control_sensors_health_extended & MAV_SYS_STATUS_SENSOR_LEAK)) {
789 0 : sub.leak_detector.set_detect();
790 : }
791 0 : break;
792 : }
793 :
794 137769 : default:
795 137769 : GCS_MAVLINK::handle_message(msg);
796 137700 : break;
797 : } // end switch
798 137987 : } // end handle mavlink
799 :
800 3 : uint64_t GCS_MAVLINK_Sub::capabilities() const
801 : {
802 : return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
803 : MAV_PROTOCOL_CAPABILITY_MISSION_INT |
804 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
805 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
806 : MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
807 : #if AP_TERRAIN_AVAILABLE
808 3 : (sub.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
809 : #endif
810 : MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
811 3 : GCS_MAVLINK::capabilities()
812 3 : );
813 : }
814 :
815 0 : MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet)
816 : {
817 0 : if (packet.param1 > 0.5f) {
818 0 : sub.arming.disarm(AP_Arming::Method::TERMINATION);
819 0 : return MAV_RESULT_ACCEPTED;
820 : }
821 0 : return MAV_RESULT_FAILED;
822 : }
823 :
824 81589 : int32_t GCS_MAVLINK_Sub::global_position_int_alt() const
825 : {
826 81589 : return static_cast<int32_t>(sub.get_alt_msl() * 1000.0f);
827 : }
828 :
829 81587 : int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const
830 : {
831 81587 : return static_cast<int32_t>(sub.get_alt_rel() * 1000.0f);
832 : }
833 :
834 : #if HAL_HIGH_LATENCY2_ENABLED
835 0 : int16_t GCS_MAVLINK_Sub::high_latency_target_altitude() const
836 : {
837 0 : AP_AHRS &ahrs = AP::ahrs();
838 0 : Location global_position_current;
839 0 : UNUSED_RESULT(ahrs.get_location(global_position_current));
840 :
841 : //return units are m
842 0 : if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
843 0 : return 0.01 * (global_position_current.alt + sub.pos_control.get_pos_error_U_cm());
844 : }
845 0 : return 0;
846 :
847 : }
848 :
849 0 : uint8_t GCS_MAVLINK_Sub::high_latency_tgt_heading() const
850 : {
851 : // return units are deg/2
852 0 : if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
853 : // need to convert -18000->18000 to 0->360/2
854 0 : return wrap_360_cd(sub.wp_nav.get_wp_bearing_to_destination_cd()) / 200;
855 : }
856 0 : return 0;
857 : }
858 :
859 0 : uint16_t GCS_MAVLINK_Sub::high_latency_tgt_dist_dam() const
860 : {
861 0 : if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
862 0 : return MIN(static_cast<uint16_t>(sub.wp_nav.get_wp_distance_to_destination_cm() * 1e-3), UINT16_MAX);
863 : }
864 0 : return 0;
865 : }
866 :
867 0 : uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const
868 : {
869 : // return units are m/s*5
870 0 : if (sub.control_mode == Mode::Number::AUTO || sub.control_mode == Mode::Number::GUIDED) {
871 0 : return MIN((sub.pos_control.get_vel_desired_NEU_cms().length()/100) * 5, UINT8_MAX);
872 : }
873 0 : return 0;
874 : }
875 : #endif // HAL_HIGH_LATENCY2_ENABLED
876 :
877 : // Send the mode with the given index (not mode number!) return the total number of modes
878 : // Index starts at 1
879 0 : uint8_t GCS_MAVLINK_Sub::send_available_mode(uint8_t index) const
880 : {
881 0 : const Mode* modes[] {
882 : &sub.mode_manual,
883 : &sub.mode_stabilize,
884 : &sub.mode_acro,
885 : &sub.mode_althold,
886 : &sub.mode_surftrak,
887 : &sub.mode_poshold,
888 : &sub.mode_auto,
889 : &sub.mode_guided,
890 : &sub.mode_circle,
891 : &sub.mode_surface,
892 : &sub.mode_motordetect,
893 : };
894 :
895 0 : const uint8_t mode_count = ARRAY_SIZE(modes);
896 :
897 : // Convert to zero indexed
898 0 : const uint8_t index_zero = index - 1;
899 0 : if (index_zero >= mode_count) {
900 : // Mode does not exist!?
901 0 : return mode_count;
902 : }
903 :
904 : // Ask the mode for its name and number
905 0 : const char* name = modes[index_zero]->name();
906 0 : const uint8_t mode_number = (uint8_t)modes[index_zero]->number();
907 :
908 0 : mavlink_msg_available_modes_send(
909 0 : chan,
910 : mode_count,
911 : index,
912 : MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
913 : mode_number,
914 : 0, // MAV_MODE_PROPERTY bitmask
915 : name
916 : );
917 :
918 0 : return mode_count;
919 : }
|
