Line data Source code
1 : /*
2 : Common GCS MAVLink functions for all vehicle types
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : #include "GCS_config.h"
19 :
20 : #if HAL_GCS_ENABLED
21 :
22 : #include "GCS.h"
23 :
24 : #include <AC_Fence/AC_Fence.h>
25 : #include <AP_Compass/AP_Compass.h>
26 : #include <AP_ADSB/AP_ADSB.h>
27 : #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
28 : #include <AP_AHRS/AP_AHRS.h>
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <AP_Arming/AP_Arming.h>
31 : #include <AP_InternalError/AP_InternalError.h>
32 : #include <AP_Logger/AP_Logger.h>
33 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
34 : #include <AP_Vehicle/AP_Vehicle.h>
35 : #include <AP_RangeFinder/AP_RangeFinder.h>
36 : #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
37 : #include <AP_Airspeed/AP_Airspeed.h>
38 : #include <AP_Camera/AP_Camera.h>
39 : #include <AP_Gripper/AP_Gripper.h>
40 : #include <AC_Sprayer/AC_Sprayer.h>
41 : #include <AP_BLHeli/AP_BLHeli.h>
42 : #include <AP_Relay/AP_Relay.h>
43 : #include <AP_RSSI/AP_RSSI.h>
44 : #include <AP_RTC/AP_RTC.h>
45 : #include <AP_Scheduler/AP_Scheduler.h>
46 : #include <AP_SerialManager/AP_SerialManager.h>
47 : #include <AP_RCTelemetry/AP_Spektrum_Telem.h>
48 : #include <AP_Mount/AP_Mount.h>
49 : #include <AP_Common/AP_FWVersion.h>
50 : #include <AP_VisualOdom/AP_VisualOdom.h>
51 : #include <AP_Baro/AP_Baro.h>
52 : #include <AP_EFI/AP_EFI.h>
53 : #include <AP_Proximity/AP_Proximity.h>
54 : #include <AP_Scripting/AP_Scripting.h>
55 : #include <SRV_Channel/SRV_Channel.h>
56 : #include <AP_Terrain/AP_Terrain.h>
57 : #include <AP_Winch/AP_Winch.h>
58 : #include <AP_Mission/AP_Mission.h>
59 : #include <AP_OpenDroneID/AP_OpenDroneID.h>
60 : #include <AP_OSD/AP_OSD.h>
61 : #include <AP_RCTelemetry/AP_CRSF_Telem.h>
62 : #include <AP_RPM/AP_RPM.h>
63 : #include <AP_AIS/AP_AIS.h>
64 : #include <AP_Filesystem/AP_Filesystem.h>
65 : #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
66 : #include <RC_Channel/RC_Channel.h>
67 : #include <AP_VisualOdom/AP_VisualOdom.h>
68 : #include <AP_LandingGear/AP_LandingGear.h>
69 : #include <AP_Landing/AP_Landing_config.h>
70 : #include <AP_Generator/AP_Generator_Loweheiser.h>
71 :
72 : #include "MissionItemProtocol_Waypoints.h"
73 : #include "MissionItemProtocol_Rally.h"
74 : #include "MissionItemProtocol_Fence.h"
75 :
76 : #include <AP_CANManager/AP_MAVLinkCAN.h>
77 :
78 : #include <AP_Notify/AP_Notify.h>
79 : #include <AP_Vehicle/AP_Vehicle_config.h>
80 :
81 : #include <stdio.h>
82 :
83 : #if AP_RADIO_ENABLED
84 : #include <AP_Radio/AP_Radio.h>
85 : #include <AP_BoardConfig/AP_BoardConfig.h>
86 : #endif
87 :
88 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
89 : #include <SITL/SITL.h>
90 : #endif
91 :
92 : #if HAL_MAX_CAN_PROTOCOL_DRIVERS
93 : #include <AP_CANManager/AP_CANManager.h>
94 : #include <AP_Common/AP_Common.h>
95 :
96 : #include <AP_PiccoloCAN/AP_PiccoloCAN.h>
97 : #include <AP_DroneCAN/AP_DroneCAN.h>
98 : #endif
99 :
100 : #include <AP_BattMonitor/AP_BattMonitor_config.h>
101 : #if AP_BATTERY_ENABLED
102 : #include <AP_BattMonitor/AP_BattMonitor.h>
103 : #endif
104 : #include <AP_GPS/AP_GPS.h>
105 :
106 : #include <AP_RCProtocol/AP_RCProtocol_config.h>
107 : #if AP_RCPROTOCOL_ENABLED
108 : #include <AP_RCProtocol/AP_RCProtocol.h>
109 : #endif
110 :
111 : #if HAL_WITH_IO_MCU
112 : #include <AP_IOMCU/AP_IOMCU.h>
113 : extern AP_IOMCU iomcu;
114 : #endif
115 :
116 : #include "GCS_FTP.h"
117 :
118 : #include <ctype.h>
119 :
120 : extern const AP_HAL::HAL& hal;
121 :
122 : struct GCS_MAVLINK::LastRadioStatus GCS_MAVLINK::last_radio_status;
123 : mavlink_channel_mask_t GCS_MAVLINK::mavlink_active = 0;
124 : mavlink_channel_mask_t GCS_MAVLINK::chan_is_streaming = 0;
125 : uint32_t GCS_MAVLINK::reserve_param_space_start_ms;
126 :
127 : // private channels are ones used for point-to-point protocols, and
128 : // don't get broadcasts or fwded packets
129 : mavlink_channel_mask_t GCS_MAVLINK::mavlink_private = 0;
130 :
131 : GCS *GCS::_singleton = nullptr;
132 :
133 : GCS_MAVLINK_InProgress GCS_MAVLINK_InProgress::in_progress_tasks[1];
134 : uint32_t GCS_MAVLINK_InProgress::last_check_ms;
135 :
136 133140 : GCS_MAVLINK::GCS_MAVLINK(AP_HAL::UARTDriver &uart)
137 : {
138 6340 : AP_Param::setup_object_defaults(this, var_info);
139 :
140 6340 : _port = &uart;
141 6340 : }
142 :
143 6340 : bool GCS_MAVLINK::init(uint8_t instance)
144 : {
145 : // get associated mavlink channel
146 6340 : chan = (mavlink_channel_t)(MAVLINK_COMM_0 + instance);
147 6340 : if (!valid_channel(chan)) {
148 0 : return false;
149 : }
150 :
151 : // find instance of MAVLink protocol; the protocol_match method in
152 : // AP_SerialManager means this will match MAVLink2 and MAVLinkHL,
153 : // too:
154 6340 : uartstate = AP::serialmanager().find_protocol_instance(AP_SerialManager::SerialProtocol_MAVLink, instance);
155 6340 : if (uartstate == nullptr) {
156 0 : return false;
157 : }
158 :
159 : // fill in the device ID (a parameter which allows a user to
160 : // understand what their MAVn_ parameters actually correspond to)
161 6340 : devid.set(uartstate->get_device_id());
162 :
163 : // PARAMETER_CONVERSION - Added: May-2025 for ArduPilot-4.7
164 : // convert parameters; we used to use bits in the UARTDriver to
165 : // remember whether the mavlink connection on that interface was
166 : // "private" or not, and whether to ignore streamrate sets via
167 : // REQUEST_DATA_STREAM. We moved that into the MAVn_OPTIONS, this
168 : // is the conversion:
169 6340 : if (!options_were_converted) {
170 919 : auto &sm = AP::serialmanager();
171 919 : options_were_converted.set_and_save(1);
172 919 : if (_port->option_is_set(AP_HAL::UARTDriver::Option::OPTION_MAVLINK_NO_FORWARD_old)) {
173 0 : enable_option(Option::NO_FORWARD);
174 : // turn the bit off in the UART options. We can only get
175 : // away with this because we are (in a very nearby commit)
176 : // changing the width of the SERIALn_OPTIONS. This means
177 : // older firmwares will still see the SERIALn_OPTIONS bits
178 : // as set, newer firmwares will see zeroes. We have a
179 : // prearm check that users have not set the old bit.
180 : // Sorry.
181 0 : sm.disable_option(uartstate->idx, AP_HAL::UARTDriver::Option::OPTION_MAVLINK_NO_FORWARD_old);
182 : }
183 919 : if (_port->option_is_set(AP_HAL::UARTDriver::Option::OPTION_NOSTREAMOVERRIDE_old)) {
184 0 : enable_option(Option::NOSTREAMOVERRIDE);
185 : // turn the bit off in the UART options. We can only get
186 : // away with this because we are (in a very nearby commit)
187 : // changing the width of the SERIALn_OPTIONS. This means
188 : // older firmwares will still see the SERIALn_OPTIONS bits
189 : // as set, newer firmwares will see zeroes. We have a
190 : // prearm check that users have not set the old bit.
191 : // Sorry.
192 0 : sm.disable_option(uartstate->idx, AP_HAL::UARTDriver::Option::OPTION_NOSTREAMOVERRIDE_old);
193 : }
194 : }
195 :
196 : // and init the gcs instance
197 :
198 : // whether this port is considered "private":
199 6340 : if (option_enabled(Option::NO_FORWARD)) {
200 11 : set_channel_private(chan);
201 : }
202 :
203 : /*
204 : Now try to cope with SiK radios that may be stuck in bootloader
205 : mode because CTS was held while powering on. This tells the
206 : bootloader to wait for a firmware. It affects any SiK radio with
207 : CTS connected that is externally powered. To cope we send 0x30
208 : 0x20 at 115200 on startup, which tells the bootloader to reset
209 : and boot normally
210 : */
211 6340 : _port->begin(115200);
212 6340 : AP_HAL::UARTDriver::flow_control old_flow_control = _port->get_flow_control();
213 6340 : _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
214 25360 : for (uint8_t i=0; i<3; i++) {
215 19020 : hal.scheduler->delay(1);
216 19020 : _port->write(0x30);
217 19020 : _port->write(0x20);
218 : }
219 : // since tcdrain() and TCSADRAIN may not be implemented...
220 6340 : hal.scheduler->delay(1);
221 :
222 6340 : _port->set_flow_control(old_flow_control);
223 :
224 : // now change back to desired baudrate
225 6340 : _port->begin(uartstate->baudrate());
226 :
227 6340 : mavlink_comm_port[chan] = _port;
228 :
229 6340 : const auto mavlink_protocol = uartstate->get_protocol();
230 :
231 6340 : if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 ||
232 : mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
233 : #if AP_MAVLINK_SIGNING_ENABLED
234 : // load signing key
235 6331 : load_signing_key();
236 : #endif // AP_MAVLINK_SIGNING_ENABLED
237 : } else {
238 : // user has asked to only send MAVLink1
239 9 : _channel_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
240 : }
241 :
242 : #if HAL_HIGH_LATENCY2_ENABLED
243 6340 : if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
244 2 : is_high_latency_link = true;
245 : }
246 : #endif
247 6340 : return true;
248 : }
249 :
250 1117088 : void GCS_MAVLINK::send_meminfo(void)
251 : {
252 1117088 : unsigned __brkval = 0;
253 1117088 : uint32_t memory = hal.util->available_memory();
254 1117088 : mavlink_msg_meminfo_send(chan, __brkval, MIN(memory, 0xFFFFU), memory);
255 1117088 : }
256 :
257 : // report power supply status
258 1117088 : void GCS_MAVLINK::send_power_status(void)
259 : {
260 1117088 : if (!gcs().vehicle_initialised()) {
261 : // avoid unnecessary errors being reported to user
262 243 : return;
263 : }
264 3350535 : mavlink_msg_power_status_send(chan,
265 1116845 : hal.analogin->board_voltage() * 1000,
266 1116845 : hal.analogin->servorail_voltage() * 1000,
267 1116845 : hal.analogin->power_status_flags());
268 : }
269 :
270 : #if AP_SCHEDULER_ENABLED
271 : // cap the MAVLink message rate. It can't be greater than 0.8 * SCHED_LOOP_RATE
272 441903 : uint16_t GCS_MAVLINK::cap_message_interval(uint16_t interval_ms) const
273 : {
274 441903 : if (interval_ms == 0) {
275 154942 : return 0;
276 : }
277 286961 : if (interval_ms*800 < AP::scheduler().get_loop_period_us()) {
278 1 : return AP::scheduler().get_loop_period_us()/800.0f;
279 : }
280 286960 : return interval_ms;
281 : }
282 : #endif
283 :
284 : #if HAL_WITH_MCU_MONITORING
285 : // report MCU voltage/temperature status
286 : void GCS_MAVLINK::send_mcu_status(void)
287 : {
288 : if (!gcs().vehicle_initialised()) {
289 : // avoid unnecessary errors being reported to user
290 : return;
291 : }
292 : mavlink_msg_mcu_status_send(chan,
293 : 0, // only one MCU
294 : hal.analogin->mcu_temperature() * 100,
295 : hal.analogin->mcu_voltage() * 1000,
296 : hal.analogin->mcu_voltage_min() * 1000,
297 : hal.analogin->mcu_voltage_max() * 1000);
298 : }
299 : #endif
300 :
301 : #if AP_BATTERY_ENABLED
302 : // returns the battery remaining percentage if valid, -1 otherwise
303 1986617 : int8_t GCS_MAVLINK::battery_remaining_pct(const uint8_t instance) const {
304 : uint8_t percentage;
305 1986617 : return AP::battery().capacity_remaining_pct(percentage, instance) ? MIN(percentage, INT8_MAX) : -1;
306 : }
307 :
308 868214 : void GCS_MAVLINK::send_battery_status(const uint8_t instance) const
309 : {
310 : // catch the battery backend not supporting the required number of cells
311 : static_assert(sizeof(AP_BattMonitor::cells) >= (sizeof(uint16_t) * MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN),
312 : "Not enough battery cells for the MAVLink message");
313 :
314 868214 : const AP_BattMonitor &battery = AP::battery();
315 : float temp;
316 868214 : bool got_temperature = battery.get_temperature(temp, instance);
317 :
318 : // prepare arrays of individual cell voltages
319 : uint16_t cell_mvolts[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN];
320 : uint16_t cell_mvolts_ext[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN];
321 868214 : const uint16_t max_cell_mV = 0xFFFEU;
322 868214 : const uint16_t invalid_cell_mV = 0xFFFFU;
323 :
324 868214 : if (battery.has_cell_voltages(instance)) {
325 302 : const AP_BattMonitor::cells& batt_cells = battery.get_cell_voltages(instance);
326 : static_assert(sizeof(cell_mvolts) <= sizeof(batt_cells.cells), "cell array length not large enough");
327 :
328 : // copy the first 10 cells
329 302 : memcpy(cell_mvolts, batt_cells.cells, sizeof(cell_mvolts));
330 : // 11 ... 14 use a second cell_volts_ext array
331 1510 : for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
332 1208 : if (MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i < uint8_t(ARRAY_SIZE(batt_cells.cells))) {
333 1208 : cell_mvolts_ext[i] = batt_cells.cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN+i];
334 : } else {
335 0 : cell_mvolts_ext[i] = 0;
336 : }
337 : }
338 : /*
339 : now adjust voltages to cope with two things:
340 : 1) we may be reporting sag corrected voltage
341 : 2) the battery may have more cells than can be reported by the backend, so the actual voltage may be higher than the sum
342 : */
343 302 : const float voltage_mV = battery.gcs_voltage(instance) * 1e3f;
344 302 : float voltage_mV_sum = 0;
345 302 : uint8_t non_zero_cell_count = 0;
346 3322 : for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
347 3020 : if (cell_mvolts[i] > 0 && cell_mvolts[i] != invalid_cell_mV) {
348 2901 : non_zero_cell_count++;
349 2901 : voltage_mV_sum += cell_mvolts[i];
350 : }
351 : }
352 1510 : for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
353 1208 : if (cell_mvolts_ext[i] > 0 && cell_mvolts_ext[i] != invalid_cell_mV) {
354 1140 : non_zero_cell_count++;
355 1140 : voltage_mV_sum += cell_mvolts_ext[i];
356 : }
357 : }
358 302 : if (voltage_mV > voltage_mV_sum && non_zero_cell_count > 0) {
359 : // distribute the extra voltage over the non-zero cells
360 283 : uint32_t extra_mV = (voltage_mV - voltage_mV_sum) / non_zero_cell_count;
361 3113 : for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
362 2830 : if (cell_mvolts[i] > 0 && cell_mvolts[i] != invalid_cell_mV) {
363 2830 : cell_mvolts[i] = MIN(cell_mvolts[i] + extra_mV, max_cell_mV);
364 : }
365 : }
366 1415 : for (uint8_t i=0; i<MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
367 1132 : if (cell_mvolts_ext[i] > 0 && cell_mvolts_ext[i] != invalid_cell_mV) {
368 1132 : cell_mvolts_ext[i] = MIN(cell_mvolts_ext[i] + extra_mV, max_cell_mV);
369 : }
370 : }
371 : }
372 : } else {
373 : // for battery monitors that cannot provide voltages for individual cells the battery's total voltage is put into the first cell
374 : // if the total voltage cannot fit into a single field, the remainder into subsequent fields.
375 : // the GCS can then recover the pack voltage by summing all non ignored cell values an we can report a pack up to 655.34 V
376 867912 : float voltage_mV = battery.gcs_voltage(instance) * 1e3f;
377 9547032 : for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) {
378 8679120 : if (voltage_mV < 0.001f) {
379 : // too small to send to the GCS, set it to the no cell value
380 7812677 : cell_mvolts[i] = UINT16_MAX;
381 : } else {
382 866443 : cell_mvolts[i] = MIN(voltage_mV, max_cell_mV); // Can't send more then UINT16_MAX - 1 in a cell
383 866443 : voltage_mV -= max_cell_mV;
384 : }
385 : }
386 4339560 : for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; i++) {
387 3471648 : cell_mvolts_ext[i] = 0;
388 : }
389 : }
390 :
391 : float current, consumed_mah, consumed_wh;
392 868214 : const int8_t percentage = battery_remaining_pct(instance);
393 :
394 868214 : if (battery.current_amps(current, instance)) {
395 868214 : current = constrain_float(current * 100,-INT16_MAX,INT16_MAX);
396 : } else {
397 0 : current = -1;
398 : }
399 868214 : if (!battery.consumed_mah(consumed_mah, instance)) {
400 0 : consumed_mah = -1;
401 : }
402 868214 : if (battery.consumed_wh(consumed_wh, instance)) {
403 867639 : consumed_wh *= 36;
404 : } else {
405 575 : consumed_wh = -1;
406 : }
407 : uint32_t time_remaining;
408 868214 : if (!battery.time_remaining(time_remaining, instance)) {
409 868214 : time_remaining = 0;
410 : }
411 :
412 1736730 : mavlink_msg_battery_status_send(chan,
413 : instance, // id
414 : MAV_BATTERY_FUNCTION_UNKNOWN, // function
415 : MAV_BATTERY_TYPE_UNKNOWN, // type
416 302 : got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown
417 : cell_mvolts, // cell voltages
418 : current, // current in centiampere
419 : consumed_mah, // total consumed current in milliampere.hour
420 : consumed_wh, // consumed energy in hJ (hecto-Joules)
421 868214 : constrain_int16(percentage, -1, 100),
422 : time_remaining, // time remaining, seconds
423 868214 : battery.get_mavlink_charge_state(instance), // battery charge state
424 : cell_mvolts_ext, // Cell 11..14 voltages
425 : 0, // battery mode
426 : battery.get_mavlink_fault_bitmask(instance)); // fault_bitmask
427 868214 : }
428 :
429 : // returns true if all battery instances were reported
430 1117755 : bool GCS_MAVLINK::send_battery_status()
431 : {
432 1117755 : const AP_BattMonitor &battery = AP::battery();
433 :
434 10274545 : for(uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) {
435 10025004 : const uint8_t battery_id = (last_battery_status_idx + 1) % AP_BATT_MONITOR_MAX_INSTANCES;
436 10025004 : const auto configured_type = battery.configured_type(battery_id);
437 868844 : if (configured_type != AP_BattMonitor::Type::NONE &&
438 10893848 : configured_type == battery.allocated_type(battery_id) &&
439 868230 : !battery.option_is_set(battery_id, AP_BattMonitor_Params::Options::InternalUseOnly)) {
440 868214 : CHECK_PAYLOAD_SIZE(BATTERY_STATUS);
441 868214 : send_battery_status(battery_id);
442 868214 : last_battery_status_idx = battery_id;
443 868214 : return true;
444 : } else {
445 9156790 : last_battery_status_idx = battery_id;
446 : }
447 : }
448 249541 : return true;
449 : }
450 : #endif // AP_BATTERY_ENABLED
451 :
452 : #if AP_RANGEFINDER_ENABLED
453 107705 : void GCS_MAVLINK::send_distance_sensor(const AP_RangeFinder_Backend *sensor, const uint8_t instance) const
454 : {
455 107705 : if (!sensor->has_data()) {
456 1760 : return;
457 : }
458 :
459 105945 : int8_t quality_pct = sensor->signal_quality_pct();
460 : // ardupilot defines this field as -1 is unknown, 0 is poor, 100 is excellent
461 : // mavlink defines this field as 0 is unknown, 1 is invalid, 100 is perfect
462 : uint8_t quality;
463 105945 : if (quality_pct == RangeFinder::SIGNAL_QUALITY_UNKNOWN) {
464 94000 : quality = 0;
465 11945 : } else if (quality_pct > 1 && quality_pct <= RangeFinder::SIGNAL_QUALITY_MAX) {
466 11113 : quality = quality_pct;
467 : } else {
468 832 : quality = 1;
469 : }
470 :
471 105945 : mavlink_msg_distance_sensor_send(
472 105945 : chan,
473 : AP_HAL::millis(), // time since system boot TODO: take time of measurement
474 105945 : MIN(sensor->min_distance() * 100, 65535),// minimum distance the sensor can measure in centimeters
475 105945 : MIN(sensor->max_distance() * 100, 65535),// maximum distance the sensor can measure in centimeters
476 211890 : MIN(sensor->distance() * 100, 65535), // current distance reading
477 105945 : sensor->get_mav_distance_sensor_type(), // type from MAV_DISTANCE_SENSOR enum
478 : instance, // onboard ID of the sensor == instance
479 105945 : sensor->orientation(), // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
480 : 0, // Measurement covariance in centimeters, 0 for unknown / invalid readings
481 : 0, // horizontal FOV
482 : 0, // vertical FOV
483 : (const float *)nullptr, // quaternion of sensor orientation for MAV_SENSOR_ROTATION_CUSTOM
484 : quality); // Signal quality of the sensor. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal.
485 : }
486 : #endif // AP_RANGEFINDER_ENABLED
487 :
488 : // send any and all distance_sensor messages. This starts by sending
489 : // any distance sensors not used by a Proximity sensor, then sends the
490 : // proximity sensor ones.
491 1117775 : void GCS_MAVLINK::send_distance_sensor()
492 : {
493 : #if AP_RANGEFINDER_ENABLED
494 1117775 : RangeFinder *rangefinder = RangeFinder::get_singleton();
495 1117775 : if (rangefinder == nullptr) {
496 0 : return;
497 : }
498 :
499 : // if we have a proximity backend that utilizes rangefinders cull
500 : // sending them here, and allow the later proximity code to manage
501 : // them
502 1117775 : bool filter_possible_proximity_sensors = false;
503 :
504 : #if HAL_PROXIMITY_ENABLED
505 1117775 : AP_Proximity *proximity = AP_Proximity::get_singleton();
506 1117775 : if (proximity != nullptr) {
507 568277 : for (uint8_t i = 0; i < proximity->num_sensors(); i++) {
508 : #if AP_PROXIMITY_RANGEFINDER_ENABLED
509 4669 : if (proximity->get_type(i) == AP_Proximity::Type::RangeFinder) {
510 0 : filter_possible_proximity_sensors = true;
511 : }
512 : #endif
513 : }
514 : }
515 : #endif
516 :
517 12295515 : for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
518 11177741 : if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
519 1 : return;
520 : }
521 11177740 : AP_RangeFinder_Backend *sensor = rangefinder->get_backend(i);
522 11177740 : if (sensor == nullptr) {
523 11070035 : continue;
524 : }
525 107705 : enum Rotation orient = sensor->orientation();
526 107705 : if (!filter_possible_proximity_sensors ||
527 0 : (orient > ROTATION_YAW_315 && orient != ROTATION_PITCH_90)) {
528 107705 : send_distance_sensor(sensor, i);
529 : }
530 : }
531 : #endif // AP_RANGEFINDER_ENABLED
532 :
533 : #if HAL_PROXIMITY_ENABLED
534 1117774 : send_proximity();
535 : #endif
536 : }
537 :
538 : #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
539 4398 : void GCS_MAVLINK::send_rangefinder() const
540 : {
541 4398 : RangeFinder *rangefinder = RangeFinder::get_singleton();
542 4398 : if (rangefinder == nullptr) {
543 0 : return;
544 : }
545 4398 : AP_RangeFinder_Backend *s = rangefinder->find_instance(ROTATION_PITCH_270);
546 4398 : if (s == nullptr) {
547 0 : return;
548 : }
549 8796 : mavlink_msg_rangefinder_send(
550 4398 : chan,
551 : s->distance(),
552 4398 : s->voltage_mv() * 0.001f);
553 : }
554 : #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
555 :
556 : #if HAL_PROXIMITY_ENABLED
557 1117774 : void GCS_MAVLINK::send_proximity()
558 : {
559 1117774 : AP_Proximity *proximity = AP_Proximity::get_singleton();
560 1117774 : if (proximity == nullptr) {
561 554167 : return; // this is wrong, but pretend we sent data and don't requeue
562 : }
563 :
564 : // get min/max distances
565 563607 : const uint16_t dist_min_cm = (uint16_t)(proximity->distance_min_m() * 100.0f); // minimum distance the sensor can measure in centimeters
566 563607 : const uint16_t dist_max_cm = (uint16_t)(proximity->distance_max_m() * 100.0f); // maximum distance the sensor can measure in centimeters
567 :
568 : // send horizontal distances
569 563607 : if (proximity->get_status() == AP_Proximity::Status::Good) {
570 : Proximity_Distance_Array dist_array;
571 3364 : if (proximity->get_horizontal_distances(dist_array)) {
572 26451 : for (uint8_t i = 0; i < PROXIMITY_MAX_DIRECTION; i++) {
573 23512 : if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
574 0 : return;
575 : }
576 23512 : if (dist_array.valid(i)) {
577 19419 : proximity_ever_valid_bitmask |= (1U << i);
578 4093 : } else if (!(proximity_ever_valid_bitmask & (1U << i))) {
579 : // we've never sent this distance out, so we don't
580 : // need to send an invalid one.
581 1768 : continue;
582 : }
583 21744 : mavlink_msg_distance_sensor_send(
584 : chan,
585 : AP_HAL::millis(), // time since system boot
586 : dist_min_cm, // minimum distance the sensor can measure in centimeters
587 : dist_max_cm, // maximum distance the sensor can measure in centimeters
588 21744 : (uint16_t)(dist_array.distance[i] * 100.0f), // current distance reading
589 : MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum
590 21744 : PROXIMITY_SENSOR_ID_START + i, // onboard ID of the sensor
591 21744 : dist_array.orientation[i], // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
592 : 0, // Measurement covariance in centimeters, 0 for unknown / invalid readings
593 : 0, 0, nullptr, 0);
594 : }
595 : }
596 : }
597 :
598 : // send upward distance
599 : float dist_up_m;
600 563607 : if (proximity->get_upward_distance(dist_up_m)) {
601 1946 : if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
602 0 : return;
603 : }
604 1946 : mavlink_msg_distance_sensor_send(
605 : chan,
606 : AP_HAL::millis(), // time since system boot
607 : dist_min_cm, // minimum distance the sensor can measure in centimeters
608 : dist_max_cm, // maximum distance the sensor can measure in centimeters
609 1946 : (uint16_t)(dist_up_m * 100.0f), // current distance reading
610 : MAV_DISTANCE_SENSOR_LASER, // type from MAV_DISTANCE_SENSOR enum
611 : PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION + 1, // onboard ID of the sensor
612 : MAV_SENSOR_ROTATION_PITCH_90, // direction upwards
613 : 0, // Measurement covariance in centimeters, 0 for unknown / invalid readings
614 : 0, 0, nullptr, 0);
615 : }
616 : }
617 : #endif // HAL_PROXIMITY_ENABLED
618 :
619 : #if AP_AHRS_ENABLED
620 : // report AHRS2 state
621 1120989 : void GCS_MAVLINK::send_ahrs2()
622 : {
623 1120989 : const auto *estimates = AP::ahrs().get_secondary_estimates();
624 1120989 : if (estimates == nullptr) {
625 7939 : return;
626 : }
627 :
628 1113050 : const Location &loc = estimates->location;
629 :
630 1113050 : mavlink_msg_ahrs2_send(chan,
631 1113050 : estimates->roll_rad,
632 1113050 : estimates->pitch_rad,
633 1113050 : estimates->yaw_rad,
634 1113050 : loc.alt*1.0e-2f,
635 1113050 : loc.lat,
636 1113050 : loc.lng);
637 : }
638 : #endif // AP_AHRS_ENABLED
639 :
640 14896 : MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const
641 : {
642 14896 : if (mission_type >= ARRAY_SIZE(missionitemprotocols)) {
643 0 : return nullptr;
644 : }
645 14896 : return missionitemprotocols[mission_type];
646 : }
647 :
648 : // handle a request for the number of items we have stored for a mission type:
649 467 : void GCS_MAVLINK::handle_mission_request_list(const mavlink_message_t &msg)
650 : {
651 : // decode
652 : mavlink_mission_request_list_t packet;
653 467 : mavlink_msg_mission_request_list_decode(&msg, &packet);
654 :
655 467 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
656 467 : if (prot == nullptr) {
657 0 : mavlink_msg_mission_ack_send(chan,
658 0 : msg.sysid,
659 0 : msg.compid,
660 : MAV_MISSION_UNSUPPORTED,
661 0 : packet.mission_type);
662 0 : return;
663 : }
664 :
665 467 : prot->handle_mission_request_list(*this, packet, msg);
666 : }
667 :
668 : /*
669 : handle a MISSION_REQUEST mavlink packet
670 : */
671 5582 : void GCS_MAVLINK::handle_mission_request_int(const mavlink_message_t &msg)
672 : {
673 : // decode
674 : mavlink_mission_request_int_t packet;
675 5582 : mavlink_msg_mission_request_int_decode(&msg, &packet);
676 :
677 5582 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
678 5582 : if (prot == nullptr) {
679 0 : return;
680 : }
681 5582 : prot->handle_mission_request_int(*this, packet, msg);
682 : }
683 :
684 : #if AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
685 0 : void GCS_MAVLINK::handle_mission_request(const mavlink_message_t &msg)
686 : {
687 : // decode
688 : mavlink_mission_request_t packet;
689 0 : mavlink_msg_mission_request_decode(&msg, &packet);
690 :
691 0 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
692 0 : if (prot == nullptr) {
693 0 : return;
694 : }
695 0 : prot->handle_mission_request(*this, packet, msg);
696 : }
697 : #endif
698 :
699 : // returns a MISSION_STATE numeration value best describing out
700 : // current mission state.
701 1115578 : MISSION_STATE GCS_MAVLINK::mission_state(const AP_Mission &mission) const
702 : {
703 1115578 : if (!mission.present()) {
704 728799 : return MISSION_STATE_NO_MISSION;
705 : }
706 386779 : switch (mission.state()) {
707 57769 : case AP_Mission::mission_state::MISSION_STOPPED:
708 57769 : return MISSION_STATE_NOT_STARTED;
709 322174 : case AP_Mission::mission_state::MISSION_RUNNING:
710 322174 : return MISSION_STATE_ACTIVE;
711 6836 : case AP_Mission::mission_state::MISSION_COMPLETE:
712 6836 : return MISSION_STATE_COMPLETE;
713 : }
714 :
715 : // compiler ensures we can't get here as no default case in above enumeration
716 :
717 0 : return MISSION_STATE_UNKNOWN;
718 : }
719 :
720 1115713 : void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t seq)
721 : {
722 1115713 : auto num_commands = mission.num_commands();
723 1115713 : if (num_commands > 0) {
724 : // exclude home location from the count; see message definition.
725 1031334 : num_commands -= 1;
726 : }
727 :
728 : #if AP_VEHICLE_ENABLED
729 1115713 : const uint8_t mission_mode = AP::vehicle()->current_mode_requires_mission() ? 1 : 0;
730 : #else
731 : const uint8_t mission_mode = 0;
732 : #endif
733 :
734 1115713 : mavlink_msg_mission_current_send(
735 : chan,
736 : seq,
737 : num_commands, // total
738 1115713 : mission_state(mission), // mission_state
739 : mission_mode); // mission_mode
740 1115713 : }
741 :
742 : #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
743 : /*
744 : handle a MISSION_SET_CURRENT mavlink packet
745 :
746 : Note that there exists a relatively new mavlink DO command,
747 : MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
748 : that the command has been received, rather than the GCS having to
749 : rely on getting back an identical sequence number as some currently
750 : do.
751 : */
752 2250 : void GCS_MAVLINK::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
753 : {
754 : // send_received_message_deprecation_warning("MISSION_SET_CURRENT");
755 :
756 : // decode
757 : mavlink_mission_set_current_t packet;
758 2250 : mavlink_msg_mission_set_current_decode(&msg, &packet);
759 :
760 : // set current command
761 2250 : if (mission.set_current_cmd(packet.seq)) {
762 : // because MISSION_SET_CURRENT is a message not a command,
763 : // there is not ACK associated with us successfully changing
764 : // our waypoint. Some GCSs use the fact we return exactly the
765 : // same mission sequence number in this packet as an ACK - so
766 : // if they send a MISSION_SET_CURRENT with seq number of 4
767 : // then they expect to receive a MISSION_CURRENT message with
768 : // exactly that sequence number in it, even if ArduPilot never
769 : // actually holds that as a sequence number (e.g. packet.seq==0).
770 375 : if (HAVE_PAYLOAD_SPACE(chan, MISSION_CURRENT)) {
771 375 : send_mission_current(mission, packet.seq);
772 : } else {
773 : // schedule it for later:
774 0 : send_message(MSG_CURRENT_WAYPOINT);
775 : }
776 : }
777 2250 : }
778 : #endif // AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
779 :
780 : /*
781 : handle a MISSION_COUNT mavlink packet
782 : */
783 3177 : void GCS_MAVLINK::handle_mission_count(const mavlink_message_t &msg)
784 : {
785 : // decode
786 : mavlink_mission_count_t packet;
787 3177 : mavlink_msg_mission_count_decode(&msg, &packet);
788 :
789 3177 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
790 3177 : if (prot == nullptr) {
791 0 : mavlink_msg_mission_ack_send(chan,
792 0 : msg.sysid,
793 0 : msg.compid,
794 : MAV_MISSION_UNSUPPORTED,
795 0 : packet.mission_type);
796 0 : return;
797 : }
798 :
799 3177 : prot->handle_mission_count(*this, packet, msg);
800 : }
801 :
802 : /*
803 : handle a MISSION_CLEAR_ALL mavlink packet
804 : */
805 4 : void GCS_MAVLINK::handle_mission_clear_all(const mavlink_message_t &msg)
806 : {
807 : // decode
808 : mavlink_mission_clear_all_t packet;
809 4 : mavlink_msg_mission_clear_all_decode(&msg, &packet);
810 :
811 4 : const MAV_MISSION_TYPE mission_type = (MAV_MISSION_TYPE)packet.mission_type;
812 4 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type(mission_type);
813 4 : if (prot == nullptr) {
814 0 : send_mission_ack(msg, mission_type, MAV_MISSION_UNSUPPORTED);
815 0 : return;
816 : }
817 :
818 4 : prot->handle_mission_clear_all(*this, msg);
819 : }
820 :
821 103052786 : bool GCS_MAVLINK::requesting_mission_items() const
822 : {
823 412158248 : for (const auto *prot : gcs().missionitemprotocols) {
824 309126289 : if (prot && prot->receiving && prot->active_link_is(this)) {
825 20827 : return true;
826 : }
827 : }
828 103031959 : return false;
829 : }
830 :
831 5 : void GCS_MAVLINK::handle_mission_write_partial_list(const mavlink_message_t &msg)
832 : {
833 : // decode
834 : mavlink_mission_write_partial_list_t packet;
835 5 : mavlink_msg_mission_write_partial_list_decode(&msg, &packet);
836 :
837 5 : MissionItemProtocol *use_prot = gcs().get_prot_for_mission_type((MAV_MISSION_TYPE)packet.mission_type);
838 5 : if (use_prot == nullptr) {
839 0 : send_mission_ack(msg, (MAV_MISSION_TYPE)packet.mission_type, MAV_MISSION_UNSUPPORTED);
840 0 : return;
841 : }
842 5 : use_prot->handle_mission_write_partial_list(*this, msg, packet);
843 : }
844 :
845 : #if HAL_MOUNT_ENABLED
846 : /*
847 : pass mavlink messages to the AP_Mount singleton
848 : */
849 23720 : void GCS_MAVLINK::handle_mount_message(const mavlink_message_t &msg)
850 : {
851 23720 : AP_Mount *mount = AP::mount();
852 23720 : if (mount == nullptr) {
853 0 : return;
854 : }
855 23720 : mount->handle_message(chan, msg);
856 : }
857 :
858 : #endif
859 :
860 : /*
861 : pass parameter value messages through to mount library
862 : */
863 26 : void GCS_MAVLINK::handle_param_value(const mavlink_message_t &msg)
864 : {
865 : #if HAL_MOUNT_ENABLED
866 26 : AP_Mount *mount = AP::mount();
867 26 : if (mount == nullptr) {
868 0 : return;
869 : }
870 26 : mount->handle_param_value(msg);
871 : #endif
872 : }
873 :
874 3812 : void GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *fmt, ...) const
875 : {
876 : va_list arg_list;
877 3812 : va_start(arg_list, fmt);
878 3812 : gcs().send_textv(severity, fmt, arg_list, (1<<chan));
879 3812 : va_end(arg_list);
880 3812 : }
881 :
882 0 : float GCS_MAVLINK::telemetry_radio_rssi()
883 : {
884 0 : if (AP_HAL::millis() - last_radio_status.received_ms > 5000) {
885 : // telemetry radio has disappeared?!
886 0 : return 0;
887 : }
888 0 : if (last_radio_status.rssi == 255) {
889 : // see RADIO_STATUS packet definition
890 0 : return 0;
891 : }
892 0 : return last_radio_status.rssi/254.0f;
893 : }
894 :
895 1429658 : bool GCS_MAVLINK::last_txbuf_is_greater(uint8_t txbuf_limit)
896 : {
897 1429658 : if (AP_HAL::millis() - last_radio_status.received_ms > 5000) {
898 : // stale report
899 1089019 : return true;
900 : }
901 340639 : return last_radio_status.txbuf > txbuf_limit;
902 : }
903 :
904 0 : void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg)
905 : {
906 : mavlink_radio_t packet;
907 0 : mavlink_msg_radio_decode(&msg, &packet);
908 :
909 0 : const uint32_t now = AP_HAL::millis();
910 :
911 0 : last_radio_status.received_ms = now;
912 0 : last_radio_status.rssi = packet.rssi;
913 :
914 : // record if the GCS has been receiving radio messages from
915 : // the aircraft
916 0 : if (packet.remrssi != 0) {
917 0 : last_radio_status.remrssi_ms = now;
918 : }
919 :
920 0 : last_radio_status.txbuf = packet.txbuf;
921 :
922 : // use the state of the transmit buffer in the radio to
923 : // control the stream rate, giving us adaptive software
924 : // flow control
925 0 : if (packet.txbuf < 20 && stream_slowdown_ms < 2000) {
926 : // we are very low on space - slow down a lot
927 0 : stream_slowdown_ms += 60;
928 0 : } else if (packet.txbuf < 50 && stream_slowdown_ms < 2000) {
929 : // we are a bit low on space, slow down slightly
930 0 : stream_slowdown_ms += 20;
931 0 : } else if (packet.txbuf > 95 && stream_slowdown_ms > 200) {
932 : // the buffer has plenty of space, speed up a lot
933 0 : stream_slowdown_ms -= 40;
934 0 : } else if (packet.txbuf > 90 && stream_slowdown_ms != 0) {
935 : // the buffer has enough space, speed up a bit
936 0 : if (stream_slowdown_ms > 20) {
937 0 : stream_slowdown_ms -= 20;
938 : } else {
939 0 : stream_slowdown_ms = 0;
940 : }
941 : }
942 :
943 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
944 : if (stream_slowdown_ms > max_slowdown_ms) {
945 : max_slowdown_ms = stream_slowdown_ms;
946 : }
947 : #endif
948 :
949 : #if HAL_LOGGING_ENABLED
950 : //log rssi, noise, etc if logging Performance monitoring data
951 0 : if (AP::logger().should_log(log_radio_bit())) {
952 0 : AP::logger().Write_Radio(packet);
953 : }
954 : #endif
955 0 : }
956 :
957 5247 : void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg)
958 : {
959 : mavlink_mission_item_int_t mission_item_int;
960 5247 : bool send_mission_item_warning = false;
961 5247 : if (msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) {
962 : mavlink_mission_item_t mission_item;
963 9 : mavlink_msg_mission_item_decode(&msg, &mission_item);
964 : #if AP_MISSION_ENABLED
965 9 : MAV_MISSION_RESULT ret = AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(mission_item, mission_item_int);
966 9 : if (ret != MAV_MISSION_ACCEPTED) {
967 0 : const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;
968 0 : send_mission_ack(msg, type, ret);
969 0 : return;
970 : }
971 : #else
972 : // No mission library, so we can't convert from MISSION_ITEM
973 : // to MISSION_ITEM_INT. Since we shouldn't be receiving fence
974 : // or rally items via MISSION_ITEM, we don't need to work hard
975 : // here, just fail:
976 : const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item.mission_type;
977 : send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED);
978 : return;
979 : #endif
980 9 : send_mission_item_warning = true;
981 : } else {
982 5238 : mavlink_msg_mission_item_int_decode(&msg, &mission_item_int);
983 : }
984 5247 : const MAV_MISSION_TYPE type = (MAV_MISSION_TYPE)mission_item_int.mission_type;
985 :
986 : #if AP_MISSION_ENABLED
987 5247 : const uint8_t current = mission_item_int.current;
988 :
989 5247 : if (type == MAV_MISSION_TYPE_MISSION && (current == 2 || current == 3)) {
990 8 : struct AP_Mission::Mission_Command cmd = {};
991 8 : MAV_MISSION_RESULT result = AP_Mission::mavlink_int_to_mission_cmd(mission_item_int, cmd);
992 8 : if (result != MAV_MISSION_ACCEPTED) {
993 : //decode failed
994 0 : send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
995 0 : return;
996 : }
997 : // guided or change-alt
998 8 : if (current == 2) {
999 : // current = 2 is a flag to tell us this is a "guided mode"
1000 : // waypoint and not for the mission
1001 7 : result = (handle_guided_request(cmd) ? MAV_MISSION_ACCEPTED
1002 : : MAV_MISSION_ERROR) ;
1003 1 : } else if (current == 3) {
1004 : //current = 3 is a flag to tell us this is a alt change only
1005 : // add home alt if needed
1006 1 : handle_change_alt_request(cmd.content.location);
1007 :
1008 : // verify we received the command
1009 1 : result = MAV_MISSION_ACCEPTED;
1010 : }
1011 8 : send_mission_ack(msg, MAV_MISSION_TYPE_MISSION, result);
1012 8 : return;
1013 : }
1014 : #endif
1015 :
1016 : // not a guided-mode reqest
1017 5239 : MissionItemProtocol *prot = gcs().get_prot_for_mission_type(type);
1018 5239 : if (prot == nullptr) {
1019 0 : send_mission_ack(msg, type, MAV_MISSION_UNSUPPORTED);
1020 0 : return;
1021 : }
1022 :
1023 5239 : if (send_mission_item_warning) {
1024 4 : prot->send_mission_item_warning();
1025 : }
1026 :
1027 5239 : if (!prot->receiving) {
1028 0 : send_mission_ack(msg, type, MAV_MISSION_ERROR);
1029 0 : return;
1030 : }
1031 :
1032 5239 : prot->handle_mission_item(msg, mission_item_int);
1033 : }
1034 :
1035 457146 : ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const
1036 : {
1037 : // MSG_NEXT_MISSION_REQUEST doesn't correspond to a mavlink message directly.
1038 : // It is used to request the next waypoint after receiving one.
1039 :
1040 : // MSG_NEXT_PARAM doesn't correspond to a mavlink message directly.
1041 : // It is used to send the next parameter in a stream after sending one
1042 :
1043 : // MSG_NAMED_FLOAT messages can't really be "streamed"...
1044 :
1045 : static const struct {
1046 : uint32_t mavlink_id;
1047 : ap_message msg_id;
1048 : } map[] {
1049 : { MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT},
1050 : { MAVLINK_MSG_ID_HOME_POSITION, MSG_HOME},
1051 : { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, MSG_ORIGIN},
1052 : { MAVLINK_MSG_ID_SYS_STATUS, MSG_SYS_STATUS},
1053 : { MAVLINK_MSG_ID_POWER_STATUS, MSG_POWER_STATUS},
1054 : #if HAL_WITH_MCU_MONITORING
1055 : { MAVLINK_MSG_ID_MCU_STATUS, MSG_MCU_STATUS},
1056 : #endif
1057 : { MAVLINK_MSG_ID_MEMINFO, MSG_MEMINFO},
1058 : { MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, MSG_NAV_CONTROLLER_OUTPUT},
1059 : { MAVLINK_MSG_ID_MISSION_CURRENT, MSG_CURRENT_WAYPOINT},
1060 : { MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, MSG_SERVO_OUTPUT_RAW},
1061 : { MAVLINK_MSG_ID_RC_CHANNELS, MSG_RC_CHANNELS},
1062 : #if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
1063 : { MAVLINK_MSG_ID_RC_CHANNELS_RAW, MSG_RC_CHANNELS_RAW},
1064 : #endif
1065 : { MAVLINK_MSG_ID_RAW_IMU, MSG_RAW_IMU},
1066 : { MAVLINK_MSG_ID_SCALED_IMU, MSG_SCALED_IMU},
1067 : { MAVLINK_MSG_ID_SCALED_IMU2, MSG_SCALED_IMU2},
1068 : { MAVLINK_MSG_ID_SCALED_IMU3, MSG_SCALED_IMU3},
1069 : #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
1070 : { MAVLINK_MSG_ID_HIGHRES_IMU, MSG_HIGHRES_IMU},
1071 : #endif
1072 : { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE},
1073 : { MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2},
1074 : { MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3},
1075 : #if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
1076 : { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW},
1077 : #endif
1078 : #if AP_GPS_GPS_RTK_SENDING_ENABLED
1079 : { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK},
1080 : #endif
1081 : #if AP_GPS_GPS2_RAW_SENDING_ENABLED
1082 : { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW},
1083 : #endif
1084 : #if AP_GPS_GPS2_RTK_SENDING_ENABLED
1085 : { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK},
1086 : #endif
1087 : { MAVLINK_MSG_ID_SYSTEM_TIME, MSG_SYSTEM_TIME},
1088 : { MAVLINK_MSG_ID_PARAM_VALUE, MSG_NEXT_PARAM},
1089 : #if AP_FENCE_ENABLED
1090 : { MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS},
1091 : #endif
1092 : #if AP_SIM_ENABLED
1093 : { MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE},
1094 : { MAVLINK_MSG_ID_SIM_STATE, MSG_SIM_STATE},
1095 : #endif
1096 : #if AP_AHRS_ENABLED
1097 : { MAVLINK_MSG_ID_AHRS2, MSG_AHRS2},
1098 : { MAVLINK_MSG_ID_AHRS, MSG_AHRS},
1099 : { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE},
1100 : { MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MSG_ATTITUDE_QUATERNION},
1101 : { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION},
1102 : #if AP_AHRS_ENABLED
1103 : { MAVLINK_MSG_ID_LOCAL_POSITION_NED, MSG_LOCAL_POSITION},
1104 : #endif // AP_AHRS_ENABLED
1105 : { MAVLINK_MSG_ID_VFR_HUD, MSG_VFR_HUD},
1106 : #endif
1107 : #if AP_MAVLINK_MSG_WIND_ENABLED
1108 : { MAVLINK_MSG_ID_WIND, MSG_WIND},
1109 : #endif // AP_MAVLINK_MSG_WIND_ENABLED
1110 : #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
1111 : { MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER},
1112 : #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
1113 : { MAVLINK_MSG_ID_DISTANCE_SENSOR, MSG_DISTANCE_SENSOR},
1114 : #if AP_TERRAIN_AVAILABLE
1115 : { MAVLINK_MSG_ID_TERRAIN_REQUEST, MSG_TERRAIN_REQUEST},
1116 : { MAVLINK_MSG_ID_TERRAIN_REPORT, MSG_TERRAIN_REPORT},
1117 : #endif
1118 : #if AP_CAMERA_ENABLED
1119 : { MAVLINK_MSG_ID_CAMERA_FEEDBACK, MSG_CAMERA_FEEDBACK},
1120 : { MAVLINK_MSG_ID_CAMERA_INFORMATION, MSG_CAMERA_INFORMATION},
1121 : { MAVLINK_MSG_ID_CAMERA_SETTINGS, MSG_CAMERA_SETTINGS},
1122 : #if AP_CAMERA_SEND_FOV_STATUS_ENABLED
1123 : { MAVLINK_MSG_ID_CAMERA_FOV_STATUS, MSG_CAMERA_FOV_STATUS},
1124 : #endif
1125 : { MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, MSG_CAMERA_CAPTURE_STATUS},
1126 : #if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
1127 : { MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, MSG_CAMERA_THERMAL_RANGE},
1128 : #endif // AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
1129 : #if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
1130 : { MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, MSG_VIDEO_STREAM_INFORMATION},
1131 : #endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
1132 : #endif // AP_CAMERA_ENABLED
1133 : #if HAL_MOUNT_ENABLED
1134 : { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
1135 : { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
1136 : { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION},
1137 : { MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, MSG_GIMBAL_MANAGER_STATUS},
1138 : #endif
1139 : #if AP_OPTICALFLOW_ENABLED
1140 : { MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW},
1141 : #endif
1142 : #if COMPASS_CAL_ENABLED
1143 : { MAVLINK_MSG_ID_MAG_CAL_PROGRESS, MSG_MAG_CAL_PROGRESS},
1144 : { MAVLINK_MSG_ID_MAG_CAL_REPORT, MSG_MAG_CAL_REPORT},
1145 : #endif
1146 : #if AP_AHRS_ENABLED
1147 : { MAVLINK_MSG_ID_EKF_STATUS_REPORT, MSG_EKF_STATUS_REPORT},
1148 : #endif // AP_AHRS_ENABLED
1149 : { MAVLINK_MSG_ID_PID_TUNING, MSG_PID_TUNING},
1150 : { MAVLINK_MSG_ID_VIBRATION, MSG_VIBRATION},
1151 : #if AP_RPM_ENABLED
1152 : { MAVLINK_MSG_ID_RPM, MSG_RPM},
1153 : #endif
1154 : { MAVLINK_MSG_ID_MISSION_ITEM_REACHED, MSG_MISSION_ITEM_REACHED},
1155 : { MAVLINK_MSG_ID_ATTITUDE_TARGET, MSG_ATTITUDE_TARGET},
1156 : { MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, MSG_POSITION_TARGET_GLOBAL_INT},
1157 : { MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, MSG_POSITION_TARGET_LOCAL_NED},
1158 : #if HAL_ADSB_ENABLED
1159 : { MAVLINK_MSG_ID_ADSB_VEHICLE, MSG_ADSB_VEHICLE},
1160 : #endif
1161 : #if AP_BATTERY_ENABLED
1162 : { MAVLINK_MSG_ID_BATTERY_STATUS, MSG_BATTERY_STATUS},
1163 : #endif
1164 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1165 : { MAVLINK_MSG_ID_AOA_SSA, MSG_AOA_SSA},
1166 : #endif // APM_BUILD_ArduPlane
1167 : #if HAL_LANDING_DEEPSTALL_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1168 : { MAVLINK_MSG_ID_DEEPSTALL, MSG_LANDING},
1169 : #endif
1170 : { MAVLINK_MSG_ID_EXTENDED_SYS_STATE, MSG_EXTENDED_SYS_STATE},
1171 : { MAVLINK_MSG_ID_AUTOPILOT_VERSION, MSG_AUTOPILOT_VERSION},
1172 : #if HAL_EFI_ENABLED
1173 : { MAVLINK_MSG_ID_EFI_STATUS, MSG_EFI_STATUS},
1174 : #endif
1175 : #if HAL_GENERATOR_ENABLED
1176 : { MAVLINK_MSG_ID_GENERATOR_STATUS, MSG_GENERATOR_STATUS},
1177 : #endif
1178 : #if AP_WINCH_ENABLED
1179 : { MAVLINK_MSG_ID_WINCH_STATUS, MSG_WINCH_STATUS},
1180 : #endif
1181 : #if HAL_WITH_ESC_TELEM
1182 : { MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4, MSG_ESC_TELEMETRY},
1183 : #endif
1184 : #if AP_RANGEFINDER_ENABLED && (APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_TYPE(APM_BUILD_ArduSub))
1185 : { MAVLINK_MSG_ID_WATER_DEPTH, MSG_WATER_DEPTH},
1186 : #endif
1187 : #if HAL_HIGH_LATENCY2_ENABLED
1188 : { MAVLINK_MSG_ID_HIGH_LATENCY2, MSG_HIGH_LATENCY2},
1189 : #endif
1190 : #if AP_AIS_ENABLED
1191 : { MAVLINK_MSG_ID_AIS_VESSEL, MSG_AIS_VESSEL},
1192 : #endif
1193 : #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
1194 : { MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_STATUS, MSG_UAVIONIX_ADSB_OUT_STATUS},
1195 : #endif
1196 : #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
1197 : { MAVLINK_MSG_ID_RELAY_STATUS, MSG_RELAY_STATUS},
1198 : #endif
1199 : #if AP_AIRSPEED_ENABLED
1200 : { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED},
1201 : #endif
1202 : { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES},
1203 : { MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, MSG_AVAILABLE_MODES_MONITOR},
1204 : #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
1205 : { MAVLINK_MSG_ID_FLIGHT_INFORMATION, MSG_FLIGHT_INFORMATION},
1206 : #endif // AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
1207 : #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
1208 : { MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, MSG_UTM_GLOBAL_POSITION},
1209 : #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
1210 : };
1211 :
1212 14840995 : for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
1213 14840992 : if (map[i].mavlink_id == mavlink_id) {
1214 457143 : return map[i].msg_id;
1215 : }
1216 : }
1217 3 : return MSG_LAST;
1218 : }
1219 :
1220 6340 : bool GCS_MAVLINK::set_mavlink_message_id_interval(const uint32_t mavlink_id,
1221 : const uint16_t interval_ms)
1222 : {
1223 6340 : const ap_message id = mavlink_id_to_ap_message_id(mavlink_id);
1224 6340 : if (id == MSG_LAST) {
1225 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1226 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)mavlink_id);
1227 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
1228 0 : return false;
1229 : }
1230 6340 : return set_ap_message_interval(id, interval_ms);
1231 : }
1232 :
1233 1089924 : bool GCS_MAVLINK::should_send_message_in_delay_callback(const ap_message id) const
1234 : {
1235 : // No ID we return true for may take more than a few hundred
1236 : // microseconds to return!
1237 :
1238 1089924 : switch (id) {
1239 467295 : case MSG_NEXT_PARAM:
1240 : case MSG_HEARTBEAT:
1241 : #if HAL_HIGH_LATENCY2_ENABLED
1242 : case MSG_HIGH_LATENCY2:
1243 : #endif
1244 : case MSG_AUTOPILOT_VERSION:
1245 467295 : return true;
1246 622629 : default:
1247 622629 : return false;
1248 : }
1249 :
1250 : return false;
1251 : }
1252 :
1253 103052786 : uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t &deferred) const
1254 : {
1255 103052786 : uint32_t interval_ms = deferred.interval_ms;
1256 :
1257 103052786 : interval_ms += stream_slowdown_ms;
1258 :
1259 : // slow most messages down if we're transfering parameters or
1260 : // waypoints:
1261 103052786 : if (_queued_parameter) {
1262 : // we are sending parameters, penalize streams:
1263 235620 : interval_ms *= 4;
1264 : }
1265 103052786 : if (requesting_mission_items()) {
1266 : // we are sending requests for waypoints, penalize streams:
1267 20827 : interval_ms *= 4;
1268 : }
1269 : #if AP_MAVLINK_FTP_ENABLED
1270 103052786 : if (AP_HAL::millis() - GCS_FTP::get_last_send_ms(chan) < 1000) {
1271 : // we are sending ftp replies
1272 105418 : interval_ms *= 4;
1273 : }
1274 : #endif
1275 :
1276 103052786 : if (interval_ms > 60000) {
1277 0 : return 60000;
1278 : }
1279 :
1280 103052786 : return interval_ms;
1281 : }
1282 :
1283 : // typical runtime on fmuv3: 5 microseconds for 3 buckets
1284 1253105 : void GCS_MAVLINK::find_next_bucket_to_send(uint16_t now16_ms)
1285 : {
1286 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1287 : void *data = hal.scheduler->disable_interrupts_save();
1288 : uint32_t start_us = AP_HAL::micros();
1289 : #endif
1290 :
1291 : // all done sending this bucket... find another bucket...
1292 1253105 : sending_bucket_id = no_bucket_to_send;
1293 1253105 : uint16_t ms_before_send_next_bucket_to_send = UINT16_MAX;
1294 13784155 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message_bucket); i++) {
1295 12531050 : if (deferred_message_bucket[i].ap_message_ids.count() == 0) {
1296 : // no entries
1297 11138519 : continue;
1298 : }
1299 1392531 : const uint16_t interval = get_reschedule_interval_ms(deferred_message_bucket[i]);
1300 1392531 : const uint16_t ms_since_last_sent = now16_ms - deferred_message_bucket[i].last_sent_ms;
1301 : uint16_t ms_before_send_this_bucket;
1302 1392531 : if (ms_since_last_sent > interval) {
1303 : // should already have sent this bucket!
1304 3989 : ms_before_send_this_bucket = 0;
1305 : } else {
1306 1388542 : ms_before_send_this_bucket = interval - ms_since_last_sent;
1307 : }
1308 1392531 : if (ms_before_send_this_bucket < ms_before_send_next_bucket_to_send) {
1309 1356101 : sending_bucket_id = i;
1310 1356101 : ms_before_send_next_bucket_to_send = ms_before_send_this_bucket;
1311 : }
1312 : }
1313 1253105 : if (sending_bucket_id != no_bucket_to_send) {
1314 1253105 : bucket_message_ids_to_send = deferred_message_bucket[sending_bucket_id].ap_message_ids;
1315 : } else {
1316 0 : bucket_message_ids_to_send.clearall();
1317 : }
1318 :
1319 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1320 : uint32_t delta_us = AP_HAL::micros() - start_us;
1321 : hal.scheduler->restore_interrupts(data);
1322 : if (delta_us > try_send_message_stats.fnbts_maxtime) {
1323 : try_send_message_stats.fnbts_maxtime = delta_us;
1324 : }
1325 : #endif
1326 1253105 : }
1327 :
1328 101811856 : ap_message GCS_MAVLINK::next_deferred_bucket_message_to_send(uint16_t now16_ms)
1329 : {
1330 101811856 : if (sending_bucket_id == no_bucket_to_send) {
1331 : // could happen if all streamrates are zero?
1332 1404487 : return no_message_to_send;
1333 : }
1334 :
1335 100407369 : const uint16_t ms_since_last_sent = now16_ms - deferred_message_bucket[sending_bucket_id].last_sent_ms;
1336 100407369 : if (ms_since_last_sent < get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id])) {
1337 : // not time to send this bucket
1338 42807133 : return no_message_to_send;
1339 : }
1340 :
1341 57600236 : const int16_t next = bucket_message_ids_to_send.first_set();
1342 57600236 : if (next == -1) {
1343 : // should not happen
1344 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1345 0 : AP_HAL::panic("next_deferred_bucket_message_to_send called on empty bucket");
1346 : #endif
1347 0 : find_next_bucket_to_send(now16_ms);
1348 0 : return no_message_to_send;
1349 : }
1350 57600236 : return (ap_message)next;
1351 : }
1352 :
1353 : // call try_send_message if appropriate. Incorporates debug code to
1354 : // record how long it takes to send a message. try_send_message is
1355 : // expected to be overridden, not this function.
1356 142406475 : bool GCS_MAVLINK::do_try_send_message(const ap_message id)
1357 : {
1358 142406475 : const bool in_delay_callback = hal.scheduler->in_delay_callback();
1359 142406475 : if (in_delay_callback && !should_send_message_in_delay_callback(id)) {
1360 622629 : return true;
1361 : }
1362 141783846 : if (telemetry_delayed()) {
1363 0 : return false;
1364 : }
1365 141783846 : WITH_SEMAPHORE(comm_chan_lock(chan));
1366 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1367 : void *data = hal.scheduler->disable_interrupts_save();
1368 : uint32_t start_send_message_us = AP_HAL::micros();
1369 : #endif
1370 141783846 : if (!try_send_message(id)) {
1371 : // didn't fit in buffer...
1372 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1373 : try_send_message_stats.no_space_for_message++;
1374 : hal.scheduler->restore_interrupts(data);
1375 : #endif
1376 82542673 : return false;
1377 : }
1378 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1379 : const uint32_t delta_us = AP_HAL::micros() - start_send_message_us;
1380 : hal.scheduler->restore_interrupts(data);
1381 : if (delta_us > try_send_message_stats.longest_time_us) {
1382 : try_send_message_stats.longest_time_us = delta_us;
1383 : try_send_message_stats.longest_id = id;
1384 : }
1385 : #endif
1386 59241173 : return true;
1387 141783846 : }
1388 :
1389 951859 : int8_t GCS_MAVLINK::get_deferred_message_index(const ap_message id) const
1390 : {
1391 3775728 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message); i++) {
1392 2836563 : if (deferred_message[i].id == id) {
1393 12694 : return i;
1394 : }
1395 : }
1396 939165 : return -1;
1397 : }
1398 :
1399 186618095 : int8_t GCS_MAVLINK::deferred_message_to_send_index(uint16_t now16_ms)
1400 : {
1401 :
1402 186618095 : if (next_deferred_message_to_send_cache == -1) {
1403 1794823 : uint16_t ms_before_next_message_to_send = UINT16_MAX;
1404 7179292 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message); i++) {
1405 5384469 : const uint16_t interval_ms = deferred_message[i].interval_ms;
1406 5384469 : if (interval_ms == 0) {
1407 1793644 : continue;
1408 : }
1409 3590825 : const uint16_t ms_since_last_sent = now16_ms - deferred_message[i].last_sent_ms;
1410 : uint16_t ms_before_send_this_message;
1411 3590825 : if (ms_since_last_sent > interval_ms) {
1412 : // should already have sent this one!
1413 55537 : ms_before_send_this_message = 0;
1414 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1415 : try_send_message_stats.behind++;
1416 : #endif
1417 : } else {
1418 3535288 : ms_before_send_this_message = interval_ms - ms_since_last_sent;
1419 : }
1420 3590825 : if (ms_before_send_this_message < ms_before_next_message_to_send) {
1421 3427442 : next_deferred_message_to_send_cache = i;
1422 3427442 : ms_before_next_message_to_send = ms_before_send_this_message;
1423 : }
1424 : }
1425 : }
1426 :
1427 186618095 : if (next_deferred_message_to_send_cache == -1) {
1428 : // this really shouldn't happen; we force parameter rates, for example.
1429 0 : return -1;
1430 : }
1431 :
1432 : // an intermediate 16-bit variable is used here to strictly avoid
1433 : // type promotion creating 32-bit (or higher) maths here. Do not
1434 : // be tempted to remove ms_since_last_sent here.
1435 186618095 : const uint16_t ms_since_last_sent = now16_ms - deferred_message[next_deferred_message_to_send_cache].last_sent_ms;
1436 186618095 : if (ms_since_last_sent < deferred_message[next_deferred_message_to_send_cache].interval_ms) {
1437 102386310 : return -1;
1438 : }
1439 :
1440 84231785 : return next_deferred_message_to_send_cache;
1441 : }
1442 :
1443 13 : bool GCS_MAVLINK_InProgress::send_ack(MAV_RESULT result)
1444 : {
1445 13 : if (!HAVE_PAYLOAD_SPACE(chan, COMMAND_ACK)) {
1446 : // can't fit the ACK, come back to this later
1447 0 : return false;
1448 : }
1449 :
1450 13 : mavlink_msg_command_ack_send(
1451 : chan,
1452 13 : mav_cmd,
1453 : result,
1454 : 0,
1455 : 0,
1456 13 : requesting_sysid,
1457 13 : requesting_compid
1458 : );
1459 :
1460 13 : return true;
1461 : }
1462 :
1463 8 : bool GCS_MAVLINK_InProgress::send_in_progress()
1464 : {
1465 8 : return send_ack(MAV_RESULT_IN_PROGRESS);
1466 : }
1467 :
1468 5 : bool GCS_MAVLINK_InProgress::conclude(MAV_RESULT result)
1469 : {
1470 5 : if (!send_ack(result)) {
1471 0 : return false;
1472 : }
1473 5 : task = Type::NONE;
1474 5 : return true;
1475 : }
1476 :
1477 5 : GCS_MAVLINK_InProgress *GCS_MAVLINK_InProgress::get_task(MAV_CMD mav_cmd, GCS_MAVLINK_InProgress::Type t, uint8_t sysid, uint8_t compid, mavlink_channel_t chan)
1478 : {
1479 : // we can't have two outstanding tasks for the same command from
1480 : // the same mavlink node or the result is ambiguous:
1481 10 : for (auto &_task : in_progress_tasks) {
1482 5 : if (_task.task == Type::NONE) {
1483 5 : continue;
1484 : }
1485 0 : if (_task.mav_cmd == mav_cmd &&
1486 0 : _task.requesting_sysid == sysid &&
1487 0 : _task.requesting_compid == compid) {
1488 0 : return nullptr;
1489 : }
1490 : }
1491 :
1492 5 : for (auto &_task : in_progress_tasks) {
1493 5 : if (_task.task != Type::NONE) {
1494 0 : continue;
1495 : }
1496 5 : _task.chan = chan;
1497 5 : _task.task = t;
1498 5 : _task.mav_cmd = mav_cmd;
1499 5 : _task.requesting_sysid = sysid;
1500 5 : _task.requesting_compid = compid;
1501 5 : return &_task;
1502 : }
1503 0 : return nullptr;
1504 : }
1505 :
1506 :
1507 126754392 : void GCS_MAVLINK_InProgress::check_tasks()
1508 : {
1509 : // run these checks only intermittently (rate-limits the
1510 : // MAV_RESULT_IN_PROGRESS messages):
1511 126754392 : const uint32_t now_ms = AP_HAL::millis();
1512 126754392 : if (now_ms - last_check_ms < 1000) {
1513 126599787 : return;
1514 : }
1515 154605 : last_check_ms = now_ms;
1516 :
1517 309210 : for (auto &task : in_progress_tasks) {
1518 154605 : switch (task.task) {
1519 154592 : case Type::NONE:
1520 154592 : break;
1521 13 : case Type::AIRSPEED_CAL: {
1522 : #if AP_AIRSPEED_ENABLED
1523 13 : const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
1524 13 : switch (airspeed->get_calibration_state()) {
1525 0 : case AP_Airspeed::CalibrationState::NOT_STARTED:
1526 : case AP_Airspeed::CalibrationState::NOT_REQUIRED_ZERO_OFFSET:
1527 : // we shouldn't get here
1528 0 : task.conclude(MAV_RESULT_FAILED);
1529 0 : break;
1530 8 : case AP_Airspeed::CalibrationState::IN_PROGRESS:
1531 8 : task.send_in_progress();
1532 8 : break;
1533 0 : case AP_Airspeed::CalibrationState::FAILED:
1534 0 : task.conclude(MAV_RESULT_FAILED);
1535 0 : break;
1536 5 : case AP_Airspeed::CalibrationState::SUCCESS:
1537 5 : task.conclude(MAV_RESULT_ACCEPTED);
1538 5 : break;
1539 : }
1540 : #endif
1541 : }
1542 13 : break;
1543 0 : case Type::SD_FORMAT:
1544 : #if AP_FILESYSTEM_FORMAT_ENABLED
1545 0 : switch (AP::FS().get_format_status()) {
1546 0 : case AP_Filesystem_Backend::FormatStatus::NOT_STARTED:
1547 : // we shouldn't get here
1548 0 : task.conclude(MAV_RESULT_FAILED);
1549 0 : break;
1550 0 : case AP_Filesystem_Backend::FormatStatus::IN_PROGRESS:
1551 : case AP_Filesystem_Backend::FormatStatus::PENDING:
1552 0 : task.send_in_progress();
1553 0 : break;
1554 0 : case AP_Filesystem_Backend::FormatStatus::SUCCESS:
1555 0 : task.conclude(MAV_RESULT_ACCEPTED);
1556 0 : break;
1557 0 : case AP_Filesystem_Backend::FormatStatus::FAILURE:
1558 0 : task.conclude(MAV_RESULT_FAILED);
1559 0 : break;
1560 : }
1561 : #endif
1562 0 : break;
1563 : }
1564 : }
1565 : }
1566 :
1567 126754392 : void GCS_MAVLINK::update_send()
1568 : {
1569 : #if HAL_LOGGING_ENABLED
1570 126754392 : if (!hal.scheduler->in_delay_callback()) {
1571 : // AP_Logger will not send log data if we are armed.
1572 126097412 : AP::logger().handle_log_send();
1573 : }
1574 : #endif
1575 126754392 : if (!deferred_messages_initialised) {
1576 6340 : initialise_message_intervals_from_streamrates();
1577 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
1578 6340 : initialise_message_intervals_from_config_files();
1579 : #endif
1580 6340 : deferred_messages_initialised = true;
1581 : }
1582 :
1583 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1584 : uint32_t retry_deferred_body_start = AP_HAL::micros();
1585 : #endif
1586 :
1587 : // check for any in-progress tasks; check_tasks does its own rate-limiting
1588 126754392 : GCS_MAVLINK_InProgress::check_tasks();
1589 :
1590 126754392 : const uint32_t start = AP_HAL::millis();
1591 126754392 : const uint16_t start16 = start & 0xFFFF;
1592 186618194 : while (AP_HAL::millis() - start < 5) { // spend a max of 5ms sending messages. This should never trigger - out_of_time() should become true
1593 186618194 : if (gcs().out_of_time()) {
1594 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1595 : try_send_message_stats.out_of_time++;
1596 : #endif
1597 99 : break;
1598 : }
1599 :
1600 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1601 : retry_deferred_body_start = AP_HAL::micros();
1602 : #endif
1603 :
1604 : // check if any "specially handled" messages should be sent out
1605 : {
1606 186618095 : const int8_t next = deferred_message_to_send_index(start16);
1607 186618095 : if (next != -1) {
1608 84231785 : if (!do_try_send_message(deferred_message[next].id)) {
1609 82443302 : break;
1610 : }
1611 : // we try to keep output on a regular clock to avoid
1612 : // user support questions:
1613 1788483 : const uint16_t interval_ms = deferred_message[next].interval_ms;
1614 1788483 : deferred_message[next].last_sent_ms += interval_ms;
1615 : // but we do not want to try to catch up too much:
1616 1788483 : if (uint16_t(start16 - deferred_message[next].last_sent_ms) > interval_ms) {
1617 179 : deferred_message[next].last_sent_ms = start16;
1618 : }
1619 :
1620 1788483 : next_deferred_message_to_send_cache = -1; // deferred_message_to_send will recalculate
1621 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1622 : const uint32_t stop = AP_HAL::micros();
1623 : const uint32_t delta = stop - retry_deferred_body_start;
1624 : if (delta > try_send_message_stats.max_retry_deferred_body_us) {
1625 : try_send_message_stats.max_retry_deferred_body_us = delta;
1626 : try_send_message_stats.max_retry_deferred_body_type = 1;
1627 : }
1628 : #endif
1629 1788483 : continue;
1630 1788483 : }
1631 : }
1632 :
1633 : // check for any messages that the code has explicitly sent
1634 102386310 : const int16_t fs = pushed_ap_message_ids.first_set();
1635 102386310 : if (fs != -1) {
1636 574454 : ap_message next = (ap_message)fs;
1637 574454 : if (!do_try_send_message(next)) {
1638 97936 : break;
1639 : }
1640 476518 : pushed_ap_message_ids.clear(next);
1641 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1642 : const uint32_t stop = AP_HAL::micros();
1643 : const uint32_t delta = stop - retry_deferred_body_start;
1644 : if (delta > try_send_message_stats.max_retry_deferred_body_us) {
1645 : try_send_message_stats.max_retry_deferred_body_us = delta;
1646 : try_send_message_stats.max_retry_deferred_body_type = 2;
1647 : }
1648 : #endif
1649 476518 : continue;
1650 476518 : }
1651 :
1652 101811856 : ap_message next = next_deferred_bucket_message_to_send(start16);
1653 101811856 : if (next != no_message_to_send) {
1654 57600236 : if (!do_try_send_message(next)) {
1655 1435 : break;
1656 : }
1657 57598801 : bucket_message_ids_to_send.clear(next);
1658 57598801 : if (bucket_message_ids_to_send.count() == 0) {
1659 : // we sent everything in the bucket. Reschedule it.
1660 : // we try to keep output on a regular clock to avoid
1661 : // user support questions:
1662 1252886 : const uint16_t interval_ms = get_reschedule_interval_ms(deferred_message_bucket[sending_bucket_id]);
1663 1252886 : deferred_message_bucket[sending_bucket_id].last_sent_ms += interval_ms;
1664 : // but we do not want to try to catch up too much:
1665 1252886 : if (uint16_t(start16 - deferred_message_bucket[sending_bucket_id].last_sent_ms) > interval_ms) {
1666 1672 : deferred_message_bucket[sending_bucket_id].last_sent_ms = start16;
1667 : }
1668 1252886 : find_next_bucket_to_send(start16);
1669 : }
1670 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1671 : const uint32_t stop = AP_HAL::micros();
1672 : const uint32_t delta = stop - retry_deferred_body_start;
1673 : if (delta > try_send_message_stats.max_retry_deferred_body_us) {
1674 : try_send_message_stats.max_retry_deferred_body_us = delta;
1675 : try_send_message_stats.max_retry_deferred_body_type = 3;
1676 : }
1677 : #endif
1678 57598801 : continue;
1679 57598801 : }
1680 44211620 : break;
1681 : }
1682 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1683 : const uint32_t stop = AP_HAL::micros();
1684 : const uint32_t delta = stop - retry_deferred_body_start;
1685 : if (delta > try_send_message_stats.max_retry_deferred_body_us) {
1686 : try_send_message_stats.max_retry_deferred_body_us = delta;
1687 : try_send_message_stats.max_retry_deferred_body_type = 4;
1688 : }
1689 : #endif
1690 :
1691 : // update the number of packets transmitted base on seqno, making
1692 : // the assumption that we don't send more than 256 messages
1693 : // between the last pass through here
1694 126754392 : send_packet_count += uint8_t(_channel_status.current_tx_seq - last_tx_seq);
1695 126754392 : last_tx_seq = _channel_status.current_tx_seq;
1696 126754392 : }
1697 :
1698 24580 : void GCS_MAVLINK::remove_message_from_bucket(int8_t bucket, ap_message id)
1699 : {
1700 24580 : deferred_message_bucket[bucket].ap_message_ids.clear(id);
1701 24580 : if (deferred_message_bucket[bucket].ap_message_ids.count() == 0) {
1702 : // bucket empty. Free it:
1703 758 : deferred_message_bucket[bucket].interval_ms = 0;
1704 758 : deferred_message_bucket[bucket].last_sent_ms = 0;
1705 : }
1706 :
1707 24580 : if (bucket == sending_bucket_id) {
1708 2552 : bucket_message_ids_to_send.clear(id);
1709 2552 : if (bucket_message_ids_to_send.count() == 0) {
1710 219 : find_next_bucket_to_send(AP_HAL::millis16());
1711 : } else {
1712 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1713 2333 : if (deferred_message_bucket[bucket].interval_ms == 0 &&
1714 0 : deferred_message_bucket[bucket].last_sent_ms == 0) {
1715 : // we just freed this bucket! this would mean that
1716 : // somehow our messages-still-to-send was a superset
1717 : // of the messages in the bucket we were sending,
1718 : // which would be bad.
1719 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1720 : }
1721 : #endif
1722 : }
1723 : }
1724 24580 : }
1725 :
1726 441730 : bool GCS_MAVLINK::set_ap_message_interval(enum ap_message id, uint16_t interval_ms)
1727 : {
1728 441730 : if (id == MSG_NEXT_PARAM) {
1729 : // force parameters to *always* get streamed so a vehicle is
1730 : // recoverable from bad configuration:
1731 6338 : if (interval_ms == 0) {
1732 3301 : interval_ms = 100;
1733 3037 : } else if (interval_ms > 1000) {
1734 0 : interval_ms = 1000;
1735 : }
1736 : }
1737 :
1738 : #if AP_SCHEDULER_ENABLED
1739 441730 : interval_ms = cap_message_interval(interval_ms);
1740 : #endif
1741 :
1742 : // check if it's a specially-handled message:
1743 441730 : const int8_t deferred_offset = get_deferred_message_index(id);
1744 441730 : if (deferred_offset != -1) {
1745 12688 : deferred_message[deferred_offset].interval_ms = interval_ms;
1746 12688 : deferred_message[deferred_offset].last_sent_ms = AP_HAL::millis16();
1747 12688 : return true;
1748 : }
1749 :
1750 : // see which bucket has the closest interval:
1751 429042 : int8_t closest_bucket = -1;
1752 429042 : uint16_t closest_bucket_interval_delta = UINT16_MAX;
1753 429042 : int8_t in_bucket = -1;
1754 429042 : int8_t empty_bucket_id = -1;
1755 4719462 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message_bucket); i++) {
1756 4290420 : const deferred_message_bucket_t &bucket = deferred_message_bucket[i];
1757 4290420 : if (bucket.interval_ms == 0) {
1758 : // unused bucket
1759 3963278 : if (empty_bucket_id == -1) {
1760 429042 : empty_bucket_id = i;
1761 : }
1762 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1763 3963278 : if (bucket.ap_message_ids.count() != 0) {
1764 0 : AP_HAL::panic("Bucket %u has zero interval but with ids set", i);
1765 : }
1766 : #endif
1767 3963278 : continue;
1768 : }
1769 327142 : if (bucket.ap_message_ids.get(id)) {
1770 55716 : in_bucket = i;
1771 : }
1772 327142 : const uint16_t interval_delta = abs(bucket.interval_ms - interval_ms);
1773 327142 : if (interval_delta < closest_bucket_interval_delta) {
1774 316486 : closest_bucket = i;
1775 316486 : closest_bucket_interval_delta = interval_delta;
1776 : }
1777 : }
1778 :
1779 429042 : if (in_bucket == -1 && interval_ms == 0) {
1780 : // not in a bucket and told to remove from scheduling
1781 154898 : return true;
1782 : }
1783 :
1784 274144 : if (in_bucket != -1) {
1785 55716 : if (interval_ms == 0) {
1786 : // remove it
1787 44 : remove_message_from_bucket(in_bucket, id);
1788 44 : return true;
1789 : }
1790 55672 : if (closest_bucket_interval_delta == 0 &&
1791 : in_bucket == closest_bucket) {
1792 : // don't need to move it
1793 31136 : return true;
1794 : }
1795 : // remove from existing bucket
1796 24536 : remove_message_from_bucket(in_bucket, id);
1797 24536 : if (empty_bucket_id == -1 &&
1798 0 : deferred_message_bucket[in_bucket].ap_message_ids.count() == 0) {
1799 0 : empty_bucket_id = in_bucket;
1800 : }
1801 : }
1802 :
1803 242964 : if (closest_bucket == -1 && empty_bucket_id == -1) {
1804 : // gah?!
1805 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1806 0 : ::fprintf(stderr, "no buckets?!\n");
1807 0 : abort();
1808 : #endif
1809 0 : return false;
1810 : }
1811 :
1812 242964 : if (closest_bucket_interval_delta != 0 &&
1813 : empty_bucket_id != -1) {
1814 : // allocate a bucket for this interval
1815 8667 : deferred_message_bucket[empty_bucket_id].interval_ms = interval_ms;
1816 8667 : deferred_message_bucket[empty_bucket_id].last_sent_ms = AP_HAL::millis16();
1817 8667 : closest_bucket = empty_bucket_id;
1818 : }
1819 :
1820 242964 : deferred_message_bucket[closest_bucket].ap_message_ids.set(id);
1821 :
1822 242964 : if (sending_bucket_id == no_bucket_to_send) {
1823 4313 : sending_bucket_id = closest_bucket;
1824 4313 : bucket_message_ids_to_send = deferred_message_bucket[closest_bucket].ap_message_ids;
1825 : }
1826 :
1827 242964 : return true;
1828 : }
1829 :
1830 : // queue a message to be sent (try_send_message does the *actual*
1831 : // mavlink work!)
1832 535645 : void GCS_MAVLINK::send_message(enum ap_message id)
1833 : {
1834 535645 : switch (id) {
1835 23630 : case MSG_HEARTBEAT:
1836 : #if HAL_HIGH_LATENCY2_ENABLED
1837 : case MSG_HIGH_LATENCY2:
1838 : #endif
1839 : #if AP_MAVLINK_SIGNING_ENABLED
1840 23630 : save_signing_timestamp(false);
1841 : #endif // AP_MAVLINK_SIGNING_ENABLED
1842 : // update the mask of all streaming channels
1843 23630 : if (is_streaming()) {
1844 14691 : GCS_MAVLINK::chan_is_streaming |= (1U<<(chan-MAVLINK_COMM_0));
1845 : } else {
1846 8939 : GCS_MAVLINK::chan_is_streaming &= ~(1U<<(chan-MAVLINK_COMM_0));
1847 : }
1848 23630 : break;
1849 512015 : default:
1850 512015 : break;
1851 : }
1852 :
1853 535645 : pushed_ap_message_ids.set(id);
1854 535645 : }
1855 :
1856 : // Plane only enables follow when scripting is enabled:
1857 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1858 : #define AP_MAVLINK_FOLLOW_HANDLING_ENABLED (AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED)
1859 : #elif APM_BUILD_TYPE(APM_BUILD_Rover) || APM_BUILD_COPTER_OR_HELI
1860 : #define AP_MAVLINK_FOLLOW_HANDLING_ENABLED AP_FOLLOW_ENABLED
1861 : #else
1862 : #define AP_MAVLINK_FOLLOW_HANDLING_ENABLED 0
1863 : #endif
1864 :
1865 2784593 : void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
1866 : const mavlink_message_t &msg)
1867 : {
1868 : // we exclude radio packets because we historically used this to
1869 : // make it possible to use the CLI over the radio
1870 2784593 : if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
1871 2784593 : mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
1872 : }
1873 2784593 : const auto mavlink_protocol = uartstate->get_protocol();
1874 2784593 : if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
1875 2784593 : (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
1876 9804 : (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 ||
1877 : mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL)) {
1878 : // if we receive any MAVLink2 packets on a connection
1879 : // currently sending MAVLink1 then switch to sending
1880 : // MAVLink2
1881 0 : _channel_status.flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
1882 : }
1883 2784593 : }
1884 :
1885 2784593 : void GCS_MAVLINK::raw_packetReceived(uint8_t framing_status,
1886 : const mavlink_status_t &status,
1887 : const mavlink_message_t &msg)
1888 : {
1889 2784593 : if (framing_status == MAVLINK_FRAMING_OK) {
1890 2784593 : packetReceived(status, msg);
1891 : }
1892 :
1893 2784593 : if (!routing.check_and_forward(framing_status, *this, msg)) {
1894 : // the routing code has indicated we should not handle this packet locally
1895 158 : return;
1896 : }
1897 2784435 : if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
1898 : #if HAL_MOUNT_ENABLED
1899 : // allow mounts to see the location of other vehicles
1900 2801 : handle_mount_message(msg);
1901 : #endif
1902 : }
1903 : #if AP_SCRIPTING_ENABLED
1904 : {
1905 2784435 : AP_Scripting *scripting = AP_Scripting::get_singleton();
1906 2784435 : if (scripting != nullptr) {
1907 2784435 : scripting->handle_message(msg, chan);
1908 : }
1909 : }
1910 : #endif // AP_SCRIPTING_ENABLED
1911 :
1912 : #if AP_MAVLINK_FOLLOW_HANDLING_ENABLED
1913 : {
1914 2637438 : AP_Follow *follow = AP_Follow::get_singleton();
1915 2637438 : if (follow != nullptr) {
1916 2637438 : follow->handle_msg(msg);
1917 : }
1918 : }
1919 : #endif
1920 :
1921 2784435 : if (!accept_packet(status, msg)) {
1922 : // e.g. enforce-sysid says we shouldn't look at this packet
1923 68 : return;
1924 : }
1925 2784367 : handle_message(msg);
1926 : }
1927 :
1928 : void
1929 126756574 : GCS_MAVLINK::update_receive(uint32_t max_time_us)
1930 : {
1931 : // do absolutely nothing if we are locked
1932 126756574 : if (locked()) {
1933 0 : return;
1934 : }
1935 :
1936 : // receive new packets
1937 : mavlink_message_t msg;
1938 : mavlink_status_t status;
1939 126756574 : uint32_t tstart_us = AP_HAL::micros();
1940 126756574 : uint32_t now_ms = AP_HAL::millis();
1941 :
1942 126756574 : status.packet_rx_drop_count = 0;
1943 :
1944 126756574 : const uint16_t nbytes = _port->available();
1945 245621799 : for (uint16_t i=0; i<nbytes; i++)
1946 : {
1947 118867078 : const uint8_t c = (uint8_t)_port->read();
1948 118867078 : const uint32_t protocol_timeout = 4000;
1949 :
1950 118867078 : if (alternative.handler &&
1951 118867078 : now_ms - alternative.last_mavlink_ms > protocol_timeout) {
1952 : /*
1953 : we have an alternative protocol handler installed and we
1954 : haven't parsed a MAVLink packet for 4 seconds. Try
1955 : parsing using alternative handler
1956 : */
1957 0 : if (alternative.handler(c, mavlink_comm_port[chan])) {
1958 0 : alternative.last_alternate_ms = now_ms;
1959 0 : gcs_alternative_active[chan] = true;
1960 : }
1961 :
1962 : /*
1963 : we may also try parsing as MAVLink if we haven't had a
1964 : successful parse on the alternative protocol for 4s
1965 : */
1966 0 : if (now_ms - alternative.last_alternate_ms <= protocol_timeout) {
1967 0 : continue;
1968 : }
1969 : }
1970 :
1971 118867078 : bool parsed_packet = false;
1972 :
1973 : // Try to get a new message
1974 118867078 : const uint8_t framing = mavlink_frame_char_buffer(channel_buffer(), channel_status(), c, &msg, &status);
1975 118867078 : if (framing != MAVLINK_FRAMING_INCOMPLETE) {
1976 2784593 : hal.util->persistent_data.last_mavlink_msgid = msg.msgid;
1977 2784593 : raw_packetReceived(framing, status, msg);
1978 2782954 : if (framing == MAVLINK_FRAMING_OK) {
1979 2782954 : parsed_packet = true;
1980 2782954 : gcs_alternative_active[chan] = false;
1981 2782954 : alternative.last_mavlink_ms = now_ms;
1982 : }
1983 2782954 : hal.util->persistent_data.last_mavlink_msgid = 0;
1984 :
1985 : }
1986 : #if AP_SCRIPTING_ENABLED
1987 118865439 : if (framing == MAVLINK_FRAMING_BAD_CRC) {
1988 : // This may be a valid message that we don't know the crc extra for, pass it to scripting which might
1989 0 : AP_Scripting *scripting = AP_Scripting::get_singleton();
1990 0 : if (scripting != nullptr) {
1991 0 : scripting->handle_message(msg, chan);
1992 : }
1993 : }
1994 : #endif // AP_SCRIPTING_ENABLED
1995 :
1996 118865439 : if (parsed_packet || i % 100 == 0) {
1997 : // make sure we don't spend too much time parsing mavlink messages
1998 5060711 : if (AP_HAL::micros() - tstart_us > max_time_us) {
1999 214 : break;
2000 : }
2001 : }
2002 : }
2003 :
2004 126754935 : const uint32_t tnow = AP_HAL::millis();
2005 :
2006 : // send a timesync message every 10 seconds; this is for data
2007 : // collection purposes
2008 : #if HAL_HIGH_LATENCY2_ENABLED
2009 126754935 : if (tnow - _timesync_request.last_sent_ms > _timesync_request.interval_ms && !is_private() && !is_high_latency_link) {
2010 : #else
2011 : if (tnow - _timesync_request.last_sent_ms > _timesync_request.interval_ms && !is_private()) {
2012 : #endif
2013 75783112 : if (HAVE_PAYLOAD_SPACE(chan, TIMESYNC)) {
2014 15191 : send_timesync();
2015 15191 : _timesync_request.last_sent_ms = tnow;
2016 : }
2017 : }
2018 :
2019 : #if HAL_LOGGING_ENABLED
2020 : // consider logging mavlink stats:
2021 126754935 : if (is_active() || is_streaming()) {
2022 67299053 : if (tnow - last_mavlink_stats_logged > 1000) {
2023 311551 : log_mavlink_stats();
2024 311551 : last_mavlink_stats_logged = tnow;
2025 : }
2026 : }
2027 : #endif
2028 :
2029 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
2030 :
2031 : const uint16_t now16_ms{AP_HAL::millis16()};
2032 :
2033 : if (uint16_t(now16_ms - try_send_message_stats.statustext_last_sent_ms) > 10000U) {
2034 : if (try_send_message_stats.longest_time_us) {
2035 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2036 : "GCS.chan(%u): ap_msg=%u took %uus to send",
2037 : chan,
2038 : try_send_message_stats.longest_id,
2039 : try_send_message_stats.longest_time_us);
2040 : try_send_message_stats.longest_time_us = 0;
2041 : }
2042 : if (try_send_message_stats.no_space_for_message &&
2043 : (is_active() || is_streaming())) {
2044 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2045 : "GCS.chan(%u): out-of-space: %u",
2046 : chan,
2047 : try_send_message_stats.no_space_for_message);
2048 : try_send_message_stats.no_space_for_message = 0;
2049 : }
2050 : if (try_send_message_stats.out_of_time) {
2051 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2052 : "GCS.chan(%u): out-of-time=%u",
2053 : chan,
2054 : try_send_message_stats.out_of_time);
2055 : try_send_message_stats.out_of_time = 0;
2056 : }
2057 : if (max_slowdown_ms) {
2058 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2059 : "GCS.chan(%u): max slowdown=%u",
2060 : chan,
2061 : max_slowdown_ms);
2062 : max_slowdown_ms = 0;
2063 : }
2064 : if (try_send_message_stats.behind) {
2065 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2066 : "GCS.chan(%u): behind=%u",
2067 : chan,
2068 : try_send_message_stats.behind);
2069 : try_send_message_stats.behind = 0;
2070 : }
2071 : if (try_send_message_stats.fnbts_maxtime) {
2072 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2073 : "GCS.chan(%u): fnbts_maxtime=%uus",
2074 : chan,
2075 : try_send_message_stats.fnbts_maxtime);
2076 : try_send_message_stats.fnbts_maxtime = 0;
2077 : }
2078 : if (try_send_message_stats.max_retry_deferred_body_us) {
2079 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2080 : "GCS.chan(%u): retry_body_maxtime=%uus (%u)",
2081 : chan,
2082 : try_send_message_stats.max_retry_deferred_body_us,
2083 : try_send_message_stats.max_retry_deferred_body_type
2084 : );
2085 : try_send_message_stats.max_retry_deferred_body_us = 0;
2086 : }
2087 :
2088 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message_bucket); i++) {
2089 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
2090 : "B. intvl. (%u): %u %u %u %u %u",
2091 : chan,
2092 : deferred_message_bucket[0].interval_ms,
2093 : deferred_message_bucket[1].interval_ms,
2094 : deferred_message_bucket[2].interval_ms,
2095 : deferred_message_bucket[3].interval_ms,
2096 : deferred_message_bucket[4].interval_ms);
2097 : }
2098 :
2099 : try_send_message_stats.statustext_last_sent_ms = now16_ms;
2100 : }
2101 : #endif
2102 : }
2103 :
2104 : #if HAL_LOGGING_ENABLED
2105 : /*
2106 : record stats about this link to logger
2107 : */
2108 311551 : void GCS_MAVLINK::log_mavlink_stats()
2109 : {
2110 311551 : uint8_t flags = 0;
2111 : #if AP_MAVLINK_SIGNING_ENABLED
2112 311551 : if (signing_enabled()) {
2113 0 : flags |= (uint8_t)Flags::USING_SIGNING;
2114 : }
2115 : #endif // AP_MAVLINK_SIGNING_ENABLED
2116 311551 : if (is_streaming()) {
2117 305565 : flags |= (uint8_t)Flags::STREAMING;
2118 : }
2119 311551 : if (is_active()) {
2120 164580 : flags |= (uint8_t)Flags::ACTIVE;
2121 : }
2122 311551 : if (is_private()) {
2123 226 : flags |= (uint8_t)Flags::PRIVATE;
2124 : }
2125 311551 : if (locked()) {
2126 0 : flags |= (uint8_t)Flags::LOCKED;
2127 : }
2128 :
2129 311551 : const struct log_MAV pkt{
2130 : LOG_PACKET_HEADER_INIT(LOG_MAV_MSG),
2131 311551 : time_us : AP_HAL::micros64(),
2132 311551 : chan : (uint8_t)chan,
2133 311551 : packet_tx_count : send_packet_count,
2134 311551 : packet_rx_success_count: _channel_status.packet_rx_success_count,
2135 311551 : packet_rx_drop_count : _channel_status.packet_rx_drop_count,
2136 : flags : flags,
2137 311551 : stream_slowdown_ms : stream_slowdown_ms,
2138 311551 : times_full : out_of_space_to_send_count,
2139 311551 : GCS_SYSID_last_seen_ms : _sysid_gcs_last_seen_time_ms,
2140 311551 : };
2141 :
2142 311551 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
2143 311551 : }
2144 : #endif
2145 :
2146 : /*
2147 : send the SYSTEM_TIME message
2148 : */
2149 1117844 : void GCS_MAVLINK::send_system_time() const
2150 : {
2151 1117844 : uint64_t time_unix = 0;
2152 : #if AP_RTC_ENABLED
2153 1117844 : AP::rtc().get_utc_usec(time_unix); // may fail, leaving time_unix at 0
2154 : #endif
2155 :
2156 1117844 : mavlink_msg_system_time_send(
2157 1117844 : chan,
2158 : time_unix,
2159 : AP_HAL::millis());
2160 1117844 : }
2161 :
2162 :
2163 1120196 : bool GCS_MAVLINK::sending_mavlink1() const
2164 : {
2165 1120196 : return ((_channel_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0);
2166 : }
2167 :
2168 : #if AP_RC_CHANNEL_ENABLED
2169 : /*
2170 : send RC_CHANNELS messages
2171 : */
2172 1117390 : void GCS_MAVLINK::send_rc_channels() const
2173 : {
2174 1117390 : uint16_t values[18] = {};
2175 1117390 : rc().get_radio_in(values, ARRAY_SIZE(values));
2176 :
2177 1117390 : mavlink_msg_rc_channels_send(
2178 1117390 : chan,
2179 : AP_HAL::millis(),
2180 1117390 : RC_Channels::get_valid_channel_count(),
2181 1117390 : values[0],
2182 1117390 : values[1],
2183 1117390 : values[2],
2184 1117390 : values[3],
2185 1117390 : values[4],
2186 1117390 : values[5],
2187 1117390 : values[6],
2188 1117390 : values[7],
2189 1117390 : values[8],
2190 1117390 : values[9],
2191 1117390 : values[10],
2192 1117390 : values[11],
2193 1117390 : values[12],
2194 1117390 : values[13],
2195 1117390 : values[14],
2196 1117390 : values[15],
2197 1117390 : values[16],
2198 1117390 : values[17],
2199 : #if AP_RSSI_ENABLED
2200 1117390 : receiver_rssi()
2201 : #else
2202 : 255 // meaning "unknown"
2203 : #endif
2204 : );
2205 1117390 : }
2206 :
2207 : #if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
2208 1117302 : void GCS_MAVLINK::send_rc_channels_raw() const
2209 : {
2210 : // for mavlink1 send RC_CHANNELS_RAW, for compatibility with OSD
2211 : // implementations
2212 1117302 : if (!sending_mavlink1()) {
2213 1117286 : return;
2214 : }
2215 :
2216 16 : uint16_t values[8] = {};
2217 16 : rc().get_radio_in(values, ARRAY_SIZE(values));
2218 :
2219 32 : mavlink_msg_rc_channels_raw_send(
2220 16 : chan,
2221 : AP_HAL::millis(),
2222 : 0,
2223 16 : values[0],
2224 16 : values[1],
2225 16 : values[2],
2226 16 : values[3],
2227 16 : values[4],
2228 16 : values[5],
2229 16 : values[6],
2230 16 : values[7],
2231 : #if AP_RSSI_ENABLED
2232 16 : receiver_rssi()
2233 : #else
2234 : 255 // meaning "unknown"
2235 : #endif
2236 : );
2237 : }
2238 : #endif // AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
2239 : #endif // AP_RC_CHANNEL_ENABLED
2240 :
2241 1117608 : void GCS_MAVLINK::send_raw_imu()
2242 : {
2243 : #if AP_INERTIALSENSOR_ENABLED
2244 1117608 : const AP_InertialSensor &ins = AP::ins();
2245 :
2246 1117608 : const Vector3f &accel = ins.get_accel(0);
2247 1117608 : const Vector3f &gyro = ins.get_gyro(0);
2248 1117608 : Vector3f mag;
2249 : #if AP_COMPASS_ENABLED
2250 1117608 : const Compass &compass = AP::compass();
2251 1117608 : if (compass.get_count() >= 1) {
2252 1117250 : mag = compass.get_field(0);
2253 : }
2254 : #endif
2255 :
2256 1117608 : mavlink_msg_raw_imu_send(
2257 : chan,
2258 : AP_HAL::micros64(),
2259 1117608 : accel.x * 1000.0f / GRAVITY_MSS,
2260 1117608 : accel.y * 1000.0f / GRAVITY_MSS,
2261 1117608 : accel.z * 1000.0f / GRAVITY_MSS,
2262 1117608 : gyro.x * 1000.0f,
2263 1117608 : gyro.y * 1000.0f,
2264 1117608 : gyro.z * 1000.0f,
2265 1117608 : mag.x,
2266 1117608 : mag.y,
2267 1117608 : mag.z,
2268 : 0, // we use SCALED_IMU and SCALED_IMU2 for other IMUs
2269 1117608 : int16_t(ins.get_temperature(0)*100));
2270 : #endif
2271 1117608 : }
2272 :
2273 : #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
2274 0 : void GCS_MAVLINK::send_highres_imu()
2275 : {
2276 : static const uint16_t HIGHRES_IMU_UPDATED_XACC = 0x01;
2277 : static const uint16_t HIGHRES_IMU_UPDATED_YACC = 0x02;
2278 : static const uint16_t HIGHRES_IMU_UPDATED_ZACC = 0x04;
2279 : static const uint16_t HIGHRES_IMU_UPDATED_XGYRO = 0x08;
2280 : static const uint16_t HIGHRES_IMU_UPDATED_YGYRO = 0x10;
2281 : static const uint16_t HIGHRES_IMU_UPDATED_ZGYRO = 0x20;
2282 : #if AP_COMPASS_ENABLED
2283 : static const uint16_t HIGHRES_IMU_UPDATED_XMAG = 0x40;
2284 : static const uint16_t HIGHRES_IMU_UPDATED_YMAG = 0x80;
2285 : static const uint16_t HIGHRES_IMU_UPDATED_ZMAG = 0x100;
2286 : #endif // AP_COMPASS_ENABLED
2287 : #if AP_BARO_ENABLED
2288 : static const uint16_t HIGHRES_IMU_UPDATED_ABS_PRESSURE = 0x200;
2289 : static const uint16_t HIGHRES_IMU_UPDATED_DIFF_PRESSURE = 0x400;
2290 : static const uint16_t HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800;
2291 : static const uint16_t HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000;
2292 : #endif // AP_BARO_ENABLED
2293 :
2294 0 : const AP_InertialSensor &ins = AP::ins();
2295 0 : const Vector3f& accel = ins.get_accel();
2296 0 : const Vector3f& gyro = ins.get_gyro();
2297 :
2298 0 : mavlink_highres_imu_t reply = {
2299 0 : .time_usec = AP_HAL::micros64(),
2300 0 : .xacc = accel.x,
2301 0 : .yacc = accel.y,
2302 0 : .zacc = accel.z,
2303 0 : .xgyro = gyro.x,
2304 0 : .ygyro = gyro.y,
2305 0 : .zgyro = gyro.z,
2306 : .xmag = 0.0,
2307 : .ymag = 0.0,
2308 : .zmag = 0.0,
2309 : .abs_pressure = 0.0,
2310 : .diff_pressure = 0.0,
2311 : .pressure_alt = 0.0,
2312 : .temperature = 0.0,
2313 : .fields_updated = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC |
2314 : HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO),
2315 0 : .id = ins.get_first_usable_accel(),
2316 0 : };
2317 :
2318 : #if AP_COMPASS_ENABLED
2319 0 : const Compass &compass = AP::compass();
2320 0 : if (compass.get_count() >= 1) {
2321 0 : const Vector3f field = compass.get_field() * 0.001f;
2322 0 : reply.xmag = field.x; // convert to gauss
2323 0 : reply.ymag = field.y;
2324 0 : reply.zmag = field.z;
2325 0 : reply.fields_updated |= (HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG);
2326 : }
2327 : #endif
2328 :
2329 : #if AP_BARO_ENABLED
2330 0 : const AP_Baro &barometer = AP::baro();
2331 0 : reply.abs_pressure = barometer.get_pressure() * 0.01f;
2332 0 : reply.temperature = barometer.get_temperature();
2333 0 : reply.pressure_alt = barometer.get_altitude_AMSL();
2334 0 : reply.diff_pressure = reply.abs_pressure - barometer.get_ground_pressure() * 0.01f;
2335 0 : reply.fields_updated |= (HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE |
2336 : HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE);
2337 : #endif
2338 :
2339 0 : mavlink_msg_highres_imu_send_struct(chan, &reply);
2340 0 : }
2341 : #endif // AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
2342 :
2343 2234176 : void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature))
2344 : {
2345 : #if AP_INERTIALSENSOR_ENABLED
2346 2234176 : const AP_InertialSensor &ins = AP::ins();
2347 2234176 : int16_t _temperature = 0;
2348 :
2349 2234176 : bool have_data = false;
2350 2234176 : Vector3f accel{};
2351 2234176 : if (ins.get_accel_count() > instance) {
2352 1123516 : accel = ins.get_accel(instance);
2353 1123516 : _temperature = ins.get_temperature(instance)*100;
2354 1123516 : have_data = true;
2355 : }
2356 2234176 : Vector3f gyro{};
2357 2234176 : if (ins.get_gyro_count() > instance) {
2358 1123516 : gyro = ins.get_gyro(instance);
2359 1123516 : have_data = true;
2360 : }
2361 2234176 : Vector3f mag;
2362 : #if AP_COMPASS_ENABLED
2363 2234176 : const Compass &compass = AP::compass();
2364 2234176 : if (compass.get_count() > instance) {
2365 2233460 : mag = compass.get_field(instance);
2366 2233460 : have_data = true;
2367 : }
2368 : #endif
2369 2234176 : if (!have_data) {
2370 358 : return;
2371 : }
2372 2233818 : send_fn(
2373 : chan,
2374 : AP_HAL::millis(),
2375 2233818 : accel.x * 1000.0f / GRAVITY_MSS,
2376 2233818 : accel.y * 1000.0f / GRAVITY_MSS,
2377 2233818 : accel.z * 1000.0f / GRAVITY_MSS,
2378 2233818 : gyro.x * 1000.0f,
2379 2233818 : gyro.y * 1000.0f,
2380 2233818 : gyro.z * 1000.0f,
2381 2233818 : mag.x,
2382 2233818 : mag.y,
2383 2233818 : mag.z,
2384 : _temperature);
2385 : #endif
2386 : }
2387 :
2388 :
2389 : // send data for barometer and airspeed sensors instances. In the
2390 : // case that we run out of instances of one before the other we send
2391 : // the relevant fields as 0.
2392 3299481 : void GCS_MAVLINK::send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff))
2393 : {
2394 3299481 : const AP_Baro &barometer = AP::baro();
2395 :
2396 3299481 : bool have_data = false;
2397 :
2398 3299481 : float press_abs = 0.0f;
2399 3299481 : int16_t temperature = 0; // Absolute pressure temperature
2400 3299481 : int16_t temperature_press_diff = 0; // Differential pressure temperature
2401 3299481 : if (instance < barometer.num_instances()) {
2402 2247577 : press_abs = barometer.get_pressure(instance) * 0.01f;
2403 2247577 : temperature = barometer.get_temperature(instance)*100;
2404 2247577 : have_data = true;
2405 : }
2406 :
2407 3299481 : float press_diff = 0; // pascal
2408 : #if AP_AIRSPEED_ENABLED
2409 3299481 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
2410 6593682 : if (airspeed != nullptr &&
2411 3294201 : airspeed->enabled(instance)) {
2412 576874 : press_diff = airspeed->get_differential_pressure(instance) * 0.01f;
2413 : float temp;
2414 576874 : if (airspeed->get_temperature(instance,temp)) {
2415 23148 : temperature_press_diff = temp * 100;
2416 23148 : if (temperature_press_diff == 0) {
2417 : // don't send zero as that is the value for 'no data'
2418 0 : temperature_press_diff = 1;
2419 : }
2420 : }
2421 576874 : have_data = true;
2422 : }
2423 : #endif
2424 :
2425 3299481 : if (!have_data) {
2426 1051799 : return;
2427 : }
2428 :
2429 2247682 : send_fn(
2430 : chan,
2431 : AP_HAL::millis(),
2432 : press_abs, // hectopascal
2433 : press_diff, // hectopascal
2434 : temperature, // 0.01 degrees C
2435 : temperature_press_diff); // 0.01 degrees C
2436 : }
2437 :
2438 1117303 : void GCS_MAVLINK::send_scaled_pressure()
2439 : {
2440 1117303 : send_scaled_pressure_instance(0, mavlink_msg_scaled_pressure_send);
2441 1117303 : }
2442 :
2443 1117088 : void GCS_MAVLINK::send_scaled_pressure2()
2444 : {
2445 1117088 : send_scaled_pressure_instance(1, mavlink_msg_scaled_pressure2_send);
2446 1117088 : }
2447 :
2448 1065090 : void GCS_MAVLINK::send_scaled_pressure3()
2449 : {
2450 1065090 : send_scaled_pressure_instance(2, mavlink_msg_scaled_pressure3_send);
2451 1065090 : }
2452 :
2453 : #if AP_AIRSPEED_ENABLED
2454 1117087 : void GCS_MAVLINK::send_airspeed()
2455 : {
2456 1117087 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
2457 1117087 : if (airspeed == nullptr) {
2458 1760 : return;
2459 : }
2460 :
2461 2769100 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
2462 : // Try and send the next sensor
2463 2214499 : const uint8_t index = (last_airspeed_idx + 1 + i) % AIRSPEED_MAX_SENSORS;
2464 2214499 : if (!airspeed->enabled(index)) {
2465 1653773 : continue;
2466 : }
2467 :
2468 : float temperature_float;
2469 560726 : int16_t temperature = INT16_MAX;
2470 560726 : if (airspeed->get_temperature(index, temperature_float)) {
2471 16453 : temperature = int16_t(temperature_float * 100);
2472 : }
2473 :
2474 560726 : uint8_t flags = 0;
2475 : // Set unhealthy flag
2476 560726 : if (!airspeed->healthy(index)) {
2477 3956 : flags |= AIRSPEED_SENSOR_FLAGS::AIRSPEED_SENSOR_UNHEALTHY;
2478 : }
2479 :
2480 : #if AP_AHRS_ENABLED
2481 : // Set using flag if the AHRS is using this sensor
2482 560726 : const AP_AHRS &ahrs = AP::ahrs();
2483 560726 : if (ahrs.using_airspeed_sensor() && (ahrs.get_active_airspeed_index() == index)) {
2484 526466 : flags |= AIRSPEED_SENSOR_FLAGS::AIRSPEED_SENSOR_USING;
2485 : }
2486 : #endif
2487 :
2488 : // Assemble message and send
2489 : const mavlink_airspeed_t msg {
2490 560726 : airspeed : airspeed->get_airspeed(index),
2491 560726 : raw_press : airspeed->get_differential_pressure(index),
2492 : temperature : temperature,
2493 : id : index,
2494 : flags : flags
2495 560726 : };
2496 :
2497 560726 : mavlink_msg_airspeed_send_struct(chan, &msg);
2498 :
2499 : // Only send one msg per call
2500 560726 : last_airspeed_idx = index;
2501 560726 : return;
2502 : }
2503 :
2504 : }
2505 : #endif // AP_AIRSPEED_ENABLED
2506 :
2507 : #if AP_AHRS_ENABLED
2508 1117757 : void GCS_MAVLINK::send_ahrs()
2509 : {
2510 1117757 : const AP_AHRS &ahrs = AP::ahrs();
2511 1117757 : const Vector3f &omega_I = ahrs.get_gyro_drift();
2512 1117757 : mavlink_msg_ahrs_send(
2513 : chan,
2514 1117757 : omega_I.x,
2515 1117757 : omega_I.y,
2516 1117757 : omega_I.z,
2517 : 0,
2518 : 0,
2519 : ahrs.get_error_rp(),
2520 : ahrs.get_error_yaw());
2521 1117757 : }
2522 : #endif // AP_AHRS_ENABLED
2523 :
2524 : /*
2525 : send a statustext text string to specific MAVLink bitmask
2526 : */
2527 262928 : void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, mavlink_channel_mask_t dest_bitmask)
2528 : {
2529 262928 : char first_piece_of_text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]{};
2530 :
2531 : do {
2532 : // send_text can be called from multiple threads; we must
2533 : // protect the "text" member with _statustext_sem
2534 262928 : WITH_SEMAPHORE(_statustext_queue.semaphore());
2535 262928 : hal.util->vsnprintf(statustext_printf_buffer, sizeof(statustext_printf_buffer), fmt, arg_list);
2536 262928 : memcpy(first_piece_of_text, statustext_printf_buffer, ARRAY_SIZE(first_piece_of_text)-1);
2537 :
2538 : // filter destination ports to only allow active ports.
2539 262928 : statustext_t statustext{};
2540 262928 : if (update_send_has_been_called) {
2541 262826 : statustext.bitmask = statustext_send_channel_mask();
2542 : } else {
2543 : // we have not yet initialised the streaming-channel-mask,
2544 : // which is done as part of the update() call. So just send
2545 : // it to all channels:
2546 102 : statustext.bitmask = (1<<_num_gcs)-1;
2547 : }
2548 262928 : statustext.bitmask &= dest_bitmask;
2549 262928 : if (!statustext.bitmask) {
2550 : // nowhere to send
2551 0 : break;
2552 : }
2553 :
2554 262928 : statustext.entry_created_ms = AP_HAL::millis16();
2555 262928 : statustext.msg.severity = severity;
2556 :
2557 : static uint16_t msgid;
2558 262928 : if (strlen(statustext_printf_buffer) > sizeof(statustext.msg.text)) {
2559 745 : msgid++;
2560 745 : if (msgid == 0) {
2561 0 : msgid = 1;
2562 : }
2563 745 : statustext.msg.id = msgid;
2564 : }
2565 :
2566 262928 : const char *remainder = statustext_printf_buffer;
2567 263679 : for (uint8_t i=0; i<_status_capacity; i++) {
2568 263679 : statustext.msg.chunk_seq = i;
2569 263679 : const size_t remainder_len = strlen(remainder);
2570 : // note that remainder_len may be zero here!
2571 263679 : uint16_t n = MIN(sizeof(statustext.msg.text), remainder_len);
2572 263679 : if (i == _status_capacity -1 && n == sizeof(statustext.msg.text)) {
2573 : // fantastic. This us a very long statustext and
2574 : // we've actually managed to push everything else out
2575 : // of the queue - this is the last chunk, so we MUST
2576 : // null-terminate.
2577 0 : n -= 1;
2578 : }
2579 263679 : memset(statustext.msg.text, '\0', sizeof(statustext.msg.text));
2580 263679 : memcpy(statustext.msg.text, remainder, n);
2581 : // The force push will ensure comm links do not block other comm links forever if they fail.
2582 : // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
2583 : // block but not until the buffer fills up.
2584 263679 : _statustext_queue.push_force(statustext);
2585 263679 : remainder = &remainder[n];
2586 :
2587 : // note that remainder_len here is the remainder length for
2588 : // the *old* remainder!
2589 263679 : if (remainder_len < sizeof(statustext.msg.text) || statustext.msg.id == 0) {
2590 : break;
2591 : }
2592 : }
2593 262928 : } while (false);
2594 :
2595 : #if HAL_LOGGING_ENABLED
2596 : // given we don't really know what these methods get up to, we
2597 : // don't hold the statustext semaphore while doing them:
2598 262928 : AP_Logger *logger = AP_Logger::get_singleton();
2599 262928 : if (logger != nullptr) {
2600 262928 : logger->Write_Message(first_piece_of_text);
2601 : }
2602 : #endif
2603 :
2604 : #if AP_FRSKY_TELEM_ENABLED
2605 262928 : frsky = AP::frsky_telem();
2606 262928 : if (frsky != nullptr) {
2607 164 : frsky->queue_message(severity, first_piece_of_text);
2608 : }
2609 : #endif
2610 : #if HAL_SPEKTRUM_TELEM_ENABLED
2611 262928 : AP_Spektrum_Telem* spektrum = AP::spektrum_telem();
2612 262928 : if (spektrum != nullptr) {
2613 0 : spektrum->queue_message(severity, first_piece_of_text);
2614 : }
2615 : #endif
2616 : #if HAL_CRSF_TELEM_ENABLED
2617 262928 : AP_CRSF_Telem* crsf = AP::crsf_telem();
2618 262928 : if (crsf != nullptr) {
2619 262928 : crsf->queue_message(severity, first_piece_of_text);
2620 : }
2621 : #endif
2622 262928 : AP_Notify *notify = AP_Notify::get_singleton();
2623 262928 : if (notify) {
2624 262928 : notify->send_text(first_piece_of_text);
2625 : }
2626 :
2627 : // push the messages out straight away until the vehicle states
2628 : // that it is initialised. At that point we can assume
2629 : // update_send is being called
2630 262928 : if (!vehicle_initialised()) {
2631 5678 : service_statustext();
2632 : }
2633 262928 : }
2634 :
2635 41936955 : void GCS::service_statustext(void)
2636 : {
2637 41936955 : WITH_SEMAPHORE(_statustext_queue.semaphore());
2638 :
2639 41936955 : if (_statustext_queue.is_empty()) {
2640 : // nothing to do
2641 36841031 : return;
2642 : }
2643 :
2644 15345735 : for (uint8_t i=first_backend_to_send; i<num_gcs(); i++) {
2645 10249811 : chan(i)->service_statustext();
2646 : }
2647 10244571 : for (uint8_t i=0; i<first_backend_to_send; i++) {
2648 5148647 : chan(i)->service_statustext();
2649 : }
2650 :
2651 5095924 : _statustext_queue.prune();
2652 41936955 : }
2653 :
2654 5095924 : void GCS::StatusTextQueue::prune(void)
2655 : {
2656 : // consider pruning the statustext queue of ancient entries
2657 5095924 : const uint32_t now_ms = AP_HAL::millis();
2658 5095924 : if (now_ms - last_prune_ms < 1000) {
2659 5050856 : return;
2660 : }
2661 45068 : last_prune_ms = now_ms;
2662 :
2663 45068 : const uint16_t now16_ms = AP_HAL::millis16();
2664 66584 : for (uint8_t idx=0; idx<available(); ) {
2665 53944 : const GCS::statustext_t *statustext = (*this)[idx];
2666 53944 : if (statustext == nullptr) {
2667 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2668 0 : break;
2669 : }
2670 : // be wary of integer promotion here
2671 53944 : const uint16_t age = now16_ms - statustext->entry_created_ms;
2672 53944 : if (age > 5000) {
2673 : // too old. Purge it.
2674 21516 : remove(idx);
2675 21516 : continue;
2676 : }
2677 : // this is a queue. If this one wasn't too old then the next
2678 : // one isn't either.
2679 32428 : break;
2680 : }
2681 : }
2682 :
2683 : /*
2684 : send a statustext message to specific MAVLink connections in a bitmask
2685 :
2686 : must be called with semaphore held
2687 : */
2688 15398458 : void GCS_MAVLINK::service_statustext(void)
2689 : {
2690 15398458 : GCS::StatusTextQueue &_statustext_queue = gcs().statustext_queue();
2691 :
2692 15398458 : const mavlink_channel_mask_t chan_bit = (1U<<chan);
2693 : // note the lack of idx++ here. We may remove the iteration item
2694 : // from the queue as the last thing we do, in which case we don't
2695 : // want to move idx.
2696 15398458 : const uint16_t payload_size = PAYLOAD_SIZE(chan, STATUSTEXT);
2697 36811251 : for (uint8_t idx=0; idx<_statustext_queue.available(); ) {
2698 31582848 : WITH_SEMAPHORE(comm_chan_lock(chan));
2699 :
2700 31582848 : if (txspace() < payload_size) {
2701 10170055 : break;
2702 : }
2703 21412793 : GCS::statustext_t *statustext = _statustext_queue[idx];
2704 21412793 : if (statustext == nullptr) {
2705 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2706 0 : break;
2707 : }
2708 :
2709 : // check to see if we need to send this queue entry:
2710 21412793 : if (statustext->bitmask & chan_bit) {
2711 133678 : mavlink_msg_statustext_send(chan,
2712 133678 : statustext->msg.severity,
2713 133678 : statustext->msg.text,
2714 133678 : statustext->msg.id,
2715 133678 : statustext->msg.chunk_seq);
2716 : // indicate we've sent the message:
2717 133678 : statustext->bitmask &= ~chan_bit;
2718 :
2719 133678 : if (statustext->bitmask == 0) {
2720 : // sent everywhere it needs to be sent, remove it from the
2721 : // queue but leave idx as-is as we want to handle the
2722 : // remaining items which have been bumped up to out
2723 : // current index
2724 101530 : _statustext_queue.remove(idx);
2725 101530 : continue;
2726 : }
2727 : }
2728 : // this item still has places to go. Continue to iterate over the queue
2729 21311263 : idx++;
2730 31582848 : }
2731 15398458 : }
2732 :
2733 26530 : void GCS::send_message(enum ap_message id)
2734 : {
2735 106108 : for (uint8_t i=0; i<num_gcs(); i++) {
2736 79578 : chan(i)->send_message(id);
2737 : }
2738 26530 : }
2739 :
2740 41931277 : void GCS::update_send()
2741 : {
2742 : // cope with changes to mavlink system ID parameter
2743 41931277 : mavlink_system.sysid = sysid;
2744 :
2745 41931277 : update_send_has_been_called = true;
2746 :
2747 41931277 : if (!initialised_missionitemprotocol_objects) {
2748 2094 : initialised_missionitemprotocol_objects = true;
2749 : // once-only initialisation of MissionItemProtocol objects:
2750 : #if AP_MISSION_ENABLED
2751 2094 : missionitemprotocols[MAV_MISSION_TYPE_MISSION] = NEW_NOTHROW MissionItemProtocol_Waypoints(AP::mission());
2752 : #endif
2753 : #if HAL_RALLY_ENABLED
2754 2094 : AP_Rally *rally = AP::rally();
2755 2094 : if (rally != nullptr) {
2756 2061 : missionitemprotocols[MAV_MISSION_TYPE_RALLY] = NEW_NOTHROW MissionItemProtocol_Rally(*rally);
2757 : }
2758 : #endif
2759 : #if AP_FENCE_ENABLED
2760 2094 : AC_Fence *fence = AP::fence();
2761 2094 : if (fence != nullptr) {
2762 2061 : missionitemprotocols[MAV_MISSION_TYPE_FENCE] = NEW_NOTHROW MissionItemProtocol_Fence(*fence);
2763 : }
2764 : #endif
2765 : }
2766 :
2767 167725108 : for (auto *prot : missionitemprotocols) {
2768 125793831 : if (prot == nullptr) {
2769 197318 : continue;
2770 : }
2771 125596513 : prot->update();
2772 : }
2773 :
2774 : // round-robin the GCS_MAVLINK backend that gets to go first so
2775 : // one backend doesn't monopolise all of the time allowed for sending
2776 : // messages
2777 126275558 : for (uint8_t i=first_backend_to_send; i<num_gcs(); i++) {
2778 84344281 : chan(i)->update_send();
2779 : }
2780 84341388 : for (uint8_t i=0; i<first_backend_to_send; i++) {
2781 42410111 : chan(i)->update_send();
2782 : }
2783 :
2784 41931277 : service_statustext();
2785 :
2786 41931277 : first_backend_to_send++;
2787 41931277 : if (first_backend_to_send >= num_gcs()) {
2788 13905159 : first_backend_to_send = 0;
2789 : }
2790 41931277 : }
2791 :
2792 41933097 : void GCS::update_receive(void)
2793 : {
2794 168688032 : for (uint8_t i=0; i<num_gcs(); i++) {
2795 126756574 : chan(i)->update_receive();
2796 : }
2797 : // also update UART pass-thru, if enabled
2798 41931458 : update_passthru();
2799 41931458 : }
2800 :
2801 1679 : void GCS::send_mission_item_reached_message(uint16_t mission_index)
2802 : {
2803 6722 : for (uint8_t i=0; i<num_gcs(); i++) {
2804 5043 : chan(i)->mission_item_reached_index = mission_index;
2805 5043 : chan(i)->send_message(MSG_MISSION_ITEM_REACHED);
2806 : }
2807 1679 : }
2808 :
2809 2094 : void GCS::setup_console()
2810 : {
2811 2094 : AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, 0);
2812 2094 : if (uart == nullptr) {
2813 : // this is probably not going to end well.
2814 0 : return;
2815 : }
2816 : if (ARRAY_SIZE(_chan_var_info) == 0) {
2817 : return;
2818 : }
2819 2094 : if (!create_gcs_mavlink_backend(*uart)) {
2820 0 : AP_BoardConfig::config_error("Failed to create MAVLink console backend");
2821 : }
2822 : }
2823 :
2824 :
2825 6340 : bool GCS::create_gcs_mavlink_backend(AP_HAL::UARTDriver &uart)
2826 : {
2827 6340 : if (_num_gcs >= ARRAY_SIZE(_chan_var_info)) {
2828 0 : return false;
2829 : }
2830 6340 : _chan[_num_gcs] = new_gcs_mavlink_backend(uart);
2831 6340 : if (_chan[_num_gcs] == nullptr) {
2832 0 : return false;
2833 : }
2834 :
2835 : // prepare parameters:
2836 6340 : _chan_var_info[_num_gcs] = _chan[_num_gcs]->var_info;
2837 6340 : AP_Param::load_object_from_eeprom(_chan[_num_gcs], _chan_var_info[_num_gcs]);
2838 :
2839 6340 : if (!_chan[_num_gcs]->init(_num_gcs)) {
2840 0 : delete _chan[_num_gcs];
2841 0 : _chan[_num_gcs] = nullptr;
2842 0 : return false;
2843 : }
2844 :
2845 6340 : _num_gcs++;
2846 6340 : return true;
2847 : }
2848 :
2849 2094 : void GCS::setup_uarts()
2850 : {
2851 : #if AP_FRSKY_TELEM_ENABLED
2852 2094 : if (frsky == nullptr) {
2853 2094 : frsky = NEW_NOTHROW AP_Frsky_Telem();
2854 2094 : if (frsky == nullptr || !frsky->init()) {
2855 2089 : delete frsky;
2856 2089 : frsky = nullptr;
2857 : }
2858 : }
2859 : #endif
2860 :
2861 : #if AP_LTM_TELEM_ENABLED
2862 2094 : ltm_telemetry.init();
2863 : #endif
2864 :
2865 : #if AP_DEVO_TELEM_ENABLED
2866 : devo_telemetry.init();
2867 : #endif
2868 :
2869 6340 : for (uint8_t i = 1; i < UINT8_MAX; i++) { // we really can't have this many serial connections, right?!
2870 6340 : AP_HAL::UARTDriver *uart = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_MAVLink, i);
2871 6340 : if (uart == nullptr) {
2872 : // no more mavlink uarts
2873 2094 : return;
2874 : }
2875 4246 : if (!create_gcs_mavlink_backend(*uart)) {
2876 0 : AP_BoardConfig::config_error("Failed to create MAVLink backend %u", unsigned(i));
2877 : }
2878 : }
2879 :
2880 : // no way we allocated UINT8_MAX backends, something has gone wrong
2881 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2882 : }
2883 :
2884 : /*
2885 : handle a SET_MODE MAVLink message
2886 : */
2887 16 : void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg)
2888 : {
2889 : mavlink_set_mode_t packet;
2890 16 : mavlink_msg_set_mode_decode(&msg, &packet);
2891 :
2892 16 : const uint8_t _base_mode = (uint8_t)packet.base_mode;
2893 16 : const uint32_t _custom_mode = packet.custom_mode;
2894 :
2895 16 : _set_mode_common(_base_mode, _custom_mode);
2896 16 : }
2897 :
2898 : /*
2899 : code common to both SET_MODE mavlink message and command long set_mode msg
2900 : */
2901 3329 : MAV_RESULT GCS_MAVLINK::_set_mode_common(const uint8_t _base_mode, const uint32_t _custom_mode)
2902 : {
2903 : // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes
2904 : #if AP_VEHICLE_ENABLED
2905 3329 : if ((_base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) != 0) {
2906 3313 : if (!AP::vehicle()->set_mode(_custom_mode, ModeReason::GCS_COMMAND)) {
2907 : // often we should be returning DENIED rather than FAILED
2908 : // here. Perhaps a "has_mode" callback on AP_::vehicle()
2909 : // would do?
2910 297 : return MAV_RESULT_FAILED;
2911 : }
2912 3016 : return MAV_RESULT_ACCEPTED;
2913 : }
2914 : #endif
2915 :
2916 16 : if (_base_mode == MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
2917 : // set the safety switch position. Must be in a command by itself
2918 16 : if (_custom_mode == 0) {
2919 : // turn safety off (pwm outputs flow to the motors)
2920 8 : hal.rcout->force_safety_off();
2921 8 : return MAV_RESULT_ACCEPTED;
2922 : }
2923 8 : if (_custom_mode == 1) {
2924 : // turn safety on (no pwm outputs to the motors)
2925 8 : if (hal.rcout->force_safety_on()) {
2926 8 : return MAV_RESULT_ACCEPTED;
2927 : }
2928 0 : return MAV_RESULT_FAILED;
2929 : }
2930 0 : return MAV_RESULT_DENIED;
2931 : }
2932 :
2933 : // Command is invalid (is supported but has invalid parameters)
2934 0 : return MAV_RESULT_DENIED;
2935 : }
2936 :
2937 : #if AP_OPTICALFLOW_ENABLED
2938 : /*
2939 : send OPTICAL_FLOW message
2940 : */
2941 1117755 : void GCS_MAVLINK::send_opticalflow()
2942 : {
2943 1117755 : const AP_OpticalFlow *optflow = AP::opticalflow();
2944 :
2945 : // exit immediately if no optical flow sensor or not healthy
2946 2233750 : if (optflow == nullptr ||
2947 1115995 : !optflow->healthy()) {
2948 1112687 : return;
2949 : }
2950 :
2951 : // get rates from sensor
2952 5068 : const Vector2f &flowRate = optflow->flowRate();
2953 5068 : const Vector2f &bodyRate = optflow->bodyRate();
2954 :
2955 5068 : float hagl = 0;
2956 : #if AP_AHRS_ENABLED
2957 5068 : if (!AP::ahrs().get_hagl(hagl)) {
2958 1384 : hagl = 0;
2959 : }
2960 : #endif
2961 :
2962 : // populate and send message
2963 5068 : mavlink_msg_optical_flow_send(
2964 : chan,
2965 5068 : AP_HAL::millis(),
2966 : 0, // sensor id is zero
2967 5068 : flowRate.x,
2968 5068 : flowRate.y,
2969 5068 : flowRate.x - bodyRate.x,
2970 5068 : flowRate.y - bodyRate.y,
2971 5068 : optflow->quality(),
2972 : hagl, // ground distance (in meters) set to zero
2973 5068 : flowRate.x,
2974 5068 : flowRate.y);
2975 : }
2976 : #endif // AP_OPTICALFLOW_ENABLED
2977 :
2978 : /*
2979 : send AUTOPILOT_VERSION packet
2980 : */
2981 30 : void GCS_MAVLINK::send_autopilot_version() const
2982 : {
2983 : uint32_t flight_sw_version;
2984 30 : uint32_t middleware_sw_version = 0;
2985 : #ifdef APJ_BOARD_ID
2986 30 : uint32_t board_version { uint32_t(APJ_BOARD_ID) << 16 };
2987 : #else
2988 0 : uint32_t board_version = 0;
2989 : #endif
2990 30 : char flight_custom_version[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_FLIGHT_CUSTOM_VERSION_LEN]{};
2991 30 : char middleware_custom_version[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_MIDDLEWARE_CUSTOM_VERSION_LEN]{};
2992 30 : char os_custom_version[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_OS_CUSTOM_VERSION_LEN]{};
2993 : #ifdef HAL_USB_VENDOR_ID
2994 : const uint16_t vendor_id { HAL_USB_VENDOR_ID };
2995 : const uint16_t product_id { HAL_USB_PRODUCT_ID };
2996 : #else
2997 30 : uint16_t vendor_id = 0;
2998 30 : uint16_t product_id = 0;
2999 : #endif
3000 30 : uint64_t uid = 0;
3001 30 : uint8_t uid2[MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_UID2_LEN] = {0};
3002 :
3003 30 : uint8_t uid_len = sizeof(uid2); // taken as reference and modified
3004 : // by following call:
3005 30 : hal.util->get_system_id_unformatted(uid2, uid_len);
3006 :
3007 30 : const AP_FWVersion &version = AP::fwversion();
3008 :
3009 30 : flight_sw_version = version.major << (8 * 3) | \
3010 30 : version.minor << (8 * 2) | \
3011 30 : version.patch << (8 * 1) | \
3012 30 : (uint32_t)(version.fw_type) << (8 * 0);
3013 :
3014 30 : if (version.fw_hash_str) {
3015 30 : strncpy_noterm(flight_custom_version, version.fw_hash_str, ARRAY_SIZE(flight_custom_version));
3016 : }
3017 :
3018 30 : if (version.middleware_hash_str) {
3019 0 : strncpy_noterm(middleware_custom_version, version.middleware_hash_str, ARRAY_SIZE(middleware_custom_version));
3020 : }
3021 :
3022 30 : if (version.os_hash_str) {
3023 0 : strncpy_noterm(os_custom_version, version.os_hash_str, ARRAY_SIZE(os_custom_version));
3024 : }
3025 :
3026 60 : mavlink_msg_autopilot_version_send(
3027 30 : chan,
3028 30 : capabilities(),
3029 : flight_sw_version,
3030 : middleware_sw_version,
3031 30 : version.os_sw_version,
3032 : board_version,
3033 : (uint8_t *)flight_custom_version,
3034 : (uint8_t *)middleware_custom_version,
3035 : (uint8_t *)os_custom_version,
3036 : vendor_id,
3037 : product_id,
3038 : uid,
3039 : uid2
3040 : );
3041 30 : }
3042 :
3043 :
3044 : #if AP_AHRS_ENABLED
3045 : /*
3046 : send LOCAL_POSITION_NED message
3047 : */
3048 1119336 : void GCS_MAVLINK::send_local_position() const
3049 : {
3050 1119336 : const AP_AHRS &ahrs = AP::ahrs();
3051 :
3052 1119336 : Vector3f local_position, velocity;
3053 2067649 : if (!ahrs.get_relative_position_NED_origin_float(local_position) ||
3054 948313 : !ahrs.get_velocity_NED(velocity)) {
3055 : // we don't know the position and velocity
3056 171236 : return;
3057 : }
3058 :
3059 948100 : mavlink_msg_local_position_ned_send(
3060 948100 : chan,
3061 : AP_HAL::millis(),
3062 : local_position.x,
3063 : local_position.y,
3064 : local_position.z,
3065 : velocity.x,
3066 : velocity.y,
3067 : velocity.z);
3068 : }
3069 : #endif
3070 :
3071 : /*
3072 : send VIBRATION message
3073 : */
3074 1115995 : void GCS_MAVLINK::send_vibration() const
3075 : {
3076 : #if AP_INERTIALSENSOR_ENABLED
3077 1115995 : const AP_InertialSensor &ins = AP::ins();
3078 :
3079 1115995 : Vector3f vibration = ins.get_vibration_levels();
3080 :
3081 1115995 : mavlink_msg_vibration_send(
3082 1115995 : chan,
3083 : AP_HAL::micros64(),
3084 : vibration.x,
3085 : vibration.y,
3086 : vibration.z,
3087 : ins.get_accel_clip_count(0),
3088 : ins.get_accel_clip_count(1),
3089 : ins.get_accel_clip_count(2));
3090 : #endif
3091 1115995 : }
3092 :
3093 468621 : void GCS_MAVLINK::send_named_float(const char *name, float value) const
3094 : {
3095 468621 : char float_name[MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN+1] {};
3096 468621 : strncpy(float_name, name, MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN);
3097 468621 : mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value);
3098 468621 : }
3099 :
3100 0 : void GCS_MAVLINK::send_named_int(const char *name, int32_t value) const
3101 : {
3102 0 : char int_name[MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN+1] {};
3103 0 : strncpy(int_name, name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN);
3104 0 : mavlink_msg_named_value_int_send(chan, AP_HAL::millis(), int_name, value);
3105 0 : }
3106 :
3107 : #if AP_AHRS_ENABLED
3108 11357 : void GCS_MAVLINK::send_home_position() const
3109 : {
3110 11357 : if (!AP::ahrs().home_is_set()) {
3111 0 : return;
3112 : }
3113 :
3114 11357 : const Location &home = AP::ahrs().get_home();
3115 :
3116 : // get home position from origin
3117 11357 : Vector3f home_pos_ned;
3118 11357 : if (home.get_vector_from_origin_NEU_cm(home_pos_ned)) {
3119 : // convert NEU in cm to NED in meters
3120 10643 : home_pos_ned *= 0.01f;
3121 10643 : home_pos_ned.z *= -1;
3122 : } else {
3123 714 : home_pos_ned = Vector3f{NAN, NAN, NAN};
3124 : }
3125 :
3126 11357 : const float q[4] {NAN, NAN, NAN, NAN};
3127 11357 : mavlink_msg_home_position_send(
3128 11357 : chan,
3129 11357 : home.lat,
3130 11357 : home.lng,
3131 11357 : home.alt * 10,
3132 : home_pos_ned.x,
3133 : home_pos_ned.y,
3134 : home_pos_ned.z,
3135 : q,
3136 : 0.0f, 0.0f, 0.0f,
3137 : AP_HAL::micros64());
3138 : }
3139 :
3140 11850 : void GCS_MAVLINK::send_gps_global_origin() const
3141 : {
3142 11850 : Location ekf_origin;
3143 11850 : if (!AP::ahrs().get_origin(ekf_origin)) {
3144 716 : return;
3145 : }
3146 11134 : mavlink_msg_gps_global_origin_send(
3147 11134 : chan,
3148 : ekf_origin.lat,
3149 : ekf_origin.lng,
3150 11134 : ekf_origin.alt * 10,
3151 : AP_HAL::micros64());
3152 : }
3153 : #endif // AP_AHRS_ENABLED
3154 :
3155 170070 : MAV_STATE GCS_MAVLINK::system_status() const
3156 : {
3157 170070 : MAV_STATE _system_status = vehicle_system_status();
3158 170070 : if (_system_status < MAV_STATE_CRITICAL) {
3159 : // note that POWEROFF and FLIGHT_TERMINATION are both >
3160 : // CRITICAL, so we will not overwrite POWEROFF and
3161 : // FLIGHT_TERMINATION even if we have internal errors. If new
3162 : // enum entries are added then this may also not overwrite
3163 : // those.
3164 166889 : if (AP::internalerror().errors()) {
3165 21 : _system_status = MAV_STATE_CRITICAL;
3166 : }
3167 : }
3168 170070 : return _system_status;
3169 : }
3170 :
3171 : /*
3172 : Send MAVLink heartbeat
3173 : */
3174 170070 : void GCS_MAVLINK::send_heartbeat() const
3175 : {
3176 680280 : mavlink_msg_heartbeat_send(
3177 170070 : chan,
3178 170070 : gcs().frame_type(),
3179 : MAV_AUTOPILOT_ARDUPILOTMEGA,
3180 170070 : base_mode(),
3181 170070 : gcs().custom_mode(),
3182 170070 : system_status());
3183 170070 : }
3184 :
3185 : #if AP_RC_CHANNEL_ENABLED
3186 25 : MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int_t &packet)
3187 : {
3188 25 : if (packet.param2 > 2) {
3189 2 : return MAV_RESULT_DENIED;
3190 : }
3191 23 : const RC_Channel::AUX_FUNC aux_func = (RC_Channel::AUX_FUNC)packet.param1;
3192 23 : const RC_Channel::AuxSwitchPos position = (RC_Channel::AuxSwitchPos)packet.param2;
3193 23 : if (!rc().run_aux_function(aux_func, position, RC_Channel::AuxFuncTrigger::Source::MAVLINK, chan)) {
3194 : // note that this is not quite right; we could be more nuanced
3195 : // about our return code here.
3196 2 : return MAV_RESULT_FAILED;
3197 : }
3198 21 : return MAV_RESULT_ACCEPTED;
3199 : }
3200 : #endif // AP_RC_CHANNEL_ENABLED
3201 :
3202 216 : MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_int_t &packet)
3203 : {
3204 216 : if (!is_zero(packet.param3)) {
3205 0 : return MAV_RESULT_DENIED;
3206 : }
3207 216 : return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2);
3208 : }
3209 :
3210 220 : MAV_RESULT GCS_MAVLINK::set_message_interval(uint32_t msg_id, int32_t interval_us)
3211 : {
3212 220 : const ap_message id = mavlink_id_to_ap_message_id(msg_id);
3213 220 : if (id == MSG_LAST) {
3214 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
3215 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No ap_message for mavlink id (%u)", (unsigned int)msg_id);
3216 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
3217 0 : return MAV_RESULT_DENIED;
3218 : }
3219 :
3220 : uint16_t interval_ms;
3221 220 : if (interval_us == 0) {
3222 : // zero is "reset to default rate"
3223 43 : if (!get_default_interval_for_ap_message(id, interval_ms)) {
3224 : // if we don't have a default interval then we assume that
3225 : // we do not send that message by default. That may not
3226 : // be strictly true if some random piece of code has set a
3227 : // rate as part of its initialisation - in which case that
3228 : // piece of code should probably be adding something into
3229 : // whatever get_default_interval_for_ap_message is looking
3230 : // at.
3231 36 : interval_ms = 0;
3232 : }
3233 177 : } else if (interval_us == -1) {
3234 : // minus-one is "stop sending"
3235 11 : interval_ms = 0;
3236 166 : } else if (interval_us < 0) {
3237 0 : return MAV_RESULT_DENIED;
3238 166 : } else if (interval_us < 1000) {
3239 : // don't squash sub-ms times to zero
3240 0 : interval_ms = 1;
3241 166 : } else if (interval_us > 60000000) {
3242 0 : interval_ms = 60000;
3243 : } else {
3244 166 : interval_ms = interval_us / 1000;
3245 : }
3246 : #if AP_SCHEDULER_ENABLED
3247 220 : if (interval_ms != 0 && cap_message_interval(interval_ms) > interval_ms) {
3248 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Requested rate for message ID %u too fast. Increase SCHED_LOOP_RATE", (unsigned int)msg_id);
3249 1 : return MAV_RESULT_DENIED;
3250 : }
3251 : #endif
3252 219 : if (set_ap_message_interval(id, interval_ms)) {
3253 219 : return MAV_RESULT_ACCEPTED;
3254 : }
3255 :
3256 0 : return MAV_RESULT_FAILED;
3257 : }
3258 :
3259 : /*
3260 : this function is reserved for use by scripting
3261 : */
3262 0 : MAV_RESULT GCS::set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us)
3263 : {
3264 0 : uint8_t channel = get_channel_from_port_number(port_num);
3265 :
3266 0 : GCS_MAVLINK *link = chan(channel);
3267 0 : if (link == nullptr) {
3268 0 : return MAV_RESULT_FAILED;
3269 : }
3270 0 : return link->set_message_interval(msg_id, interval_us);
3271 : }
3272 :
3273 0 : uint8_t GCS::get_channel_from_port_number(uint8_t port_num)
3274 : {
3275 0 : const AP_HAL::UARTDriver *u = AP::serialmanager().get_serial_by_id(port_num);
3276 0 : for (uint8_t i=0; i<num_gcs(); i++) {
3277 0 : if (chan(i)->get_uart() == u) {
3278 0 : return i;
3279 : }
3280 : }
3281 :
3282 0 : return UINT8_MAX;
3283 : }
3284 :
3285 450566 : MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int_t &packet)
3286 : {
3287 450566 : const uint32_t mavlink_id = (uint32_t)packet.param1;
3288 :
3289 450566 : if (mavlink_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
3290 2 : const mavlink_command_int_t msg_interval_cmd = { .param1 = packet.param2, };
3291 2 : return handle_command_get_message_interval(msg_interval_cmd);
3292 : }
3293 :
3294 450564 : const ap_message id = mavlink_id_to_ap_message_id(mavlink_id);
3295 450564 : if (id == MSG_LAST) {
3296 0 : return MAV_RESULT_FAILED;
3297 : }
3298 :
3299 450564 : switch(id) {
3300 0 : case MSG_AVAILABLE_MODES:
3301 0 : available_modes.requested = true;
3302 0 : available_modes.should_send = true;
3303 0 : available_modes.next_index = 1;
3304 0 : available_modes.requested_index = (uint8_t)packet.param2;
3305 0 : break;
3306 :
3307 : #if AP_CAMERA_ENABLED
3308 13 : case MSG_CAMERA_INFORMATION:
3309 : // param2 selects a specific camera instance (1-based); 0 means all
3310 13 : if (packet.param2 >= 1) {
3311 8 : AP_Camera *camera = AP::camera();
3312 8 : if (camera != nullptr) {
3313 8 : camera->set_camera_information_send_instance((int16_t)(packet.param2 - 1));
3314 : }
3315 : }
3316 13 : break;
3317 : #endif
3318 :
3319 450551 : default:
3320 450551 : break;
3321 : }
3322 :
3323 450564 : send_message(id);
3324 450564 : return MAV_RESULT_ACCEPTED;
3325 : }
3326 :
3327 510129 : bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms) const
3328 : {
3329 : // check if it's a specially-handled message:
3330 510129 : const int8_t deferred_offset = get_deferred_message_index(id);
3331 510129 : if (deferred_offset != -1) {
3332 6 : interval_ms = deferred_message[deferred_offset].interval_ms;
3333 6 : return true;
3334 : }
3335 :
3336 : // check the deferred message buckets:
3337 634337 : for (uint8_t i=0; i<ARRAY_SIZE(deferred_message_bucket); i++) {
3338 634332 : const deferred_message_bucket_t &bucket = deferred_message_bucket[i];
3339 634332 : if (bucket.ap_message_ids.get(id)) {
3340 510118 : interval_ms = bucket.interval_ms;
3341 510118 : return true;
3342 : }
3343 : }
3344 :
3345 5 : return false;
3346 : }
3347 :
3348 16 : MAV_RESULT GCS_MAVLINK::handle_command_get_message_interval(const mavlink_command_int_t &packet)
3349 : {
3350 16 : if (txspace() < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) {
3351 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
3352 : }
3353 :
3354 16 : const uint32_t mavlink_id = (uint32_t)packet.param1;
3355 16 : if (mavlink_id >= 2 << 15) {
3356 : // response packet limits range this works against!
3357 0 : mavlink_msg_message_interval_send(chan, mavlink_id, 0); // not available
3358 0 : return MAV_RESULT_FAILED;
3359 : }
3360 :
3361 16 : const ap_message id = mavlink_id_to_ap_message_id(mavlink_id);
3362 16 : if (id == MSG_LAST) {
3363 1 : mavlink_msg_message_interval_send(chan, mavlink_id, 0); // not available
3364 1 : return MAV_RESULT_FAILED;
3365 : }
3366 :
3367 15 : uint16_t interval_ms = 0;
3368 15 : if (!get_ap_message_interval(id, interval_ms) || interval_ms == 0) {
3369 : // not streaming this message at the moment...
3370 5 : mavlink_msg_message_interval_send(chan, mavlink_id, -1); // disabled
3371 5 : return MAV_RESULT_ACCEPTED;
3372 : }
3373 :
3374 10 : mavlink_msg_message_interval_send(chan, mavlink_id, interval_ms * 1000);
3375 10 : return MAV_RESULT_ACCEPTED;
3376 : }
3377 :
3378 :
3379 : // are we still delaying telemetry to try to avoid Xbee bricking?
3380 141783846 : bool GCS_MAVLINK::telemetry_delayed() const
3381 : {
3382 141783846 : uint32_t tnow = AP_HAL::millis() >> 10;
3383 141783846 : if (tnow >= gcs().telem_delay()) {
3384 141783846 : return false;
3385 : }
3386 0 : if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) {
3387 : // this is USB telemetry, so won't be an Xbee
3388 0 : return false;
3389 : }
3390 : // we're either on the 2nd UART, or no USB cable is connected
3391 : // we need to delay telemetry by the TELEM_DELAY time
3392 0 : return true;
3393 : }
3394 :
3395 :
3396 : /*
3397 : send SERVO_OUTPUT_RAW
3398 : */
3399 1192240 : void GCS_MAVLINK::send_servo_output_raw()
3400 : {
3401 1192240 : const uint32_t enabled_mask = ~SRV_Channels::get_output_channel_mask(SRV_Channel::k_GPIO);
3402 1192240 : if (enabled_mask == 0) {
3403 0 : return;
3404 : }
3405 :
3406 : #if NUM_SERVO_CHANNELS >= 17
3407 : static const uint8_t max_channels = 32;
3408 : #else
3409 : static const uint8_t max_channels = 16;
3410 : #endif
3411 :
3412 1192240 : uint16_t values[max_channels] {};
3413 1192240 : hal.rcout->read(values, max_channels);
3414 39343920 : for (uint8_t i=0; i<max_channels; i++) {
3415 38151680 : if (values[i] == 65535) {
3416 0 : values[i] = 0;
3417 : }
3418 : }
3419 1192240 : if ((enabled_mask & 0xFFFF) != 0) {
3420 1192240 : mavlink_msg_servo_output_raw_send(
3421 : chan,
3422 : AP_HAL::micros(),
3423 : 0, // port
3424 1192240 : values[0], values[1], values[2], values[3],
3425 1192240 : values[4], values[5], values[6], values[7],
3426 1192240 : values[8], values[9], values[10], values[11],
3427 1192240 : values[12], values[13], values[14], values[15]);
3428 : }
3429 :
3430 : #if NUM_SERVO_CHANNELS >= 17
3431 1192240 : if ((enabled_mask & 0xFFFF0000) != 0) {
3432 712 : mavlink_msg_servo_output_raw_send(
3433 : chan,
3434 : AP_HAL::micros(),
3435 : 1, // port
3436 712 : values[16], values[17], values[18], values[19],
3437 712 : values[20], values[21], values[22], values[23],
3438 712 : values[24], values[25], values[26], values[27],
3439 712 : values[28], values[29], values[30], values[31]);
3440 : }
3441 : #endif
3442 : }
3443 :
3444 :
3445 65 : void GCS_MAVLINK::send_accelcal_vehicle_position(uint32_t position)
3446 : {
3447 65 : if (HAVE_PAYLOAD_SPACE(chan, COMMAND_LONG)) {
3448 59 : mavlink_msg_command_long_send(
3449 : chan,
3450 : 0,
3451 : 0,
3452 : MAV_CMD_ACCELCAL_VEHICLE_POS,
3453 : 0,
3454 : (float) position,
3455 : 0, 0, 0, 0, 0, 0);
3456 : }
3457 65 : }
3458 :
3459 :
3460 181416 : float GCS_MAVLINK::vfr_hud_airspeed() const
3461 : {
3462 : #if AP_AIRSPEED_ENABLED
3463 181416 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
3464 181416 : if (airspeed != nullptr && airspeed->healthy()) {
3465 5528 : return airspeed->get_airspeed();
3466 : }
3467 : #endif
3468 :
3469 : #if AP_GPS_ENABLED
3470 : // because most vehicles don't have airspeed sensors, we return a
3471 : // different sort of speed estimate in the relevant field for
3472 : // comparison's sake.
3473 175888 : return AP::gps().ground_speed();
3474 : #endif
3475 :
3476 : return 0.0;
3477 : }
3478 :
3479 1105338 : float GCS_MAVLINK::vfr_hud_climbrate() const
3480 : {
3481 : #if AP_AHRS_ENABLED
3482 1105338 : Vector3f velned;
3483 1150402 : if (AP::ahrs().get_velocity_NED(velned) ||
3484 45064 : AP::ahrs().get_vert_pos_rate_D(velned.z)) {
3485 1104642 : return -velned.z;
3486 : }
3487 : #endif
3488 696 : return 0;
3489 : }
3490 :
3491 : #if AP_AHRS_ENABLED
3492 1063138 : float GCS_MAVLINK::vfr_hud_alt() const
3493 : {
3494 1063138 : return global_position_current_loc.alt * 0.01f; // cm -> m
3495 : }
3496 :
3497 1119088 : void GCS_MAVLINK::send_vfr_hud()
3498 : {
3499 1119088 : AP_AHRS &ahrs = AP::ahrs();
3500 :
3501 : // return values ignored; we send stale data
3502 1119088 : UNUSED_RESULT(ahrs.get_location(global_position_current_loc));
3503 :
3504 2238176 : mavlink_msg_vfr_hud_send(
3505 : chan,
3506 1119088 : vfr_hud_airspeed(),
3507 : ahrs.groundspeed(),
3508 1119088 : ahrs.get_yaw_deg(),
3509 1119088 : abs(vfr_hud_throttle()),
3510 1119088 : vfr_hud_alt(),
3511 1119088 : vfr_hud_climbrate());
3512 1119088 : }
3513 : #endif // AP_AHRS_ENABLED
3514 :
3515 : /*
3516 : handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command
3517 :
3518 : Optionally disable PX4IO overrides. This is done for quadplanes to
3519 : prevent the mixer running while rebooting which can start the VTOL
3520 : motors. That can be dangerous when a preflight reboot is done with
3521 : the pilot close to the aircraft and can also damage the aircraft
3522 : */
3523 1640 : MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
3524 : {
3525 1640 : if ((packet.target_component != MAV_COMP_ID_ALL) && (packet.target_component != mavlink_system.compid)) {
3526 : // Make sure reboot/shutdown commands sent to a component don't cause the autopilot to reboot.
3527 1 : return MAV_RESULT_DENIED;
3528 : }
3529 :
3530 1639 : if (is_equal(packet.param1, 42.0f) &&
3531 1639 : is_equal(packet.param2, 24.0f) &&
3532 0 : is_equal(packet.param3, 71.0f)) {
3533 : #if AP_MAVLINK_FAILURE_CREATION_ENABLED
3534 0 : if (is_equal(packet.param4, 93.0f)) {
3535 : // this is a magic sequence to force the main loop to
3536 : // lockup. This is for testing the stm32 watchdog
3537 : // functionality
3538 : while (true) {
3539 0 : send_text(MAV_SEVERITY_WARNING,"entering lockup");
3540 0 : hal.scheduler->delay(250);
3541 : }
3542 : }
3543 0 : if (is_equal(packet.param4, 94.0f)) {
3544 : // the following text is unlikely to make it out...
3545 0 : send_text(MAV_SEVERITY_WARNING,"dereferencing a bad thing");
3546 :
3547 : #if CONFIG_HAL_BOARD != HAL_BOARD_ESP32
3548 : // esp32 can't do this bit, skip it, return an error
3549 0 : void *foo = (void*)0xE000ED38;
3550 :
3551 : typedef void (*fptr)();
3552 0 : fptr gptr = (fptr) (void *) foo;
3553 0 : gptr();
3554 : #endif
3555 0 : return MAV_RESULT_FAILED;
3556 : }
3557 0 : if (is_equal(packet.param4, 95.0f)) {
3558 : // the following text is unlikely to make it out...
3559 0 : send_text(MAV_SEVERITY_WARNING,"calling AP_HAL::panic(...)");
3560 :
3561 0 : AP_HAL::panic("panicing");
3562 :
3563 : // keep calm and carry on
3564 : }
3565 0 : if (is_equal(packet.param4, 96.0f)) {
3566 : // deliberately corrupt parameter storage
3567 0 : send_text(MAV_SEVERITY_WARNING,"wiping parameter storage header");
3568 0 : StorageAccess param_storage{StorageManager::StorageParam};
3569 0 : uint8_t zeros[40] {};
3570 0 : param_storage.write_block(0, zeros, sizeof(zeros));
3571 0 : return MAV_RESULT_ACCEPTED;
3572 : }
3573 0 : if (is_equal(packet.param4, 97.0f)) {
3574 : // create a really long loop
3575 0 : send_text(MAV_SEVERITY_WARNING,"Creating long loop");
3576 : // 250ms:
3577 0 : for (uint8_t i=0; i<250; i++) {
3578 0 : hal.scheduler->delay_microseconds(1000);
3579 : }
3580 0 : return MAV_RESULT_ACCEPTED;
3581 : }
3582 0 : if (is_equal(packet.param4, 98.0f)) {
3583 0 : send_text(MAV_SEVERITY_WARNING,"Creating internal error");
3584 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
3585 0 : return MAV_RESULT_ACCEPTED;
3586 : }
3587 0 : if (is_equal(packet.param4, 100.0f)) {
3588 0 : send_text(MAV_SEVERITY_WARNING,"Creating mutex deadlock");
3589 0 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&GCS_MAVLINK::deadlock_sem, void));
3590 0 : while (!_deadlock_sem.taken) {
3591 0 : hal.scheduler->delay(1);
3592 : }
3593 0 : WITH_SEMAPHORE(_deadlock_sem.sem);
3594 0 : send_text(MAV_SEVERITY_WARNING,"deadlock passed");
3595 0 : return MAV_RESULT_ACCEPTED;
3596 0 : }
3597 0 : if (is_equal(packet.param4, 101.0f)) {
3598 : // the capital-U and ~ here are actually important for
3599 : // testing a MissionPlanner bug!
3600 0 : AP_BoardConfig::config_error("YOU~RE WELCOME!");
3601 : }
3602 0 : if (is_equal(packet.param4, 102.0f)) {
3603 : // attempt to write to address 0x5 (in the bottom 1kB on H7)
3604 : // which we either memory-protect or check for
3605 : // non-zeroness. We don't want to use 0x0 as that *even
3606 : // more magic*. So choose an offset which looks like
3607 : // we're dereferencing nullptr:
3608 0 : uint8_t *foo = (uint8_t*)0x05;
3609 :
3610 : #pragma GCC diagnostic push
3611 : #pragma GCC diagnostic ignored "-Warray-bounds"
3612 : #if !defined(__clang__) // avoid -Wunknown-warning-option
3613 : #pragma GCC diagnostic ignored "-Wstringop-overflow"
3614 : #endif
3615 0 : *foo = 0xab;
3616 : #pragma GCC diagnostic pop
3617 :
3618 0 : return MAV_RESULT_ACCEPTED;
3619 : }
3620 0 : if (is_equal(packet.param4, 103.0f)) {
3621 : // attempt to read from address 0x5 (in the bottom 1kB on
3622 : // H7) which we either memory-protect or check for
3623 : // non-zeroness. We don't want to use 0x0 as that *even
3624 : // more magic*. So choose an offset which looks like
3625 : // we're dereferencing nullptr:
3626 0 : uint8_t *foo = (uint8_t*)0x05;
3627 :
3628 : // we use send_text here to ensure we don't get elided.
3629 : // String is kept short for space reasons.
3630 : #pragma GCC diagnostic push
3631 : #pragma GCC diagnostic ignored "-Warray-bounds"
3632 : #if !defined(__clang__) // avoid -Wunknown-warning-option
3633 : #pragma GCC diagnostic ignored "-Wstringop-overflow"
3634 : #endif
3635 0 : send_text(MAV_SEVERITY_INFO, "x: %u", (unsigned)*foo);
3636 : #pragma GCC diagnostic pop
3637 :
3638 0 : return MAV_RESULT_ACCEPTED;
3639 : }
3640 : #endif // AP_MAVLINK_FAILURE_CREATION_ENABLED
3641 :
3642 : #if HAL_ENABLE_DFU_BOOT
3643 : if (is_equal(packet.param4, 99.0f)) {
3644 : #if AP_SIGNED_FIRMWARE
3645 : send_text(MAV_SEVERITY_ERROR, "Refusing DFU for secure firmware");
3646 : return MAV_RESULT_FAILED;
3647 : #else
3648 : send_text(MAV_SEVERITY_WARNING, "Entering DFU mode");
3649 : hal.util->boot_to_dfu();
3650 : return MAV_RESULT_ACCEPTED;
3651 : #endif
3652 : }
3653 : #endif
3654 : }
3655 :
3656 1639 : const bool reboot = is_equal(packet.param1, static_cast<float>(REBOOT_SHUTDOWN_ACTION_REBOOT));
3657 1639 : const bool reboot_to_bootloader = is_equal(packet.param1,
3658 : static_cast<float>(REBOOT_SHUTDOWN_ACTION_REBOOT_TO_BOOTLOADER));
3659 :
3660 : #if AP_REBOOT_MASS_STORAGE_ENABLED
3661 : const bool reboot_to_mass_storage = is_equal(packet.param1,
3662 : static_cast<float>(REBOOT_SHUTDOWN_ACTION_REBOOT_TO_MASS_STORAGE));
3663 :
3664 : // exporting writable storage must never be entered while armed
3665 : if (reboot_to_mass_storage && hal.util->get_soft_armed()) {
3666 : return MAV_RESULT_FAILED;
3667 : }
3668 : #endif
3669 :
3670 : // refuse reboot when armed:
3671 1639 : if (hal.util->get_soft_armed()) {
3672 : /// but allow it if forced:
3673 6 : const uint32_t magic_force_reboot_value = 20190226;
3674 6 : if (packet.y != magic_force_reboot_value) {
3675 0 : return MAV_RESULT_FAILED;
3676 : }
3677 : }
3678 :
3679 : #if AP_REBOOT_MASS_STORAGE_ENABLED
3680 : const bool supported_reboot_action = reboot || reboot_to_bootloader || reboot_to_mass_storage;
3681 : #else
3682 1639 : const bool supported_reboot_action = reboot || reboot_to_bootloader;
3683 : #endif
3684 1639 : if (!supported_reboot_action) {
3685 : // param1 must select a supported reboot action
3686 0 : return MAV_RESULT_UNSUPPORTED;
3687 : }
3688 :
3689 : #if AP_REBOOT_MASS_STORAGE_ENABLED
3690 : if (reboot_to_mass_storage && !hal.util->request_usb_msd()) {
3691 : return MAV_RESULT_UNSUPPORTED;
3692 : }
3693 : #endif
3694 :
3695 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
3696 : { // autotest relies in receiving the ACK for the reboot. Ensure
3697 : // there is space for it:
3698 1639 : const uint32_t tstart_ms = AP_HAL::millis();
3699 1639 : while (AP_HAL::millis() - tstart_ms < 1000) {
3700 1639 : if (HAVE_PAYLOAD_SPACE(chan, COMMAND_ACK)) {
3701 1639 : break;
3702 : }
3703 0 : hal.scheduler->delay(10);
3704 : }
3705 : }
3706 : #endif
3707 :
3708 : // send ack before we reboot
3709 1639 : mavlink_msg_command_ack_send(chan, packet.command, MAV_RESULT_ACCEPTED,
3710 : 0, 0,
3711 1639 : msg.sysid,
3712 1639 : msg.compid);
3713 :
3714 : #if AP_VEHICLE_ENABLED
3715 1639 : AP::vehicle()->reboot(reboot_to_bootloader); // not expected to return
3716 : #else
3717 : hal.scheduler->reboot(reboot_to_bootloader);
3718 : #endif
3719 :
3720 0 : return MAV_RESULT_FAILED;
3721 : }
3722 :
3723 : #if AP_MAVLINK_FAILURE_CREATION_ENABLED
3724 : /*
3725 : take a semaphore and do not release it, triggering a deadlock
3726 : */
3727 0 : void GCS_MAVLINK::deadlock_sem(void)
3728 : {
3729 0 : if (!_deadlock_sem.taken) {
3730 0 : _deadlock_sem.taken = true;
3731 0 : _deadlock_sem.sem.take_blocking();
3732 : }
3733 0 : }
3734 : #endif
3735 :
3736 : /*
3737 : handle a flight termination request
3738 : */
3739 5 : MAV_RESULT GCS_MAVLINK::handle_flight_termination(const mavlink_command_int_t &packet)
3740 : {
3741 : #if AP_ADVANCEDFAILSAFE_ENABLED
3742 5 : AP_AdvancedFailsafe *failsafe = AP::advancedfailsafe();
3743 5 : if (failsafe == nullptr) {
3744 0 : return MAV_RESULT_UNSUPPORTED;
3745 : }
3746 :
3747 5 : bool should_terminate = packet.param1 > 0.5f;
3748 :
3749 5 : if (failsafe->gcs_terminate(should_terminate, "GCS request")) {
3750 5 : return MAV_RESULT_ACCEPTED;
3751 : }
3752 0 : return MAV_RESULT_FAILED;
3753 : #else
3754 : return MAV_RESULT_UNSUPPORTED;
3755 : #endif
3756 : }
3757 :
3758 : #if AP_RCPROTOCOL_ENABLED
3759 : /*
3760 : handle a R/C bind request (for spektrum)
3761 : */
3762 0 : MAV_RESULT GCS_MAVLINK::handle_START_RX_PAIR(const mavlink_command_int_t &packet)
3763 : {
3764 : // initiate bind procedure. We houls accept the DSM type from either
3765 : // param1 or param2 due to a past mixup with what parameter is the
3766 : // right one: const int dsmMode = (packet.param2 > 0) ? packet.param2 : packet.param1;
3767 0 : AP::RC().start_bind();
3768 :
3769 0 : return MAV_RESULT_ACCEPTED;
3770 : }
3771 : #endif // AP_RCPROTOCOL_ENABLED
3772 :
3773 14853 : uint64_t GCS_MAVLINK::timesync_receive_timestamp_ns() const
3774 : {
3775 14853 : uint64_t ret = _port->receive_time_constraint_us(PAYLOAD_SIZE(chan, TIMESYNC));
3776 14853 : if (ret == 0) {
3777 0 : ret = AP_HAL::micros64();
3778 : }
3779 14853 : return ret*1000LL;
3780 : }
3781 :
3782 15191 : uint64_t GCS_MAVLINK::timesync_timestamp_ns() const
3783 : {
3784 : // we add in our own system id try to ensure we only consider
3785 : // responses to our own timesync request messages
3786 15191 : return AP_HAL::micros64()*1000LL + mavlink_system.sysid;
3787 : }
3788 :
3789 : /*
3790 : return a timesync request
3791 : Sends back ts1 as received, and tc1 is the local timestamp in usec
3792 : */
3793 14853 : void GCS_MAVLINK::handle_timesync(const mavlink_message_t &msg)
3794 : {
3795 : // decode incoming timesync message
3796 : mavlink_timesync_t tsync;
3797 14853 : mavlink_msg_timesync_decode(&msg, &tsync);
3798 :
3799 14853 : if (tsync.tc1 != 0) {
3800 : // this is a response to a timesync request
3801 0 : if (tsync.ts1 != _timesync_request.sent_ts1) {
3802 : // we didn't actually send the request.... or it's a
3803 : // response to an ancient request...
3804 0 : return;
3805 : }
3806 : #if 0
3807 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,
3808 : "timesync response sysid=%u (latency=%fms)",
3809 : msg.sysid,
3810 : round_trip_time_us*0.001f);
3811 : #endif
3812 :
3813 : #if HAL_LOGGING_ENABLED
3814 0 : const uint64_t round_trip_time_us = (timesync_receive_timestamp_ns() - _timesync_request.sent_ts1)*0.001f;
3815 0 : AP_Logger *logger = AP_Logger::get_singleton();
3816 0 : if (logger != nullptr) {
3817 0 : AP::logger().Write(
3818 : "TSYN",
3819 : "TimeUS,SysID,RTT",
3820 : "s-s",
3821 : "F-F",
3822 : "QBQ",
3823 : AP_HAL::micros64(),
3824 0 : msg.sysid,
3825 : round_trip_time_us
3826 : );
3827 : }
3828 : #endif // HAL_LOGGING_ENABLED
3829 0 : return;
3830 : }
3831 :
3832 14853 : if (!HAVE_PAYLOAD_SPACE(chan, TIMESYNC)) {
3833 : // drop this timesync request entirely
3834 0 : return;
3835 : }
3836 :
3837 : // create new timesync struct with tc1 field as system time in
3838 : // nanoseconds. The client timestamp is as close as possible to
3839 : // the time we received the TIMESYNC message.
3840 : mavlink_timesync_t rsync;
3841 14853 : rsync.tc1 = timesync_receive_timestamp_ns();
3842 14853 : rsync.ts1 = tsync.ts1;
3843 :
3844 : // respond with a timesync message
3845 14853 : mavlink_msg_timesync_send(
3846 : chan,
3847 : rsync.tc1,
3848 : rsync.ts1
3849 : );
3850 : }
3851 :
3852 : /*
3853 : * broadcast a timesync message. We may get multiple responses to this request.
3854 : */
3855 15191 : void GCS_MAVLINK::send_timesync()
3856 : {
3857 15191 : _timesync_request.sent_ts1 = timesync_timestamp_ns();
3858 15191 : mavlink_msg_timesync_send(
3859 : chan,
3860 : 0,
3861 : _timesync_request.sent_ts1
3862 : );
3863 15191 : }
3864 :
3865 1027468 : void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg)
3866 : {
3867 : #if HAL_LOGGING_ENABLED
3868 1027468 : AP_Logger *logger = AP_Logger::get_singleton();
3869 1027468 : if (logger == nullptr) {
3870 0 : return;
3871 : }
3872 :
3873 : mavlink_statustext_t packet;
3874 1027468 : mavlink_msg_statustext_decode(&msg, &packet);
3875 :
3876 1027468 : const uint8_t max_prefix_len = 14;
3877 1027468 : const uint8_t text_len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+max_prefix_len;
3878 1027468 : if (msg.sysid != statustext_chunking.last_src_system ||
3879 1025369 : msg.compid != statustext_chunking.last_src_system ||
3880 1025346 : packet.id != statustext_chunking.last_id) {
3881 454141 : statustext_chunking.last_src_system = msg.sysid;
3882 454141 : statustext_chunking.last_src_component = msg.compid;
3883 454141 : statustext_chunking.last_id = packet.id;
3884 454141 : statustext_chunking.msg_id = AP::logger().get_MSG_id();
3885 : }
3886 :
3887 1027468 : uint8_t offset = 0;
3888 1027468 : char text[text_len] = {};
3889 1027468 : if (packet.chunk_seq == 0) {
3890 : // prefix with origin information
3891 454127 : if (gcs().sysid_is_gcs(msg.sysid)) {
3892 7036 : strncpy(text, "GCS:", ARRAY_SIZE(text));
3893 7036 : offset = strlen(text);
3894 : } else {
3895 894182 : offset = hal.util->snprintf(text,
3896 : max_prefix_len,
3897 : "SRC=%u/%u:",
3898 447091 : msg.sysid,
3899 447091 : msg.compid);
3900 447091 : offset = MIN(offset, max_prefix_len);
3901 : }
3902 : }
3903 :
3904 1027468 : memcpy(&text[offset], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
3905 :
3906 1027468 : logger->Write_MessageChunk(statustext_chunking.msg_id, text, packet.chunk_seq);
3907 : #endif
3908 : }
3909 :
3910 :
3911 : /*
3912 : handle logging of named values from mavlink.
3913 : */
3914 0 : void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const
3915 : {
3916 : #if HAL_LOGGING_ENABLED
3917 0 : auto *logger = AP_Logger::get_singleton();
3918 0 : if (logger == nullptr) {
3919 0 : return;
3920 : }
3921 : mavlink_named_value_float_t p;
3922 0 : mavlink_msg_named_value_float_decode(&msg, &p);
3923 0 : char s[11] {};
3924 0 : strncpy(s, p.name, sizeof(s)-1);
3925 0 : logger->Write("NVAL", "TimeUS,TimeBootMS,Name,Value,SSys,SCom", "ss#---", "FC----", "QINfBB",
3926 : AP_HAL::micros64(),
3927 : p.time_boot_ms,
3928 : s,
3929 0 : p.value,
3930 0 : msg.sysid,
3931 0 : msg.compid);
3932 : #endif
3933 : }
3934 :
3935 : #if AP_RTC_ENABLED
3936 2 : void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg)
3937 : {
3938 : mavlink_system_time_t packet;
3939 2 : mavlink_msg_system_time_decode(&msg, &packet);
3940 :
3941 2 : AP::rtc().set_utc_usec(packet.time_unix_usec, AP_RTC::SOURCE_MAVLINK_SYSTEM_TIME);
3942 2 : }
3943 : #endif
3944 :
3945 : #if AP_CAMERA_ENABLED
3946 13 : MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_int_t &packet)
3947 : {
3948 13 : AP_Camera *camera = AP::camera();
3949 13 : if (camera == nullptr) {
3950 0 : return MAV_RESULT_UNSUPPORTED;
3951 : }
3952 :
3953 13 : return camera->handle_command(packet);
3954 : }
3955 : #endif
3956 :
3957 :
3958 : #if AP_AHRS_ENABLED
3959 : // sets ekf_origin if it has not been set.
3960 : // should only be used when there is no GPS to provide an absolute position
3961 7 : MAV_RESULT GCS_MAVLINK::set_ekf_origin(const Location& loc)
3962 : {
3963 : // check location is valid
3964 7 : if (loc.is_zero()) {
3965 1 : return MAV_RESULT_DENIED;
3966 : }
3967 6 : if (!loc.check_latlng()) {
3968 0 : return MAV_RESULT_DENIED;
3969 : }
3970 :
3971 6 : AP_AHRS &ahrs = AP::ahrs();
3972 :
3973 : // check if EKF origin has already been set
3974 6 : Location ekf_origin;
3975 6 : if (ahrs.get_origin(ekf_origin)) {
3976 1 : return MAV_RESULT_FAILED;
3977 : }
3978 :
3979 5 : if (!ahrs.set_origin(loc)) {
3980 3 : return MAV_RESULT_FAILED;
3981 : }
3982 :
3983 : // send ekf origin to GCS
3984 2 : if (!try_send_message(MSG_ORIGIN)) {
3985 : // try again later
3986 0 : send_message(MSG_ORIGIN);
3987 : }
3988 2 : return MAV_RESULT_ACCEPTED;
3989 : }
3990 :
3991 : #if AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED
3992 5 : void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t &msg)
3993 : {
3994 : mavlink_set_gps_global_origin_t packet;
3995 5 : mavlink_msg_set_gps_global_origin_decode(&msg, &packet);
3996 :
3997 : // sanity check location
3998 5 : if (!check_latlng(packet.latitude, packet.longitude)) {
3999 : // silently drop the request
4000 0 : return;
4001 : }
4002 : // sanity check altitude in mm, note that abs can't handle INT32_MIN correctly
4003 5 : if (packet.altitude < -LOCATION_ALT_MAX_M * 1000 || packet.altitude > LOCATION_ALT_MAX_M * 1000) {
4004 : // silently drop the request
4005 0 : return;
4006 : }
4007 :
4008 : const Location ekf_origin {
4009 : packet.latitude,
4010 : packet.longitude,
4011 5 : int32_t(packet.altitude * 0.1f), // mm -> cm
4012 : Location::AltFrame::ABSOLUTE
4013 5 : };
4014 5 : set_ekf_origin(ekf_origin);
4015 : }
4016 : #endif // AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED
4017 :
4018 : #endif // AP_AHRS_ENABLED
4019 :
4020 : /*
4021 : handle a DATA96 message
4022 : */
4023 0 : void GCS_MAVLINK::handle_data_packet(const mavlink_message_t &msg)
4024 : {
4025 : #if AP_RADIO_ENABLED
4026 : mavlink_data96_t m;
4027 : mavlink_msg_data96_decode(&msg, &m);
4028 : switch (m.type) {
4029 : case 42:
4030 : case 43: {
4031 : // pass to AP_Radio (for firmware upload and playing test tunes)
4032 : AP_Radio *radio = AP_Radio::get_singleton();
4033 : if (radio != nullptr) {
4034 : radio->handle_data_packet(chan, m);
4035 : }
4036 : break;
4037 : }
4038 : default:
4039 : // unknown
4040 : break;
4041 : }
4042 : #endif
4043 0 : }
4044 :
4045 : #if HAL_VISUALODOM_ENABLED
4046 3735 : void GCS_MAVLINK::handle_vision_position_delta(const mavlink_message_t &msg)
4047 : {
4048 3735 : AP_VisualOdom *visual_odom = AP::visualodom();
4049 3735 : if (visual_odom == nullptr) {
4050 0 : return;
4051 : }
4052 3735 : visual_odom->handle_vision_position_delta_msg(msg);
4053 : }
4054 :
4055 10472 : void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg)
4056 : {
4057 : mavlink_vision_position_estimate_t m;
4058 10472 : mavlink_msg_vision_position_estimate_decode(&msg, &m);
4059 :
4060 10472 : handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
4061 10472 : PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE));
4062 10472 : }
4063 :
4064 0 : void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t &msg)
4065 : {
4066 : mavlink_global_vision_position_estimate_t m;
4067 0 : mavlink_msg_global_vision_position_estimate_decode(&msg, &m);
4068 :
4069 0 : handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
4070 0 : PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE));
4071 0 : }
4072 :
4073 0 : void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg)
4074 : {
4075 : mavlink_vicon_position_estimate_t m;
4076 0 : mavlink_msg_vicon_position_estimate_decode(&msg, &m);
4077 :
4078 : // vicon position estimate does not include reset counter
4079 0 : handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, 0,
4080 0 : PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE));
4081 0 : }
4082 :
4083 : /*
4084 : handle ODOMETRY message. This message combines position, velocity
4085 : and attitude data
4086 : */
4087 0 : void GCS_MAVLINK::handle_odometry(const mavlink_message_t &msg)
4088 : {
4089 0 : AP_VisualOdom *visual_odom = AP::visualodom();
4090 0 : if (visual_odom == nullptr) {
4091 0 : return;
4092 : }
4093 :
4094 : mavlink_odometry_t m;
4095 0 : mavlink_msg_odometry_decode(&msg, &m);
4096 :
4097 0 : if (m.frame_id != MAV_FRAME_LOCAL_FRD ||
4098 0 : m.child_frame_id != MAV_FRAME_BODY_FRD) {
4099 : // only support local FRD frame data
4100 0 : return;
4101 : }
4102 :
4103 0 : Quaternion q{m.q[0],m.q[1],m.q[2],m.q[3]};
4104 :
4105 0 : float posErr = 0;
4106 0 : float angErr = 0;
4107 0 : if (!isnan(m.pose_covariance[0])) {
4108 0 : posErr = sqrtf(m.pose_covariance[0]+m.pose_covariance[6]+m.pose_covariance[11]);
4109 0 : angErr = sqrtf(m.pose_covariance[15]+m.pose_covariance[18]+m.pose_covariance[20]);
4110 : }
4111 :
4112 0 : const uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ODOMETRY));
4113 0 : visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter, m.quality);
4114 :
4115 : // convert velocity vector from FRD to NED frame
4116 0 : Vector3f vel{m.vx, m.vy, m.vz};
4117 0 : vel = q * vel;
4118 0 : visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, 0, m.reset_counter, m.quality);
4119 : }
4120 :
4121 : // there are several messages which all have identical fields in them.
4122 : // This function provides common handling for the data contained in
4123 : // these packets
4124 10472 : void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t usec,
4125 : const float x,
4126 : const float y,
4127 : const float z,
4128 : const float roll,
4129 : const float pitch,
4130 : const float yaw,
4131 : const float covariance[21],
4132 : const uint8_t reset_counter,
4133 : const uint16_t payload_size)
4134 : {
4135 10472 : float posErr = 0;
4136 10472 : float angErr = 0;
4137 : // correct offboard timestamp to be in local ms since boot
4138 10472 : uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size);
4139 :
4140 10472 : AP_VisualOdom *visual_odom = AP::visualodom();
4141 10472 : if (visual_odom == nullptr) {
4142 0 : return;
4143 : }
4144 :
4145 10472 : if (!isnan(covariance[0])) {
4146 10472 : posErr = sqrtf(covariance[0]+covariance[6]+covariance[11]);
4147 10472 : angErr = sqrtf(covariance[15]+covariance[18]+covariance[20]);
4148 : }
4149 :
4150 10472 : visual_odom->handle_pose_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter, 0);
4151 : }
4152 :
4153 0 : void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
4154 : {
4155 : mavlink_att_pos_mocap_t m;
4156 0 : mavlink_msg_att_pos_mocap_decode(&msg, &m);
4157 :
4158 : // correct offboard timestamp to be in local ms since boot
4159 0 : uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP));
4160 :
4161 0 : AP_VisualOdom *visual_odom = AP::visualodom();
4162 0 : if (visual_odom == nullptr) {
4163 0 : return;
4164 : }
4165 :
4166 0 : float posErr = 0;
4167 0 : float angErr = 0;
4168 0 : if (!isnan(m.covariance[0])) {
4169 0 : posErr = sqrtf(m.covariance[0]+m.covariance[6]+m.covariance[11]);
4170 0 : angErr = sqrtf(m.covariance[15]+m.covariance[18]+m.covariance[20]);
4171 : }
4172 :
4173 : // note: att_pos_mocap does not include reset counter
4174 0 : visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, posErr, angErr, 0, 0);
4175 : }
4176 :
4177 4438 : void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
4178 : {
4179 4438 : AP_VisualOdom *visual_odom = AP::visualodom();
4180 4438 : if (visual_odom == nullptr) {
4181 0 : return;
4182 : }
4183 : mavlink_vision_speed_estimate_t m;
4184 4438 : mavlink_msg_vision_speed_estimate_decode(&msg, &m);
4185 4438 : const Vector3f vel = {m.x, m.y, m.z};
4186 4438 : uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
4187 4438 : float vel_err = 0;
4188 4438 : if (!isnan(m.covariance[0])) {
4189 4438 : vel_err = sqrtf(m.covariance[0]+m.covariance[4]+m.covariance[8]);
4190 : }
4191 4438 : visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, vel_err, m.reset_counter, 0);
4192 : }
4193 : #endif // HAL_VISUALODOM_ENABLED
4194 :
4195 7454 : void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)
4196 : {
4197 : mavlink_command_ack_t packet;
4198 7454 : mavlink_msg_command_ack_decode(&msg, &packet);
4199 :
4200 : #if HAL_INS_ACCELCAL_ENABLED
4201 7454 : AP_AccelCal *accelcal = AP::ins().get_acal();
4202 7454 : if (accelcal != nullptr) {
4203 6 : accelcal->handle_command_ack(packet, msg.sysid, msg.compid);
4204 : }
4205 : #endif // HAL_INS_ACCELCAL_ENABLED
4206 :
4207 : #if AP_GENERATOR_LOWEHEISER_ENABLED
4208 : // this might be an ACK from a loweheiser generator:
4209 7454 : handle_generator_message(msg);
4210 : #endif // AP_GENERATOR_LOWEHEISER_ENABLED
4211 7454 : }
4212 :
4213 : #if AP_RC_CHANNEL_ENABLED
4214 : // allow override of RC channel values for complete GCS
4215 : // control of switch position and RC PWM values.
4216 792 : void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
4217 : {
4218 792 : if (!gcs().sysid_is_gcs(msg.sysid)) {
4219 0 : return; // Only accept control from our gcs
4220 : }
4221 :
4222 792 : const uint32_t tnow = AP_HAL::millis();
4223 :
4224 : mavlink_rc_channels_override_t packet;
4225 792 : mavlink_msg_rc_channels_override_decode(&msg, &packet);
4226 :
4227 : const uint16_t override_data[] = {
4228 792 : packet.chan1_raw,
4229 792 : packet.chan2_raw,
4230 792 : packet.chan3_raw,
4231 792 : packet.chan4_raw,
4232 792 : packet.chan5_raw,
4233 792 : packet.chan6_raw,
4234 792 : packet.chan7_raw,
4235 792 : packet.chan8_raw,
4236 792 : packet.chan9_raw,
4237 792 : packet.chan10_raw,
4238 792 : packet.chan11_raw,
4239 792 : packet.chan12_raw,
4240 792 : packet.chan13_raw,
4241 792 : packet.chan14_raw,
4242 792 : packet.chan15_raw,
4243 792 : packet.chan16_raw
4244 792 : };
4245 :
4246 7128 : for (uint8_t i=0; i<8; i++) {
4247 : // Per MAVLink spec a value of UINT16_MAX means to ignore this field.
4248 6336 : if (override_data[i] != UINT16_MAX) {
4249 1802 : RC_Channels::set_override(i, override_data[i], tnow);
4250 : }
4251 : }
4252 7128 : for (uint8_t i=8; i<ARRAY_SIZE(override_data); i++) {
4253 : // Per MAVLink spec a value of zero or UINT16_MAX means to
4254 : // ignore this field.
4255 6336 : if (override_data[i] != 0 && override_data[i] != UINT16_MAX) {
4256 : // per the mavlink spec, a value of UINT16_MAX-1 means
4257 : // return the field to RC radio values:
4258 305 : const uint16_t value = override_data[i] == (UINT16_MAX-1) ? 0 : override_data[i];
4259 305 : RC_Channels::set_override(i, value, tnow);
4260 : }
4261 : }
4262 :
4263 792 : sysid_mygcs_seen(tnow);
4264 :
4265 : }
4266 : #endif // AP_RC_CHANNEL_ENABLED
4267 :
4268 : #if AP_OPTICALFLOW_ENABLED
4269 0 : void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
4270 : {
4271 0 : AP_OpticalFlow *optflow = AP::opticalflow();
4272 0 : if (optflow == nullptr) {
4273 0 : return;
4274 : }
4275 0 : optflow->handle_msg(msg);
4276 : }
4277 : #endif
4278 :
4279 :
4280 : #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
4281 : /*
4282 : handle MAV_CMD_FIXED_MAG_CAL_YAW
4283 : */
4284 2 : MAV_RESULT GCS_MAVLINK::handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet)
4285 : {
4286 2 : Compass &compass = AP::compass();
4287 2 : if (!compass.mag_cal_fixed_yaw(packet.param1,
4288 2 : uint8_t(packet.param2),
4289 2 : packet.param3,
4290 2 : packet.param4)) {
4291 0 : return MAV_RESULT_FAILED;
4292 : }
4293 2 : return MAV_RESULT_ACCEPTED;
4294 : }
4295 : #endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
4296 :
4297 : #if COMPASS_CAL_ENABLED
4298 37 : MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_int_t &packet)
4299 : {
4300 37 : return AP::compass().handle_mag_cal_command(packet);
4301 : }
4302 : #endif // COMPASS_CAL_ENABLED
4303 :
4304 : #if AP_MAVLINKCAN_ENABLED
4305 : /*
4306 : handle MAV_CMD_CAN_FORWARD
4307 : */
4308 0 : MAV_RESULT GCS_MAVLINK::handle_can_forward(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
4309 : {
4310 0 : return AP_MAVLinkCAN::handle_can_forward(chan, packet, msg) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
4311 : }
4312 :
4313 : /*
4314 : handle CAN_FRAME messages
4315 : */
4316 0 : void GCS_MAVLINK::handle_can_frame(const mavlink_message_t &msg) const
4317 : {
4318 0 : AP_MAVLinkCAN::handle_can_frame(msg);
4319 0 : }
4320 : #endif // AP_MAVLINKCAN_ENABLED
4321 :
4322 497 : void GCS_MAVLINK::handle_distance_sensor(const mavlink_message_t &msg)
4323 : {
4324 : #if AP_RANGEFINDER_ENABLED
4325 497 : RangeFinder *rangefinder = AP::rangefinder();
4326 497 : if (rangefinder != nullptr) {
4327 497 : rangefinder->handle_msg(msg);
4328 : }
4329 : #endif
4330 :
4331 : #if HAL_PROXIMITY_ENABLED
4332 497 : AP_Proximity *proximity = AP::proximity();
4333 497 : if (proximity != nullptr) {
4334 497 : proximity->handle_msg(msg);
4335 : }
4336 : #endif
4337 497 : }
4338 :
4339 : #if HAL_PROXIMITY_ENABLED
4340 3 : void GCS_MAVLINK::handle_obstacle_distance(const mavlink_message_t &msg)
4341 : {
4342 3 : AP_Proximity *proximity = AP::proximity();
4343 3 : if (proximity != nullptr) {
4344 3 : proximity->handle_msg(msg);
4345 : }
4346 3 : }
4347 :
4348 376 : void GCS_MAVLINK::handle_obstacle_distance_3d(const mavlink_message_t &msg)
4349 : {
4350 376 : AP_Proximity *proximity = AP::proximity();
4351 376 : if (proximity != nullptr) {
4352 376 : proximity->handle_msg(msg);
4353 : }
4354 376 : }
4355 : #endif
4356 :
4357 : #if HAL_ADSB_ENABLED
4358 15 : void GCS_MAVLINK::handle_adsb_message(const mavlink_message_t &msg)
4359 : {
4360 15 : AP_ADSB *adsb = AP::ADSB();
4361 15 : if (adsb != nullptr) {
4362 15 : adsb->handle_message(chan, msg);
4363 : }
4364 15 : }
4365 : #endif
4366 :
4367 : #if OSD_PARAM_ENABLED
4368 0 : void GCS_MAVLINK::handle_osd_param_config(const mavlink_message_t &msg) const
4369 : {
4370 0 : AP_OSD *osd = AP::osd();
4371 0 : if (osd != nullptr) {
4372 0 : osd->handle_msg(msg, *this);
4373 : }
4374 0 : }
4375 : #endif
4376 :
4377 1064136 : void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg)
4378 : {
4379 : // if the heartbeat is from our GCS then we don't failsafe for
4380 : // now...
4381 1064136 : if (gcs().sysid_is_gcs(msg.sysid)) {
4382 14071 : sysid_mygcs_seen(AP_HAL::millis());
4383 : }
4384 1064136 : }
4385 :
4386 : /*
4387 : handle messages which don't require vehicle specific data
4388 : */
4389 2777428 : void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
4390 : {
4391 2777428 : switch (msg.msgid) {
4392 :
4393 1064136 : case MAVLINK_MSG_ID_HEARTBEAT: {
4394 1064136 : handle_heartbeat(msg);
4395 1064136 : break;
4396 : }
4397 :
4398 7454 : case MAVLINK_MSG_ID_COMMAND_ACK: {
4399 7454 : handle_command_ack(msg);
4400 7454 : break;
4401 : }
4402 :
4403 0 : case MAVLINK_MSG_ID_SETUP_SIGNING:
4404 0 : handle_setup_signing(msg);
4405 0 : break;
4406 :
4407 38620 : case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
4408 : case MAVLINK_MSG_ID_PARAM_SET:
4409 : case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
4410 38620 : handle_common_param_message(msg);
4411 38620 : break;
4412 :
4413 : #if AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED
4414 5 : case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
4415 5 : handle_set_gps_global_origin(msg);
4416 5 : break;
4417 : #endif
4418 :
4419 : #if AP_MAVLINK_MSG_DEVICE_OP_ENABLED
4420 0 : case MAVLINK_MSG_ID_DEVICE_OP_READ:
4421 0 : handle_device_op_read(msg);
4422 0 : break;
4423 0 : case MAVLINK_MSG_ID_DEVICE_OP_WRITE:
4424 0 : handle_device_op_write(msg);
4425 0 : break;
4426 : #endif
4427 :
4428 14853 : case MAVLINK_MSG_ID_TIMESYNC:
4429 14853 : handle_timesync(msg);
4430 14853 : break;
4431 : #if HAL_LOGGING_ENABLED
4432 23305 : case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
4433 : case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
4434 : case MAVLINK_MSG_ID_LOG_ERASE:
4435 : case MAVLINK_MSG_ID_LOG_REQUEST_END:
4436 : case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS:
4437 23305 : AP::logger().handle_mavlink_msg(*this, msg);
4438 23305 : break;
4439 : #endif
4440 :
4441 : #if AP_MAVLINK_FTP_ENABLED
4442 125 : case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
4443 125 : GCS_FTP::handle_file_transfer_protocol(msg, chan);
4444 125 : break;
4445 : #endif
4446 :
4447 : #if AP_CAMERA_ENABLED
4448 5 : case MAVLINK_MSG_ID_DIGICAM_CONTROL:
4449 : case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: // heartbeat from a GoPro in Solo gimbal
4450 : case MAVLINK_MSG_ID_CAMERA_INFORMATION:
4451 : {
4452 5 : AP_Camera *camera = AP::camera();
4453 5 : if (camera == nullptr) {
4454 0 : return;
4455 : }
4456 5 : camera->handle_message(chan, msg);
4457 : }
4458 5 : break;
4459 : #endif
4460 :
4461 16 : case MAVLINK_MSG_ID_SET_MODE:
4462 16 : handle_set_mode(msg);
4463 16 : break;
4464 :
4465 : #if AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED
4466 0 : case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
4467 0 : handle_send_autopilot_version(msg);
4468 0 : break;
4469 : #endif
4470 :
4471 16732 : case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
4472 : case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
4473 : case MAVLINK_MSG_ID_MISSION_COUNT:
4474 : case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
4475 : case MAVLINK_MSG_ID_MISSION_ITEM:
4476 : case MAVLINK_MSG_ID_MISSION_ITEM_INT:
4477 : case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
4478 : case MAVLINK_MSG_ID_MISSION_REQUEST:
4479 : case MAVLINK_MSG_ID_MISSION_ACK:
4480 : case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
4481 16732 : handle_common_mission_message(msg);
4482 16732 : break;
4483 :
4484 530536 : case MAVLINK_MSG_ID_COMMAND_LONG:
4485 530536 : handle_command_long(msg);
4486 528897 : break;
4487 :
4488 618 : case MAVLINK_MSG_ID_COMMAND_INT:
4489 618 : handle_command_int(msg);
4490 618 : break;
4491 :
4492 : #if AP_TERRAIN_AVAILABLE
4493 15 : case MAVLINK_MSG_ID_TERRAIN_DATA:
4494 : case MAVLINK_MSG_ID_TERRAIN_CHECK: {
4495 15 : AP_Terrain *terrain = AP::terrain();
4496 15 : if (terrain != nullptr) {
4497 15 : terrain->handle_message(*this, msg);
4498 : }
4499 15 : break;
4500 : }
4501 : #endif
4502 :
4503 : #if HAL_MOUNT_ENABLED
4504 20919 : case MAVLINK_MSG_ID_GIMBAL_REPORT:
4505 : case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
4506 : case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
4507 : case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
4508 : case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW:
4509 20919 : handle_mount_message(msg);
4510 20919 : break;
4511 : #endif
4512 :
4513 26 : case MAVLINK_MSG_ID_PARAM_VALUE:
4514 26 : handle_param_value(msg);
4515 26 : break;
4516 :
4517 0 : case MAVLINK_MSG_ID_RADIO:
4518 : case MAVLINK_MSG_ID_RADIO_STATUS:
4519 0 : handle_radio_status(msg);
4520 0 : break;
4521 :
4522 : #if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED
4523 0 : case MAVLINK_MSG_ID_SERIAL_CONTROL:
4524 0 : handle_serial_control(msg);
4525 0 : break;
4526 : #endif
4527 :
4528 : #if AP_GPS_ENABLED
4529 915 : case MAVLINK_MSG_ID_GPS_RTCM_DATA:
4530 : case MAVLINK_MSG_ID_GPS_INPUT:
4531 : case MAVLINK_MSG_ID_GPS_INJECT_DATA:
4532 915 : AP::gps().handle_msg(chan, msg);
4533 915 : break;
4534 : #endif
4535 :
4536 1027468 : case MAVLINK_MSG_ID_STATUSTEXT:
4537 1027468 : handle_statustext(msg);
4538 1027468 : break;
4539 :
4540 : #if AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED
4541 0 : case MAVLINK_MSG_ID_LED_CONTROL:
4542 : // send message to Notify
4543 0 : AP_Notify::handle_led_control(msg);
4544 0 : break;
4545 : #endif
4546 :
4547 : #if AP_RC_CHANNEL_ENABLED
4548 56 : case MAVLINK_MSG_ID_MANUAL_CONTROL:
4549 56 : handle_manual_control(msg);
4550 56 : break;
4551 : #endif
4552 :
4553 : #if AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED
4554 0 : case MAVLINK_MSG_ID_PLAY_TUNE:
4555 : // send message to Notify
4556 0 : AP_Notify::handle_play_tune(msg);
4557 0 : break;
4558 : #endif
4559 :
4560 2124 : case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
4561 2124 : if (option_enabled(Option::NOSTREAMOVERRIDE)) {
4562 : // options indicate we are to ignore this stream rate
4563 : // request
4564 0 : break;
4565 : }
4566 2124 : handle_request_data_stream(msg);
4567 2124 : break;
4568 :
4569 0 : case MAVLINK_MSG_ID_DATA96:
4570 0 : handle_data_packet(msg);
4571 0 : break;
4572 :
4573 : #if HAL_VISUALODOM_ENABLED
4574 3735 : case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
4575 3735 : handle_vision_position_delta(msg);
4576 3735 : break;
4577 :
4578 10472 : case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
4579 10472 : handle_vision_position_estimate(msg);
4580 10472 : break;
4581 :
4582 0 : case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
4583 0 : handle_global_vision_position_estimate(msg);
4584 0 : break;
4585 :
4586 0 : case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
4587 0 : handle_vicon_position_estimate(msg);
4588 0 : break;
4589 :
4590 0 : case MAVLINK_MSG_ID_ODOMETRY:
4591 0 : handle_odometry(msg);
4592 0 : break;
4593 :
4594 0 : case MAVLINK_MSG_ID_ATT_POS_MOCAP:
4595 0 : handle_att_pos_mocap(msg);
4596 0 : break;
4597 :
4598 4438 : case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE:
4599 4438 : handle_vision_speed_estimate(msg);
4600 4438 : break;
4601 : #endif // HAL_VISUALODOM_ENABLED
4602 :
4603 : #if AP_RTC_ENABLED
4604 2 : case MAVLINK_MSG_ID_SYSTEM_TIME:
4605 2 : handle_system_time_message(msg);
4606 2 : break;
4607 : #endif
4608 :
4609 : #if AP_RC_CHANNEL_ENABLED
4610 792 : case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
4611 792 : handle_rc_channels_override(msg);
4612 792 : break;
4613 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
4614 0 : case MAVLINK_MSG_ID_RADIO_RC_CHANNELS:
4615 0 : handle_radio_rc_channels(msg);
4616 0 : break;
4617 : #endif
4618 : #endif
4619 :
4620 : #if AP_OPTICALFLOW_ENABLED
4621 0 : case MAVLINK_MSG_ID_OPTICAL_FLOW:
4622 0 : handle_optical_flow(msg);
4623 0 : break;
4624 : #endif
4625 :
4626 497 : case MAVLINK_MSG_ID_DISTANCE_SENSOR:
4627 497 : handle_distance_sensor(msg);
4628 497 : break;
4629 :
4630 : #if HAL_PROXIMITY_ENABLED
4631 3 : case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
4632 3 : handle_obstacle_distance(msg);
4633 3 : break;
4634 :
4635 376 : case MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D:
4636 376 : handle_obstacle_distance_3d(msg);
4637 376 : break;
4638 : #endif
4639 :
4640 : #if OSD_PARAM_ENABLED
4641 0 : case MAVLINK_MSG_ID_OSD_PARAM_CONFIG:
4642 : case MAVLINK_MSG_ID_OSD_PARAM_SHOW_CONFIG:
4643 0 : handle_osd_param_config(msg);
4644 0 : break;
4645 : #endif
4646 :
4647 : #if HAL_ADSB_ENABLED
4648 15 : case MAVLINK_MSG_ID_ADSB_VEHICLE:
4649 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
4650 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
4651 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
4652 : case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CONTROL:
4653 15 : handle_adsb_message(msg);
4654 15 : break;
4655 : #endif
4656 :
4657 671 : case MAVLINK_MSG_ID_LANDING_TARGET:
4658 671 : handle_landing_target(msg);
4659 671 : break;
4660 :
4661 0 : case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
4662 0 : handle_named_value(msg);
4663 0 : break;
4664 :
4665 : #if HAL_CANMANAGER_ENABLED
4666 0 : case MAVLINK_MSG_ID_CAN_FRAME:
4667 : case MAVLINK_MSG_ID_CANFD_FRAME:
4668 0 : handle_can_frame(msg);
4669 0 : break;
4670 : #endif
4671 :
4672 : #if AP_MAVLINKCAN_ENABLED
4673 0 : case MAVLINK_MSG_ID_CAN_FILTER_MODIFY:
4674 0 : AP_MAVLinkCAN::handle_can_filter_modify(msg);
4675 0 : break;
4676 : #endif // AP_MAVLINKCAN_ENABLED
4677 :
4678 : #if AP_OPENDRONEID_ENABLED
4679 0 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS:
4680 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID:
4681 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID:
4682 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
4683 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
4684 : case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE:
4685 0 : AP::opendroneid().handle_msg(chan, msg);
4686 0 : break;
4687 : #endif
4688 :
4689 : #if AP_SIGNED_FIRMWARE
4690 : case MAVLINK_MSG_ID_SECURE_COMMAND:
4691 : case MAVLINK_MSG_ID_SECURE_COMMAND_REPLY:
4692 : AP_CheckFirmware::handle_msg(chan, msg);
4693 : break;
4694 : #endif
4695 :
4696 : #if AP_EFI_MAV_ENABLED
4697 0 : case MAVLINK_MSG_ID_EFI_STATUS:
4698 : {
4699 0 : AP_EFI *efi = AP::EFI();
4700 0 : if (efi) {
4701 0 : efi->handle_EFI_message(msg);
4702 : }
4703 0 : break;
4704 : }
4705 : #endif
4706 : #if AP_GENERATOR_LOWEHEISER_ENABLED
4707 3815 : case MAVLINK_MSG_ID_LOWEHEISER_GOV_EFI:
4708 : // message received from Loweheiser mavlink connection
4709 3815 : handle_generator_message(msg);
4710 3815 : break;
4711 : #endif
4712 : }
4713 :
4714 : }
4715 :
4716 11269 : void GCS_MAVLINK::handle_generator_message(const mavlink_message_t &msg)
4717 : {
4718 : #if AP_GENERATOR_LOWEHEISER_ENABLED
4719 11269 : AP_Generator *generator = AP::generator();
4720 11269 : if (generator == nullptr) {
4721 0 : return;
4722 : }
4723 :
4724 11269 : auto *backend = generator->get_loweheiser();
4725 11269 : if (backend == nullptr) {
4726 64 : return;
4727 : }
4728 :
4729 11205 : backend->handle_mavlink_msg(*this, msg);
4730 : #endif
4731 : }
4732 :
4733 16732 : void GCS_MAVLINK::handle_common_mission_message(const mavlink_message_t &msg)
4734 : {
4735 16732 : switch (msg.msgid) {
4736 5 : case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: // MAV ID: 38
4737 : {
4738 5 : handle_mission_write_partial_list(msg);
4739 5 : break;
4740 : }
4741 :
4742 : // GCS has sent us a mission item, store to EEPROM
4743 5247 : case MAVLINK_MSG_ID_MISSION_ITEM: // MAV ID: 39
4744 : case MAVLINK_MSG_ID_MISSION_ITEM_INT:
4745 5247 : handle_mission_item(msg);
4746 5247 : break;
4747 :
4748 : // read an individual command from EEPROM and send it to the GCS
4749 5582 : case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
4750 5582 : handle_mission_request_int(msg);
4751 5582 : break;
4752 :
4753 : #if AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
4754 0 : case MAVLINK_MSG_ID_MISSION_REQUEST:
4755 0 : handle_mission_request(msg);
4756 0 : break;
4757 : #endif
4758 :
4759 : #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
4760 2250 : case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // MAV ID: 41
4761 2250 : handle_mission_set_current(AP::mission(), msg);
4762 2250 : break;
4763 : #endif
4764 :
4765 : // GCS request the full list of commands, we return just the number and leave the GCS to then request each command individually
4766 467 : case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: // MAV ID: 43
4767 : {
4768 467 : handle_mission_request_list(msg);
4769 467 : break;
4770 : }
4771 :
4772 : // GCS provides the full number of commands it wishes to upload
4773 : // individual commands will then be sent from the GCS using the MAVLINK_MSG_ID_MISSION_ITEM message
4774 3177 : case MAVLINK_MSG_ID_MISSION_COUNT: // MAV ID: 44
4775 : {
4776 3177 : handle_mission_count(msg);
4777 3177 : break;
4778 : }
4779 :
4780 4 : case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // MAV ID: 45
4781 : {
4782 4 : handle_mission_clear_all(msg);
4783 4 : break;
4784 : }
4785 :
4786 0 : case MAVLINK_MSG_ID_MISSION_ACK:
4787 : /* not used */
4788 0 : break;
4789 : }
4790 16732 : }
4791 :
4792 : #if AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED
4793 0 : void GCS_MAVLINK::handle_send_autopilot_version(const mavlink_message_t &msg)
4794 : {
4795 0 : send_message(MSG_AUTOPILOT_VERSION);
4796 0 : }
4797 : #endif
4798 :
4799 1004 : void GCS_MAVLINK::send_banner()
4800 : {
4801 : // mark the firmware version in the tlog
4802 1004 : const AP_FWVersion &fwver = AP::fwversion();
4803 :
4804 1004 : send_text(MAV_SEVERITY_INFO, "%s", fwver.fw_string);
4805 :
4806 1004 : if (fwver.middleware_name && fwver.os_name) {
4807 0 : send_text(MAV_SEVERITY_INFO, "%s: %s %s: %s",
4808 0 : fwver.middleware_name, fwver.middleware_hash_str,
4809 0 : fwver.os_name, fwver.os_hash_str);
4810 1004 : } else if (fwver.os_name) {
4811 0 : send_text(MAV_SEVERITY_INFO, "%s: %s",
4812 0 : fwver.os_name, fwver.os_hash_str);
4813 : }
4814 :
4815 : // send system ID if we can
4816 : char sysid[50];
4817 1004 : if (hal.util->get_system_id(sysid)) {
4818 1004 : send_text(MAV_SEVERITY_INFO, "%s", sysid);
4819 : }
4820 :
4821 : // send MCUID if we can
4822 : #if HAL_WITH_IO_MCU
4823 : #define REVID_MASK 0xFFFF0000
4824 : #define DEVID_MASK 0xFFF
4825 : if (AP_BoardConfig::io_enabled()) {
4826 : uint32_t mcuid = iomcu.get_mcu_id();
4827 : send_text(MAV_SEVERITY_INFO, "IOMCU: %x %x %lx", uint16_t(mcuid & DEVID_MASK), uint16_t((mcuid & REVID_MASK) >> 16U),
4828 : iomcu.get_cpu_id());
4829 : }
4830 : #endif
4831 :
4832 : // send RC output mode info if available
4833 : char banner_msg[50];
4834 1004 : if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) {
4835 0 : send_text(MAV_SEVERITY_INFO, "%s", banner_msg);
4836 : }
4837 :
4838 : #if AP_INERTIALSENSOR_ENABLED
4839 : // output any fast sampling status messages
4840 7028 : for (uint8_t i = 0; i < INS_MAX_BACKENDS; i++) {
4841 6024 : if (AP::ins().get_output_banner(i, banner_msg, sizeof(banner_msg))) {
4842 0 : send_text(MAV_SEVERITY_INFO, "%s", banner_msg);
4843 : }
4844 : }
4845 : #endif
4846 1004 : }
4847 :
4848 :
4849 : #if AP_SIM_ENABLED
4850 1127758 : void GCS_MAVLINK::send_simstate() const
4851 : {
4852 1127758 : SITL::SIM *sitl = AP::sitl();
4853 1127758 : if (sitl == nullptr) {
4854 0 : return;
4855 : }
4856 1127758 : sitl->simstate_send(get_chan());
4857 : }
4858 :
4859 2791 : void GCS_MAVLINK::send_sim_state() const
4860 : {
4861 2791 : SITL::SIM *sitl = AP::sitl();
4862 2791 : if (sitl == nullptr) {
4863 0 : return;
4864 : }
4865 2791 : sitl->sim_state_send(get_chan());
4866 : }
4867 : #endif
4868 :
4869 : #if AP_AHRS_ENABLED
4870 3 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_global_origin(const mavlink_command_int_t &packet)
4871 : {
4872 3 : Location global_origin;
4873 3 : if (!location_from_command_t(packet, global_origin)) {
4874 1 : return MAV_RESULT_DENIED;
4875 : }
4876 :
4877 2 : return set_ekf_origin(global_origin);
4878 : }
4879 : #endif // AP_AHRS_ENABLED
4880 :
4881 : #if AP_BOOTLOADER_FLASHING_ENABLED
4882 : MAV_RESULT GCS_MAVLINK::handle_command_flash_bootloader(const mavlink_command_int_t &packet)
4883 : {
4884 : if (packet.x != 290876) {
4885 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Magic not set");
4886 : return MAV_RESULT_FAILED;
4887 : }
4888 :
4889 : switch (hal.util->flash_bootloader()) {
4890 : case AP_HAL::Util::FlashBootloader::OK:
4891 : case AP_HAL::Util::FlashBootloader::NO_CHANGE:
4892 : // consider NO_CHANGE as success (so as not to display error to user)
4893 : return MAV_RESULT_ACCEPTED;
4894 : #if AP_SIGNED_FIRMWARE
4895 : case AP_HAL::Util::FlashBootloader::NOT_SIGNED:
4896 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader not signed");
4897 : break;
4898 : #endif
4899 : default:
4900 : break;
4901 : }
4902 :
4903 : return MAV_RESULT_FAILED;
4904 : }
4905 : #endif // AP_BOOTLOADER_FLASHING_ENABLED
4906 :
4907 6 : MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink_message_t &msg)
4908 : {
4909 : #if AP_BARO_CALIBRATION_ENABLED
4910 : // fast barometer calibration
4911 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Updating barometer calibration");
4912 6 : AP::baro().update_calibration();
4913 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer calibration complete");
4914 : #endif // AP_BARO_CALIBRATION_ENABLED
4915 :
4916 : #if AP_AIRSPEED_ENABLED
4917 :
4918 6 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
4919 6 : if (airspeed != nullptr && airspeed->enabled()) {
4920 5 : GCS_MAVLINK_InProgress *task = GCS_MAVLINK_InProgress::get_task(MAV_CMD_PREFLIGHT_CALIBRATION, GCS_MAVLINK_InProgress::Type::AIRSPEED_CAL, msg.sysid, msg.compid, chan);
4921 5 : if (task == nullptr) {
4922 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
4923 : }
4924 5 : airspeed->calibrate(false);
4925 5 : return MAV_RESULT_IN_PROGRESS;
4926 : }
4927 : #endif
4928 :
4929 1 : return MAV_RESULT_ACCEPTED;
4930 : }
4931 :
4932 51 : MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
4933 : {
4934 51 : MAV_RESULT ret = MAV_RESULT_UNSUPPORTED;
4935 :
4936 51 : EXPECT_DELAY_MS(30000);
4937 51 : if (is_equal(packet.param1,1.0f)) {
4938 : #if AP_INERTIALSENSOR_ENABLED
4939 2 : if (!AP::ins().calibrate_gyros()) {
4940 0 : return MAV_RESULT_FAILED;
4941 : }
4942 : #else
4943 : return MAV_RESULT_UNSUPPORTED;
4944 : #endif
4945 2 : return MAV_RESULT_ACCEPTED;
4946 : }
4947 :
4948 49 : if (is_equal(packet.param3,1.0f)) {
4949 6 : return _handle_command_preflight_calibration_baro(msg);
4950 : }
4951 :
4952 : #if AP_RC_CHANNEL_ENABLED
4953 43 : rc().calibrating(is_positive(packet.param4));
4954 : #endif
4955 :
4956 : #if HAL_INS_ACCELCAL_ENABLED
4957 43 : if (packet.x == PREFLIGHT_CALIBRATION_ACCELEROMETER_FULL) {
4958 : // start with gyro calibration
4959 2 : if (!AP::ins().calibrate_gyros()) {
4960 0 : return MAV_RESULT_FAILED;
4961 : }
4962 : // start accel cal
4963 2 : AP::ins().acal_init();
4964 2 : AP::ins().get_acal()->start(this, msg.sysid, msg.compid);
4965 2 : return MAV_RESULT_ACCEPTED;
4966 : }
4967 : #endif
4968 :
4969 : #if AP_INERTIALSENSOR_ENABLED
4970 : #if AP_AHRS_ENABLED
4971 41 : if (packet.x == PREFLIGHT_CALIBRATION_ACCELEROMETER_TRIM) {
4972 3 : return AP::ins().calibrate_trim();
4973 : }
4974 : #endif
4975 :
4976 38 : if (packet.x == PREFLIGHT_CALIBRATION_ACCELEROMETER_SIMPLE) {
4977 : // simple accel calibration
4978 27 : return AP::ins().simple_accel_cal();
4979 : }
4980 :
4981 : /*
4982 : allow GCS to force an existing calibration of accel and/or
4983 : compass to be written as valid. This is useful when reloading
4984 : parameters after a full parameter erase
4985 : */
4986 11 : if (packet.x == PREFLIGHT_CALIBRATION_ACCELEROMETER_FORCE_SAVE) {
4987 : // force existing accel calibration to be accepted as valid
4988 3 : AP::ins().force_save_calibration();
4989 3 : ret = MAV_RESULT_ACCEPTED;
4990 : }
4991 : #endif // AP_INERTIALSENSOR_ENABLED
4992 :
4993 : #if AP_COMPASS_ENABLED
4994 11 : if (is_equal(packet.param2, static_cast<float>(PREFLIGHT_CALIBRATION_MAGNETOMETER_FORCE_SAVE))) {
4995 : // force existing compass calibration to be accepted as valid
4996 8 : AP::compass().force_save_calibration();
4997 8 : ret = MAV_RESULT_ACCEPTED;
4998 : }
4999 : #endif
5000 :
5001 11 : return ret;
5002 51 : }
5003 :
5004 54 : MAV_RESULT GCS_MAVLINK::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5005 : {
5006 54 : if (hal.util->get_soft_armed()) {
5007 : // *preflight*, remember?
5008 2 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Disarm to allow calibration");
5009 2 : return MAV_RESULT_FAILED;
5010 : }
5011 : // now call subclass methods:
5012 52 : return _handle_command_preflight_calibration(packet, msg);
5013 : }
5014 :
5015 : #if AP_ARMING_ENABLED
5016 66595 : MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_int_t &packet)
5017 : {
5018 66595 : if (hal.util->get_soft_armed()) {
5019 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
5020 : }
5021 66595 : (void)AP::arming().pre_arm_checks(true);
5022 66595 : return MAV_RESULT_ACCEPTED;
5023 : }
5024 : #endif // AP_ARMING_ENABLED
5025 :
5026 : #if AP_MISSION_ENABLED
5027 : // changes the current waypoint; at time of writing GCS
5028 : // implementations use the mavlink message MISSION_SET_CURRENT to set
5029 : // the current waypoint, rather than this DO command. It is hoped we
5030 : // can move to this command in the future to avoid acknowledgement
5031 : // issues with MISSION_SET_CURRENT
5032 12 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
5033 : {
5034 12 : AP_Mission &mission = AP::mission();
5035 :
5036 12 : if (is_equal(packet.param1, -1.0f) || packet.param1 >= 0) {
5037 : // these are the only values we handle - either -1 meaning do
5038 : // not reset waypoint (but reset counters) or a non-negative
5039 : // waypoint number
5040 : } else {
5041 : // invalid param1
5042 1 : return MAV_RESULT_DENIED;
5043 : }
5044 :
5045 11 : if (packet.param1 >= 0) {
5046 9 : const uint32_t seq = (uint32_t)packet.param1;
5047 9 : if (seq > INT16_MAX || !mission.is_valid_index(seq)) {
5048 2 : return MAV_RESULT_FAILED;
5049 : }
5050 : }
5051 :
5052 : // From https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MISSION_CURRENT:
5053 : // Param 2: Reset Mission
5054 : // - Resets mission. 1: true, 0: false. Resets jump counters to initial values
5055 : // and changes mission state "completed" to be "active" or "paused".
5056 9 : const bool reset_and_restart = is_equal(packet.param2, 1.0f);
5057 9 : if (reset_and_restart) {
5058 : // reset jump counters only
5059 3 : mission.reset_jump_counters();
5060 : }
5061 9 : if (packet.param1 >= 0) {
5062 7 : const uint32_t seq = (uint32_t)packet.param1;
5063 7 : if (!mission.set_current_cmd(seq)) {
5064 0 : return MAV_RESULT_FAILED;
5065 : }
5066 : }
5067 9 : if (reset_and_restart && packet.param1 >= 0) {
5068 2 : mission.resume();
5069 : }
5070 :
5071 : // volunteer the new current waypoint for all listeners
5072 9 : send_message(MSG_CURRENT_WAYPOINT);
5073 :
5074 9 : return MAV_RESULT_ACCEPTED;
5075 : }
5076 :
5077 5 : MAV_RESULT GCS_MAVLINK::handle_command_do_jump_tag(const mavlink_command_int_t &packet)
5078 : {
5079 5 : const uint32_t tag = (uint32_t)packet.param1;
5080 5 : if (tag > UINT16_MAX) {
5081 1 : return MAV_RESULT_DENIED;
5082 : }
5083 4 : if (!AP::mission().jump_to_tag(tag)) {
5084 3 : return MAV_RESULT_FAILED;
5085 : }
5086 :
5087 : // volunteer the new current waypoint for all listeners
5088 1 : send_message(MSG_CURRENT_WAYPOINT);
5089 :
5090 1 : return MAV_RESULT_ACCEPTED;
5091 : }
5092 : #endif
5093 :
5094 : #if AP_BATTERY_ENABLED
5095 18 : MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_int_t &packet)
5096 : {
5097 18 : const uint16_t battery_mask = packet.param1;
5098 18 : const float percentage = packet.param2;
5099 18 : if (AP::battery().reset_remaining_mask(battery_mask, percentage)) {
5100 18 : return MAV_RESULT_ACCEPTED;
5101 : }
5102 0 : return MAV_RESULT_FAILED;
5103 : }
5104 : #endif
5105 :
5106 : #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
5107 28 : MAV_RESULT GCS_MAVLINK::handle_command_request_autopilot_capabilities(const mavlink_command_int_t &packet)
5108 : {
5109 28 : if (!is_equal(packet.param1,1.0f)) {
5110 0 : return MAV_RESULT_FAILED;
5111 : }
5112 :
5113 28 : send_message(MSG_AUTOPILOT_VERSION);
5114 :
5115 28 : return MAV_RESULT_ACCEPTED;
5116 : }
5117 : #endif
5118 :
5119 3313 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_mode(const mavlink_command_int_t &packet)
5120 : {
5121 3313 : const uint8_t _base_mode = (uint8_t)packet.param1;
5122 3313 : const uint32_t _custom_mode = (uint32_t)packet.param2;
5123 :
5124 3313 : return _set_mode_common(_base_mode, _custom_mode);
5125 : }
5126 :
5127 : #if AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
5128 4738 : MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_int_t &packet)
5129 : {
5130 4738 : if (!AP::ahrs().home_is_set()) {
5131 2785 : return MAV_RESULT_FAILED;
5132 : }
5133 1953 : if (!try_send_message(MSG_HOME)) {
5134 : // try again later
5135 0 : send_message(MSG_HOME);
5136 : }
5137 1953 : if (!try_send_message(MSG_ORIGIN)) {
5138 : // try again later
5139 0 : send_message(MSG_ORIGIN);
5140 : }
5141 :
5142 1953 : return MAV_RESULT_ACCEPTED;
5143 : }
5144 : #endif // AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
5145 :
5146 0 : MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_int_t &packet)
5147 : {
5148 : // magic number must be supplied to trap; you must *really* mean it.
5149 0 : if (uint32_t(packet.param1) != 32451) {
5150 0 : return MAV_RESULT_DENIED;
5151 : }
5152 0 : if (hal.util->trap()) {
5153 0 : return MAV_RESULT_ACCEPTED;
5154 : }
5155 0 : return MAV_RESULT_UNSUPPORTED;
5156 : }
5157 :
5158 : #if AP_AHRS_ENABLED
5159 6 : MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_int_t &packet)
5160 : {
5161 : // source set must be between 1 and 3
5162 6 : uint32_t source_set = uint32_t(packet.param1);
5163 6 : if ((source_set >= 1) && (source_set <= 3)) {
5164 : // mavlink command uses range 1 to 3 while ahrs interface accepts 0 to 2
5165 4 : AP::ahrs().set_posvelyaw_source_set((AP_NavEKF_Source::SourceSetSelection)(source_set-1));
5166 4 : return MAV_RESULT_ACCEPTED;
5167 : }
5168 2 : return MAV_RESULT_DENIED;
5169 : }
5170 : #endif
5171 :
5172 : #if AP_GRIPPER_ENABLED
5173 16 : MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_int_t &packet)
5174 : {
5175 16 : AP_Gripper &gripper = AP::gripper();
5176 :
5177 : // param1 : gripper number (ignored)
5178 : // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum.
5179 16 : if(!gripper.enabled()) {
5180 0 : return MAV_RESULT_FAILED;
5181 : }
5182 :
5183 16 : MAV_RESULT result = MAV_RESULT_ACCEPTED;
5184 :
5185 16 : switch ((uint8_t)packet.param2) {
5186 8 : case GRIPPER_ACTION_RELEASE:
5187 8 : gripper.release();
5188 8 : break;
5189 8 : case GRIPPER_ACTION_GRAB:
5190 8 : gripper.grab();
5191 8 : break;
5192 0 : default:
5193 0 : result = MAV_RESULT_FAILED;
5194 0 : break;
5195 : }
5196 :
5197 16 : return result;
5198 : }
5199 : #endif // AP_GRIPPER_ENABLED
5200 :
5201 : #if HAL_SPRAYER_ENABLED
5202 4 : MAV_RESULT GCS_MAVLINK::handle_command_do_sprayer(const mavlink_command_int_t &packet)
5203 : {
5204 4 : AC_Sprayer *sprayer = AP::sprayer();
5205 4 : if (sprayer == nullptr) {
5206 0 : return MAV_RESULT_FAILED;
5207 : }
5208 :
5209 4 : if (is_equal(packet.param1, 1.0f)) {
5210 2 : sprayer->run(true);
5211 2 : } else if (is_zero(packet.param1)) {
5212 2 : sprayer->run(false);
5213 : }
5214 :
5215 4 : return MAV_RESULT_ACCEPTED;
5216 : }
5217 : #endif
5218 :
5219 : #if AP_LANDINGGEAR_ENABLED
5220 : /*
5221 : handle MAV_CMD_AIRFRAME_CONFIGURATION for landing gear control
5222 : */
5223 4 : MAV_RESULT GCS_MAVLINK::handle_command_airframe_configuration(const mavlink_command_int_t &packet)
5224 : {
5225 : // Param 1: Select which gear, not used in ArduPilot
5226 : // Param 2: 0 = Deploy, 1 = Retract
5227 : // For safety, anything other than 1 will deploy
5228 4 : AP_LandingGear *lg = AP_LandingGear::get_singleton();
5229 4 : if (lg == nullptr) {
5230 0 : return MAV_RESULT_UNSUPPORTED;
5231 : }
5232 4 : switch ((uint8_t)packet.param2) {
5233 1 : case 1:
5234 1 : lg->set_position(AP_LandingGear::LandingGear_Retract);
5235 1 : return MAV_RESULT_ACCEPTED;
5236 3 : default:
5237 3 : lg->set_position(AP_LandingGear::LandingGear_Deploy);
5238 3 : return MAV_RESULT_ACCEPTED;
5239 : }
5240 : return MAV_RESULT_FAILED;
5241 : }
5242 : #endif
5243 :
5244 : #if HAL_INS_ACCELCAL_ENABLED
5245 0 : MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet)
5246 : {
5247 0 : if (AP::ins().get_acal() == nullptr ||
5248 0 : !AP::ins().get_acal()->gcs_vehicle_position(packet.param1)) {
5249 0 : return MAV_RESULT_FAILED;
5250 : }
5251 0 : return MAV_RESULT_ACCEPTED;
5252 : }
5253 : #endif // HAL_INS_ACCELCAL_ENABLED
5254 :
5255 : #if HAL_MOUNT_ENABLED
5256 437 : MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5257 : {
5258 437 : AP_Mount *mount = AP::mount();
5259 437 : if (mount == nullptr) {
5260 0 : return MAV_RESULT_UNSUPPORTED;
5261 : }
5262 437 : return mount->handle_command(packet, msg);
5263 : }
5264 : #endif // HAL_MOUNT_ENABLED
5265 :
5266 : #if AP_ARMING_ENABLED
5267 1681 : MAV_RESULT GCS_MAVLINK::handle_command_component_arm_disarm(const mavlink_command_int_t &packet)
5268 : {
5269 1681 : if (is_equal(packet.param1,1.0f)) {
5270 1178 : if (AP::arming().is_armed()) {
5271 26 : return MAV_RESULT_ACCEPTED;
5272 : }
5273 : // run pre_arm_checks and arm_checks and display failures
5274 1152 : const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value) && !is_equal(packet.param2,magic_force_arm_disarm_value);
5275 1152 : if (AP::arming().arm(AP_Arming::Method::MAVLINK, do_arming_checks)) {
5276 1084 : return MAV_RESULT_ACCEPTED;
5277 : }
5278 68 : return MAV_RESULT_FAILED;
5279 : }
5280 503 : if (is_zero(packet.param1)) {
5281 503 : if (!AP::arming().is_armed()) {
5282 32 : return MAV_RESULT_ACCEPTED;
5283 : }
5284 471 : const bool forced = is_equal(packet.param2, magic_force_arm_disarm_value);
5285 : // note disarm()'s second parameter is "do_disarm_checks"
5286 471 : if (AP::arming().disarm(AP_Arming::Method::MAVLINK, !forced)) {
5287 464 : return MAV_RESULT_ACCEPTED;
5288 : }
5289 7 : return MAV_RESULT_FAILED;
5290 : }
5291 :
5292 0 : return MAV_RESULT_UNSUPPORTED;
5293 : }
5294 : #endif // AP_ARMING_ENABLED
5295 :
5296 202 : bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Location &out)
5297 : {
5298 : // sanity check location
5299 202 : if (!check_latlng(in.x, in.y)) {
5300 2 : return false;
5301 : }
5302 :
5303 200 : if (!command_long_stores_location((MAV_CMD)in.command)) {
5304 0 : return false;
5305 : }
5306 :
5307 : // integer storage imposes limits on the altitudes we can accept:
5308 200 : if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) {
5309 0 : return false;
5310 : }
5311 :
5312 : Location::AltFrame frame;
5313 200 : if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in.frame, frame)) {
5314 : // unknown coordinate frame
5315 0 : return false;
5316 : }
5317 :
5318 200 : out.lat = in.x;
5319 200 : out.lng = in.y;
5320 :
5321 200 : out.set_alt_m(in.z, frame);
5322 :
5323 200 : return true;
5324 : }
5325 :
5326 1590145 : bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command)
5327 : {
5328 1590145 : switch(command) {
5329 284 : case MAV_CMD_DO_SET_HOME:
5330 : case MAV_CMD_DO_SET_ROI:
5331 : case MAV_CMD_DO_SET_ROI_LOCATION:
5332 : // case MAV_CMD_NAV_TAKEOFF: // technically yes, but we don't do lat/lng
5333 : // case MAV_CMD_NAV_VTOL_TAKEOFF:
5334 : case MAV_CMD_DO_REPOSITION:
5335 : case MAV_CMD_EXTERNAL_POSITION_ESTIMATE:
5336 : case MAV_CMD_DO_SET_GLOBAL_ORIGIN:
5337 284 : return true;
5338 1589861 : default:
5339 1589861 : return false;
5340 : }
5341 : return false;
5342 : }
5343 :
5344 : #if AP_MAVLINK_COMMAND_LONG_ENABLED
5345 : // when conveyed via COMMAND_LONG, a command doesn't come with an
5346 : // explicit frame. When conveying a location they do have an assumed
5347 : // frame in ArduPilot, and this function returns that frame.
5348 28 : bool GCS_MAVLINK::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
5349 : {
5350 : static const struct {
5351 : uint32_t command;
5352 : MAV_FRAME frame;
5353 : } frame_map[] {
5354 : { MAV_CMD_FIXED_MAG_CAL_YAW, MAV_FRAME_GLOBAL_RELATIVE_ALT },
5355 : { MAV_CMD_DO_SET_ROI, MAV_FRAME_GLOBAL_RELATIVE_ALT },
5356 : { MAV_CMD_DO_SET_ROI_LOCATION, MAV_FRAME_GLOBAL_RELATIVE_ALT },
5357 : { MAV_CMD_DO_SET_HOME, MAV_FRAME_GLOBAL },
5358 : };
5359 :
5360 : // map from command to frame:
5361 83 : for (const auto &map : frame_map) {
5362 83 : if (map.command != packet_command) {
5363 55 : continue;
5364 : }
5365 28 : frame = map.frame;
5366 28 : return true;
5367 : }
5368 :
5369 0 : return false;
5370 : }
5371 :
5372 530536 : MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
5373 : {
5374 530536 : MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
5375 530536 : if (command_long_stores_location((MAV_CMD)packet.command)) {
5376 : // we must be able to supply a frame for the location:
5377 28 : if (!mav_frame_for_command_long(frame, (MAV_CMD)packet.command)) {
5378 0 : return MAV_RESULT_UNSUPPORTED;
5379 : }
5380 : }
5381 :
5382 : // convert and run the command
5383 : mavlink_command_int_t command_int;
5384 530536 : convert_COMMAND_LONG_to_COMMAND_INT(packet, command_int, frame);
5385 :
5386 530536 : return handle_command_int_packet(command_int, msg);
5387 : }
5388 :
5389 : // returns a value suitable for COMMAND_INT.x or y based on a value
5390 : // coming in from COMMAND_LONG.p5 or p6:
5391 2118818 : static int32_t convert_COMMAND_LONG_loc_param(float param, bool stores_location)
5392 : {
5393 2118818 : if (isnan(param)) {
5394 0 : return 0;
5395 : }
5396 :
5397 2118818 : if (stores_location) {
5398 112 : return param *1e7;
5399 : }
5400 :
5401 2118706 : return param;
5402 : }
5403 :
5404 1059409 : void GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame)
5405 : {
5406 1059409 : out = {};
5407 1059409 : out.target_system = in.target_system;
5408 1059409 : out.target_component = in.target_component;
5409 1059409 : out.frame = frame;
5410 1059409 : out.command = in.command;
5411 1059409 : out.current = 0;
5412 1059409 : out.autocontinue = 0;
5413 1059409 : out.param1 = in.param1;
5414 1059409 : out.param2 = in.param2;
5415 1059409 : out.param3 = in.param3;
5416 1059409 : out.param4 = in.param4;
5417 1059409 : const bool stores_location = command_long_stores_location((MAV_CMD)in.command);
5418 1059409 : out.x = convert_COMMAND_LONG_loc_param(in.param5, stores_location);
5419 1059409 : out.y = convert_COMMAND_LONG_loc_param(in.param6, stores_location);
5420 1059409 : out.z = in.param7;
5421 1059409 : }
5422 :
5423 530536 : void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
5424 : {
5425 : // decode packet
5426 : mavlink_command_long_t packet;
5427 530536 : mavlink_msg_command_long_decode(&msg, &packet);
5428 :
5429 : #if AP_SCRIPTING_ENABLED
5430 530536 : AP_Scripting *scripting = AP_Scripting::get_singleton();
5431 530536 : if (scripting != nullptr && scripting->is_handling_command(packet.command)) {
5432 : // Scripting has registered to receive this command, do not procces it internaly
5433 0 : return;
5434 : }
5435 : #endif
5436 :
5437 530536 : hal.util->persistent_data.last_mavlink_cmd = packet.command;
5438 :
5439 530536 : const MAV_RESULT result = try_command_long_as_command_int(packet, msg);
5440 :
5441 : // send ACK or NAK
5442 528897 : mavlink_msg_command_ack_send(chan, packet.command, result,
5443 : 0, 0,
5444 528897 : msg.sysid,
5445 528897 : msg.compid);
5446 :
5447 : #if HAL_LOGGING_ENABLED
5448 : // log the packet:
5449 : mavlink_command_int_t packet_int;
5450 528897 : convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int);
5451 528897 : AP::logger().Write_Command(packet_int, msg.sysid, msg.compid, result, true);
5452 : #endif
5453 :
5454 528897 : hal.util->persistent_data.last_mavlink_cmd = 0;
5455 : }
5456 :
5457 : #else
5458 : void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
5459 : {
5460 : // decode packet
5461 : mavlink_command_long_t packet;
5462 : mavlink_msg_command_long_decode(&msg, &packet);
5463 :
5464 : // send ACK or NAK
5465 : mavlink_msg_command_ack_send(
5466 : chan,
5467 : packet.command,
5468 : MAV_RESULT_COMMAND_INT_ONLY,
5469 : 0,
5470 : 0,
5471 : msg.sysid,
5472 : msg.compid
5473 : );
5474 :
5475 : }
5476 : #endif // AP_MAVLINK_COMMAND_LONG_ENABLED
5477 :
5478 0 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc)
5479 : {
5480 : #if HAL_MOUNT_ENABLED
5481 0 : AP_Mount *mount = AP::mount();
5482 0 : if (mount == nullptr) {
5483 0 : return MAV_RESULT_UNSUPPORTED;
5484 : }
5485 :
5486 : // sanity check location
5487 0 : if (!roi_loc.check_latlng()) {
5488 0 : return MAV_RESULT_FAILED;
5489 : }
5490 :
5491 0 : if (!roi_loc.initialised()) {
5492 0 : mount->clear_roi_target();
5493 : } else {
5494 0 : mount->set_roi_target(roi_loc);
5495 : }
5496 0 : return MAV_RESULT_ACCEPTED;
5497 : #else
5498 : return MAV_RESULT_UNSUPPORTED;
5499 : #endif
5500 : }
5501 :
5502 :
5503 671 : void GCS_MAVLINK::handle_landing_target(const mavlink_message_t &msg)
5504 : {
5505 : mavlink_landing_target_t m;
5506 671 : mavlink_msg_landing_target_decode(&msg, &m);
5507 : // correct offboard timestamp
5508 671 : const uint32_t corrected_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, LANDING_TARGET));
5509 671 : handle_landing_target(m, corrected_ms);
5510 671 : }
5511 :
5512 :
5513 : #if AP_HOME_ENABLED
5514 17 : bool GCS_MAVLINK::set_home_to_current_location(bool _lock)
5515 : {
5516 : #if AP_VEHICLE_ENABLED
5517 17 : return AP::vehicle()->set_home_to_current_location(_lock);
5518 : #else
5519 : return false;
5520 : #endif
5521 : }
5522 :
5523 83 : bool GCS_MAVLINK::set_home(const Location& loc, bool _lock) {
5524 : #if AP_VEHICLE_ENABLED
5525 83 : return AP::vehicle()->set_home(loc, _lock);
5526 : #else
5527 : return false;
5528 : #endif
5529 : }
5530 : #endif // AP_HOME_ENABLED
5531 :
5532 : #if AP_HOME_ENABLED
5533 101 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_home(const mavlink_command_int_t &packet)
5534 : {
5535 101 : if (is_equal(packet.param1, 1.0f) || (packet.x == 0 && packet.y == 0)) {
5536 : // param1 is 1 (or both lat and lon are zero); use current location
5537 17 : if (!set_home_to_current_location(true)) {
5538 0 : return MAV_RESULT_FAILED;
5539 : }
5540 17 : return MAV_RESULT_ACCEPTED;
5541 : }
5542 : // ensure param1 is zero
5543 84 : if (!is_zero(packet.param1)) {
5544 0 : return MAV_RESULT_FAILED;
5545 : }
5546 84 : Location new_home_loc;
5547 84 : if (!location_from_command_t(packet, new_home_loc)) {
5548 1 : return MAV_RESULT_DENIED;
5549 : }
5550 83 : if (!set_home(new_home_loc, true)) {
5551 0 : return MAV_RESULT_FAILED;
5552 : }
5553 83 : return MAV_RESULT_ACCEPTED;
5554 : }
5555 : #endif // AP_HOME_ENABLED
5556 :
5557 : #if AP_AHRS_POSITION_RESET_ENABLED
5558 2 : MAV_RESULT GCS_MAVLINK::handle_command_int_external_position_estimate(const mavlink_command_int_t &packet)
5559 : {
5560 4 : if ((packet.frame != MAV_FRAME_GLOBAL && packet.frame != MAV_FRAME_GLOBAL_INT) ||
5561 2 : !isnan(packet.z)) {
5562 : // we only support global frame without altitude
5563 0 : return MAV_RESULT_DENIED;
5564 : }
5565 :
5566 : // cope with the NaN when convering to Location
5567 2 : Location loc;
5568 2 : mavlink_command_int_t p2 = packet;
5569 2 : p2.z = 0;
5570 :
5571 2 : if (!location_from_command_t(p2, loc)) {
5572 0 : return MAV_RESULT_DENIED;
5573 : }
5574 2 : uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(uint64_t(p2.param1*1e6), PAYLOAD_SIZE(chan, COMMAND_INT));
5575 2 : const uint32_t processing_ms = p2.param2*1e3;
5576 2 : const float pos_accuracy = p2.param3;
5577 2 : if (timestamp_ms > processing_ms) {
5578 2 : timestamp_ms -= processing_ms;
5579 : }
5580 2 : if (!AP::ahrs().handle_external_position_estimate(loc, pos_accuracy, timestamp_ms)) {
5581 1 : return MAV_RESULT_FAILED;
5582 : }
5583 1 : return MAV_RESULT_ACCEPTED;
5584 : }
5585 : #endif // AP_AHRS_POSITION_RESET_ENABLED
5586 :
5587 : #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
5588 22 : MAV_RESULT GCS_MAVLINK::handle_command_int_external_wind_estimate(const mavlink_command_int_t &packet)
5589 : {
5590 22 : if (packet.param1 < 0) {
5591 4 : return MAV_RESULT_DENIED;
5592 : }
5593 18 : if (packet.param3 < 0 || packet.param3 > 360) {
5594 4 : return MAV_RESULT_DENIED;
5595 : }
5596 :
5597 14 : AP::ahrs().set_external_wind_estimate(packet.param1, packet.param3);
5598 14 : return MAV_RESULT_ACCEPTED;
5599 : }
5600 : #endif // AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
5601 :
5602 38 : MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &packet)
5603 : {
5604 : // be aware that this method is called for both MAV_CMD_DO_SET_ROI
5605 : // and MAV_CMD_DO_SET_ROI_LOCATION. If you intend to support any
5606 : // of the extra fields in the former then you will need to split
5607 : // off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't
5608 : // support the extra fields).
5609 :
5610 : // param1 : /* Region of interest mode (not used)*/
5611 : // param2 : /* MISSION index/ target ID (not used)*/
5612 : // param3 : /* ROI index (not used)*/
5613 : // param4 : /* empty */
5614 : // x : lat
5615 : // y : lon
5616 : // z : alt
5617 38 : Location roi_loc;
5618 38 : if (!location_from_command_t(packet, roi_loc)) {
5619 0 : return MAV_RESULT_DENIED;
5620 : }
5621 38 : return handle_command_do_set_roi(roi_loc);
5622 : }
5623 :
5624 : #if AP_FILESYSTEM_FORMAT_ENABLED
5625 0 : MAV_RESULT GCS_MAVLINK::handle_command_storage_format(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5626 : {
5627 0 : if (!is_equal(packet.param1, 1.0f) ||
5628 0 : !is_equal(packet.param2, 1.0f)) {
5629 0 : return MAV_RESULT_UNSUPPORTED;
5630 : }
5631 0 : GCS_MAVLINK_InProgress *task = GCS_MAVLINK_InProgress::get_task(MAV_CMD_STORAGE_FORMAT, GCS_MAVLINK_InProgress::Type::SD_FORMAT, msg.sysid, msg.compid, chan);
5632 0 : if (task == nullptr) {
5633 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
5634 : }
5635 0 : if (!AP::FS().format()) {
5636 0 : task->abort();
5637 0 : return MAV_RESULT_FAILED;
5638 : }
5639 0 : return MAV_RESULT_IN_PROGRESS;
5640 : }
5641 : #endif
5642 :
5643 2 : MAV_RESULT GCS_MAVLINK::handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5644 : {
5645 2 : switch ((SAFETY_SWITCH_STATE)packet.param1) {
5646 1 : case SAFETY_SWITCH_STATE_DANGEROUS:
5647 : // turn safety off (pwm outputs flow to the motors)
5648 1 : hal.rcout->force_safety_off();
5649 1 : return MAV_RESULT_ACCEPTED;
5650 1 : case SAFETY_SWITCH_STATE_SAFE:
5651 : // turn safety on (no pwm outputs to the motors)
5652 1 : if (hal.rcout->force_safety_on()) {
5653 1 : return MAV_RESULT_ACCEPTED;
5654 : }
5655 0 : return MAV_RESULT_FAILED;
5656 0 : default:
5657 0 : return MAV_RESULT_DENIED;
5658 : }
5659 : }
5660 :
5661 : #if AP_MAVLINK_FOLLOW_HANDLING_ENABLED
5662 0 : MAV_RESULT GCS_MAVLINK::handle_command_do_follow(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5663 : {
5664 0 : AP_Follow *follow = AP_Follow::get_singleton();
5665 0 : if (follow == nullptr) {
5666 0 : return MAV_RESULT_UNSUPPORTED;
5667 : }
5668 :
5669 : // param1: sysid of target to follow
5670 0 : if ((packet.param1 > 0) && (packet.param1 <= 255)) {
5671 0 : follow->set_target_sysid((uint8_t)packet.param1);
5672 0 : return MAV_RESULT_ACCEPTED;
5673 : }
5674 0 : return MAV_RESULT_DENIED;
5675 : }
5676 : #endif // AP_MAVLINK_FOLLOW_HANDLING_ENABLED
5677 :
5678 530592 : MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
5679 : {
5680 530592 : switch (packet.command) {
5681 :
5682 : #if HAL_INS_ACCELCAL_ENABLED
5683 0 : case MAV_CMD_ACCELCAL_VEHICLE_POS:
5684 0 : return handle_command_accelcal_vehicle_pos(packet);
5685 : #endif
5686 :
5687 : #if AP_LANDINGGEAR_ENABLED
5688 4 : case MAV_CMD_AIRFRAME_CONFIGURATION:
5689 4 : return handle_command_airframe_configuration(packet);
5690 : #endif
5691 :
5692 : #if AP_BATTERY_ENABLED
5693 18 : case MAV_CMD_BATTERY_RESET:
5694 18 : return handle_command_battery_reset(packet);
5695 : #endif
5696 :
5697 : #if AP_MAVLINKCAN_ENABLED
5698 0 : case MAV_CMD_CAN_FORWARD:
5699 0 : return handle_can_forward(packet, msg);
5700 : #endif // AP_MAVLINKCAN_ENABLED
5701 :
5702 : #if HAL_HIGH_LATENCY2_ENABLED
5703 16 : case MAV_CMD_CONTROL_HIGH_LATENCY:
5704 16 : return handle_control_high_latency(packet);
5705 : #endif // HAL_HIGH_LATENCY2_ENABLED
5706 :
5707 0 : case MAV_CMD_DEBUG_TRAP:
5708 0 : return handle_command_debug_trap(packet);
5709 :
5710 : #if HAL_ADSB_ENABLED
5711 0 : case MAV_CMD_DO_ADSB_OUT_IDENT:
5712 0 : if ((AP::ADSB() != nullptr) && AP::ADSB()->ident_start()) {
5713 0 : return MAV_RESULT_ACCEPTED;
5714 : }
5715 0 : return MAV_RESULT_FAILED;
5716 : #endif
5717 :
5718 : #if AP_AHRS_ENABLED
5719 3 : case MAV_CMD_DO_SET_GLOBAL_ORIGIN:
5720 3 : return handle_command_do_set_global_origin(packet);
5721 : #endif // AP_AHRS_ENABLED
5722 :
5723 : #if AP_RC_CHANNEL_ENABLED
5724 25 : case MAV_CMD_DO_AUX_FUNCTION:
5725 25 : return handle_command_do_aux_function(packet);
5726 : #endif
5727 :
5728 : #if AP_FENCE_ENABLED
5729 95 : case MAV_CMD_DO_FENCE_ENABLE:
5730 95 : return handle_command_do_fence_enable(packet);
5731 : #endif
5732 :
5733 7 : case MAV_CMD_DO_FLIGHTTERMINATION:
5734 7 : return handle_flight_termination(packet);
5735 :
5736 : #if AP_GRIPPER_ENABLED
5737 16 : case MAV_CMD_DO_GRIPPER:
5738 16 : return handle_command_do_gripper(packet);
5739 : #endif
5740 :
5741 : #if AP_MISSION_ENABLED
5742 5 : case MAV_CMD_DO_JUMP_TAG:
5743 5 : return handle_command_do_jump_tag(packet);
5744 :
5745 12 : case MAV_CMD_DO_SET_MISSION_CURRENT:
5746 12 : return handle_command_do_set_mission_current(packet);
5747 : #endif
5748 :
5749 3313 : case MAV_CMD_DO_SET_MODE:
5750 3313 : return handle_command_do_set_mode(packet);
5751 :
5752 : #if HAL_SPRAYER_ENABLED
5753 4 : case MAV_CMD_DO_SPRAYER:
5754 4 : return handle_command_do_sprayer(packet);
5755 : #endif
5756 :
5757 : #if AP_CAMERA_ENABLED
5758 13 : case MAV_CMD_DO_DIGICAM_CONFIGURE:
5759 : case MAV_CMD_DO_DIGICAM_CONTROL:
5760 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
5761 : case MAV_CMD_SET_CAMERA_ZOOM:
5762 : case MAV_CMD_SET_CAMERA_FOCUS:
5763 : case MAV_CMD_SET_CAMERA_SOURCE:
5764 : case MAV_CMD_IMAGE_START_CAPTURE:
5765 : case MAV_CMD_IMAGE_STOP_CAPTURE:
5766 : case MAV_CMD_CAMERA_TRACK_POINT:
5767 : case MAV_CMD_CAMERA_TRACK_RECTANGLE:
5768 : case MAV_CMD_CAMERA_STOP_TRACKING:
5769 : case MAV_CMD_VIDEO_START_CAPTURE:
5770 : case MAV_CMD_VIDEO_STOP_CAPTURE:
5771 13 : return handle_command_camera(packet);
5772 : #endif
5773 :
5774 14 : case MAV_CMD_DO_SET_ROI_NONE: {
5775 14 : const Location zero_loc;
5776 14 : return handle_command_do_set_roi(zero_loc);
5777 : }
5778 :
5779 38 : case MAV_CMD_DO_SET_ROI:
5780 : case MAV_CMD_DO_SET_ROI_LOCATION:
5781 38 : return handle_command_do_set_roi(packet);
5782 :
5783 : #if HAL_MOUNT_ENABLED
5784 437 : case MAV_CMD_DO_SET_ROI_SYSID:
5785 : case MAV_CMD_DO_MOUNT_CONFIGURE:
5786 : case MAV_CMD_DO_MOUNT_CONTROL:
5787 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
5788 : case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
5789 437 : return handle_command_mount(packet, msg);
5790 : #endif // HAL_MOUNT_ENABLED
5791 :
5792 4 : case MAV_CMD_DO_SEND_BANNER:
5793 4 : send_banner();
5794 4 : return MAV_RESULT_ACCEPTED;
5795 :
5796 : #if AP_HOME_ENABLED
5797 101 : case MAV_CMD_DO_SET_HOME:
5798 101 : return handle_command_do_set_home(packet);
5799 : #endif
5800 :
5801 : #if AP_AHRS_POSITION_RESET_ENABLED
5802 2 : case MAV_CMD_EXTERNAL_POSITION_ESTIMATE:
5803 2 : return handle_command_int_external_position_estimate(packet);
5804 : #endif
5805 : #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
5806 22 : case MAV_CMD_EXTERNAL_WIND_ESTIMATE:
5807 22 : return handle_command_int_external_wind_estimate(packet);
5808 : #endif
5809 : #if AP_ARMING_ENABLED
5810 1712 : case MAV_CMD_COMPONENT_ARM_DISARM:
5811 1712 : return handle_command_component_arm_disarm(packet);
5812 : #endif
5813 : #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
5814 2 : case MAV_CMD_FIXED_MAG_CAL_YAW:
5815 2 : return handle_command_fixed_mag_cal_yaw(packet);
5816 : #endif
5817 : #if COMPASS_CAL_ENABLED
5818 37 : case MAV_CMD_DO_START_MAG_CAL:
5819 : case MAV_CMD_DO_ACCEPT_MAG_CAL:
5820 : case MAV_CMD_DO_CANCEL_MAG_CAL:
5821 37 : return handle_command_mag_cal(packet);
5822 : #endif
5823 :
5824 : #if AP_BOOTLOADER_FLASHING_ENABLED
5825 : case MAV_CMD_FLASH_BOOTLOADER:
5826 : return handle_command_flash_bootloader(packet);
5827 : #endif
5828 :
5829 : #if AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
5830 4738 : case MAV_CMD_GET_HOME_POSITION:
5831 4738 : return handle_command_get_home_position(packet);
5832 : #endif // AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
5833 :
5834 54 : case MAV_CMD_PREFLIGHT_CALIBRATION:
5835 54 : return handle_command_preflight_calibration(packet, msg);
5836 :
5837 0 : case MAV_CMD_PREFLIGHT_STORAGE:
5838 0 : if (uint8_t(packet.param1) == PARAM_RESET_FACTORY_DEFAULT ||
5839 0 : uint8_t(packet.param1) == PARAM_RESET_ALL_DEFAULT) {
5840 0 : AP_Param::erase_all();
5841 0 : send_text(MAV_SEVERITY_WARNING, "All parameters reset, reboot board");
5842 0 : return MAV_RESULT_ACCEPTED;
5843 : }
5844 0 : return MAV_RESULT_DENIED;
5845 :
5846 1640 : case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
5847 1640 : return handle_preflight_reboot(packet, msg);
5848 :
5849 2 : case MAV_CMD_DO_SET_SAFETY_SWITCH_STATE:
5850 2 : return handle_do_set_safety_switch_state(packet, msg);
5851 :
5852 : #if AP_MAVLINK_SERVO_RELAY_ENABLED
5853 823 : case MAV_CMD_DO_SET_SERVO:
5854 : case MAV_CMD_DO_REPEAT_SERVO:
5855 : case MAV_CMD_DO_SET_RELAY:
5856 : case MAV_CMD_DO_REPEAT_RELAY:
5857 823 : return handle_servorelay_message(packet);
5858 : #endif
5859 :
5860 : #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
5861 28 : case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
5862 28 : return handle_command_request_autopilot_capabilities(packet);
5863 : #endif
5864 :
5865 : #if AP_ARMING_ENABLED
5866 66595 : case MAV_CMD_RUN_PREARM_CHECKS:
5867 66595 : return handle_command_run_prearm_checks(packet);
5868 : #endif
5869 :
5870 : #if AP_SCRIPTING_ENABLED
5871 10 : case MAV_CMD_SCRIPTING:
5872 : {
5873 10 : AP_Scripting *scripting = AP_Scripting::get_singleton();
5874 10 : if (scripting == nullptr) {
5875 0 : return MAV_RESULT_UNSUPPORTED;
5876 : }
5877 10 : return scripting->handle_command_int_packet(packet);
5878 : }
5879 : #endif // AP_SCRIPTING_ENABLED
5880 :
5881 : #if AP_AHRS_ENABLED
5882 6 : case MAV_CMD_SET_EKF_SOURCE_SET:
5883 6 : return handle_command_set_ekf_source_set(packet);
5884 : #endif
5885 :
5886 : #if AP_RCPROTOCOL_ENABLED
5887 0 : case MAV_CMD_START_RX_PAIR:
5888 0 : return handle_START_RX_PAIR(packet);
5889 : #endif
5890 :
5891 : #if AP_FILESYSTEM_FORMAT_ENABLED
5892 0 : case MAV_CMD_STORAGE_FORMAT:
5893 0 : return handle_command_storage_format(packet, msg);
5894 : #endif
5895 :
5896 : // support for dealing with streamrate for a specific message and
5897 : // requesting a message instance:
5898 216 : case MAV_CMD_SET_MESSAGE_INTERVAL:
5899 216 : return handle_command_set_message_interval(packet);
5900 :
5901 14 : case MAV_CMD_GET_MESSAGE_INTERVAL:
5902 14 : return handle_command_get_message_interval(packet);
5903 :
5904 450566 : case MAV_CMD_REQUEST_MESSAGE:
5905 450566 : return handle_command_request_message(packet);
5906 :
5907 : #if AP_MAVLINK_FOLLOW_HANDLING_ENABLED
5908 0 : case MAV_CMD_DO_FOLLOW:
5909 0 : return handle_command_do_follow(packet, msg);
5910 : #endif
5911 : }
5912 :
5913 0 : return MAV_RESULT_UNSUPPORTED;
5914 : }
5915 :
5916 618 : void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg)
5917 : {
5918 : // decode packet
5919 : mavlink_command_int_t packet;
5920 618 : mavlink_msg_command_int_decode(&msg, &packet);
5921 :
5922 : #if AP_SCRIPTING_ENABLED
5923 618 : AP_Scripting *scripting = AP_Scripting::get_singleton();
5924 618 : if (scripting != nullptr && scripting->is_handling_command(packet.command)) {
5925 : // Scripting has registered to receive this command, do not procces it internaly
5926 0 : return;
5927 : }
5928 : #endif
5929 :
5930 618 : hal.util->persistent_data.last_mavlink_cmd = packet.command;
5931 :
5932 618 : const MAV_RESULT result = handle_command_int_packet(packet, msg);
5933 :
5934 : // send ACK or NAK
5935 618 : mavlink_msg_command_ack_send(chan, packet.command, result,
5936 : 0, 0,
5937 618 : msg.sysid,
5938 618 : msg.compid);
5939 :
5940 : #if HAL_LOGGING_ENABLED
5941 618 : AP::logger().Write_Command(packet, msg.sysid, msg.compid, result);
5942 : #endif
5943 :
5944 618 : hal.util->persistent_data.last_mavlink_cmd = 0;
5945 : }
5946 :
5947 422 : void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) const {
5948 422 : MissionItemProtocol *prot = get_prot_for_mission_type(type);
5949 422 : if (prot == nullptr) {
5950 0 : return;
5951 : }
5952 422 : prot->queued_request_send();
5953 : }
5954 :
5955 1117385 : bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
5956 : {
5957 1117385 : switch (id) {
5958 : #if AP_MISSION_ENABLED
5959 1115338 : case MSG_CURRENT_WAYPOINT:
5960 : {
5961 1115338 : CHECK_PAYLOAD_SIZE(MISSION_CURRENT);
5962 1115338 : AP_Mission &mission = AP::mission();
5963 1115338 : send_mission_current(mission, mission.get_current_nav_index());
5964 1115338 : break;
5965 : }
5966 1625 : case MSG_MISSION_ITEM_REACHED:
5967 1625 : CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
5968 1625 : mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
5969 1625 : break;
5970 335 : case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
5971 335 : CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
5972 335 : gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_MISSION);
5973 335 : break;
5974 : #endif
5975 : #if HAL_RALLY_ENABLED
5976 6 : case MSG_NEXT_MISSION_REQUEST_RALLY:
5977 6 : CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
5978 6 : gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_RALLY);
5979 6 : break;
5980 : #endif
5981 : #if AP_FENCE_ENABLED
5982 81 : case MSG_NEXT_MISSION_REQUEST_FENCE:
5983 81 : CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
5984 81 : gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_FENCE);
5985 81 : break;
5986 : #endif
5987 0 : default:
5988 0 : break;
5989 : }
5990 1117385 : return true;
5991 : }
5992 :
5993 : #if AP_RPM_ENABLED
5994 1120967 : void GCS_MAVLINK::send_rpm() const
5995 : {
5996 1120967 : AP_RPM *rpm = AP::rpm();
5997 1120967 : if (rpm == nullptr) {
5998 1064786 : return;
5999 : }
6000 :
6001 1120967 : if (!rpm->enabled(0) && !rpm->enabled(1)) {
6002 1064786 : return;
6003 : }
6004 :
6005 56181 : float rpm1 = -1, rpm2 = -1;
6006 :
6007 56181 : rpm->get_rpm(0, rpm1);
6008 56181 : rpm->get_rpm(1, rpm2);
6009 :
6010 56181 : mavlink_msg_rpm_send(
6011 56181 : chan,
6012 : rpm1,
6013 : rpm2);
6014 : }
6015 : #endif // AP_RPM_ENABLED
6016 :
6017 1117266 : void GCS_MAVLINK::send_sys_status()
6018 : {
6019 : // send extended status only once vehicle has been initialised
6020 : // to avoid unnecessary errors being reported to user
6021 1117266 : if (!gcs().vehicle_initialised()) {
6022 319 : return;
6023 : }
6024 : #if AP_BATTERY_ENABLED
6025 1116947 : const AP_BattMonitor &battery = AP::battery();
6026 : float battery_current;
6027 1116947 : const int8_t battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE);
6028 :
6029 1116947 : if (battery.healthy() && battery.current_amps(battery_current)) {
6030 866204 : battery_current = constrain_float(battery_current * 100,-INT16_MAX,INT16_MAX);
6031 : } else {
6032 250743 : battery_current = -1;
6033 : }
6034 : #endif
6035 :
6036 : uint32_t control_sensors_present;
6037 : uint32_t control_sensors_enabled;
6038 : uint32_t control_sensors_health;
6039 :
6040 1116947 : gcs().get_sensor_status_flags(control_sensors_present, control_sensors_enabled, control_sensors_health);
6041 :
6042 1116947 : const uint32_t errors = AP::internalerror().errors();
6043 1116947 : const uint16_t errors1 = errors & 0xffff;
6044 1116947 : const uint16_t errors2 = (errors>>16) & 0xffff;
6045 1116947 : const uint16_t errors4 = AP::internalerror().count() & 0xffff;
6046 1116947 : uint16_t errors_comm = 0;
6047 1116947 : mavlink_status_t *ms = mavlink_get_channel_status(chan);
6048 1116947 : if (ms) {
6049 1116947 : errors_comm = ms->packet_rx_drop_count;
6050 : }
6051 :
6052 : #if HAL_LOGGING_ENABLED
6053 1116947 : const uint16_t dropped_logmessage_count = AP::logger().num_dropped();
6054 : #else
6055 : const uint16_t dropped_logmessage_count = UINT16_MAX;
6056 : #endif // HAL_LOGGING_ENABLED
6057 :
6058 3350841 : mavlink_msg_sys_status_send(
6059 : chan,
6060 : control_sensors_present,
6061 : control_sensors_enabled,
6062 : control_sensors_health,
6063 : #if AP_SCHEDULER_ENABLED
6064 1116947 : static_cast<uint16_t>(AP::scheduler().load_average() * 1000),
6065 : #else
6066 : 0,
6067 : #endif
6068 : #if AP_BATTERY_ENABLED
6069 1116947 : battery.gcs_voltage() * 1000, // mV
6070 : battery_current, // in 10mA units
6071 : battery_remaining, // in %
6072 : #else
6073 : 0,
6074 : -1,
6075 : -1,
6076 : #endif
6077 : 0, // comm drops %,
6078 : errors_comm, // comm drops in pkts,
6079 : errors1,
6080 : errors2,
6081 : dropped_logmessage_count, // errors3
6082 : errors4, // errors4
6083 : 0, // control_sensors_present2,
6084 : 0, // control_sensors_enabled2,
6085 : 0 // control_sensors_health2
6086 : );
6087 : }
6088 :
6089 3716 : void GCS_MAVLINK::send_extended_sys_state() const
6090 : {
6091 3716 : mavlink_msg_extended_sys_state_send(chan, vtol_state(), landed_state());
6092 3716 : }
6093 :
6094 591993 : void GCS_MAVLINK::send_attitude() const
6095 : {
6096 : #if AP_AHRS_ENABLED
6097 591993 : const AP_AHRS &ahrs = AP::ahrs();
6098 591993 : const Vector3f omega = ahrs.get_gyro();
6099 591993 : mavlink_msg_attitude_send(
6100 591993 : chan,
6101 : AP_HAL::millis(),
6102 : ahrs.get_roll_rad(),
6103 : ahrs.get_pitch_rad(),
6104 : ahrs.get_yaw_rad(),
6105 591993 : omega.x,
6106 591993 : omega.y,
6107 591993 : omega.z);
6108 : #endif
6109 591993 : }
6110 :
6111 111 : void GCS_MAVLINK::send_attitude_quaternion() const
6112 : {
6113 : #if AP_AHRS_ENABLED
6114 111 : const AP_AHRS &ahrs = AP::ahrs();
6115 111 : Quaternion quat;
6116 111 : if (!ahrs.get_quaternion(quat)) {
6117 0 : return;
6118 : }
6119 111 : const Vector3f omega = ahrs.get_gyro();
6120 111 : const float repr_offseq_q[] {0,0,0,0}; // unused, but probably should correspond to the AHRS view?
6121 222 : mavlink_msg_attitude_quaternion_send(
6122 111 : chan,
6123 : AP_HAL::millis(),
6124 : quat.q1,
6125 : quat.q2,
6126 : quat.q3,
6127 : quat.q4,
6128 111 : omega.x, // rollspeed
6129 111 : omega.y, // pitchspeed
6130 111 : omega.z, // yawspeed
6131 : repr_offseq_q
6132 : );
6133 : #endif
6134 : }
6135 :
6136 1470955 : int32_t GCS_MAVLINK::global_position_int_alt() const {
6137 1470955 : return global_position_current_loc.alt * 10UL;
6138 : }
6139 1469481 : int32_t GCS_MAVLINK::global_position_int_relative_alt() const {
6140 : #if AP_AHRS_ENABLED
6141 : float posD;
6142 1469481 : AP::ahrs().get_relative_position_D_home(posD);
6143 1469481 : posD *= -1000.0f; // change from down to up and metres to millimeters
6144 1469481 : return posD;
6145 : #else
6146 : return 0;
6147 : #endif
6148 : }
6149 :
6150 1552531 : void GCS_MAVLINK::send_global_position_int()
6151 : {
6152 : #if AP_AHRS_ENABLED
6153 1552531 : AP_AHRS &ahrs = AP::ahrs();
6154 :
6155 1552531 : UNUSED_RESULT(ahrs.get_location(global_position_current_loc)); // return value ignored; we send stale data
6156 :
6157 1552531 : Vector3f vel;
6158 1552531 : if (!ahrs.get_velocity_NED(vel)) {
6159 44218 : vel.zero();
6160 : }
6161 :
6162 1552531 : mavlink_msg_global_position_int_send(
6163 : chan,
6164 : AP_HAL::millis(),
6165 : global_position_current_loc.lat, // in 1E7 degrees
6166 : global_position_current_loc.lng, // in 1E7 degrees
6167 1552531 : global_position_int_alt(), // millimeters above ground/sea level
6168 1552531 : global_position_int_relative_alt(), // millimeters above home
6169 1552531 : vel.x * 100, // X speed cm/s (+ve North)
6170 1552531 : vel.y * 100, // Y speed cm/s (+ve East)
6171 1552531 : vel.z * 100, // Z speed cm/s (+ve Down)
6172 1552531 : ahrs.yaw_sensor); // compass heading in 1/100 degree
6173 : #endif // AP_AHRS_ENABLED
6174 1552531 : }
6175 :
6176 : #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
6177 13 : void GCS_MAVLINK::send_utm_global_position() const
6178 : {
6179 13 : uint8_t flags = 0;
6180 :
6181 : // time (UNIX epoch microseconds)
6182 13 : uint64_t time_usec = 0;
6183 : #if AP_RTC_ENABLED
6184 13 : if (AP::rtc().get_utc_usec(time_usec)) {
6185 13 : flags |= UTM_DATA_AVAIL_FLAGS_TIME_VALID;
6186 : }
6187 : #endif // AP_RTC_ENABLED
6188 :
6189 : // UAS ID from hardware UID
6190 13 : uint8_t uas_id[MAVLINK_MSG_UTM_GLOBAL_POSITION_FIELD_UAS_ID_LEN] = {};
6191 : {
6192 13 : uint8_t len = ARRAY_SIZE(uas_id);
6193 13 : hal.util->get_system_id_unformatted(uas_id, len);
6194 : }
6195 13 : flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE;
6196 :
6197 : // position
6198 13 : int32_t lat = 0;
6199 13 : int32_t lon = 0;
6200 13 : int32_t alt_mm = 0;
6201 13 : int32_t relative_alt_mm = 0;
6202 :
6203 13 : AP_AHRS &ahrs = AP::ahrs();
6204 13 : Location loc;
6205 13 : if (ahrs.get_location(loc)) {
6206 13 : lat = loc.lat;
6207 13 : lon = loc.lng;
6208 13 : flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE;
6209 : }
6210 :
6211 : #if AP_GPS_ENABLED
6212 : // the message is very specific about wanting geoid altitude. At
6213 : // the moment the only place you can get one of those in ArduPilot
6214 : // is in the AP_GPS library.
6215 13 : float undulation_m = NaNf;
6216 13 : if (AP::gps().get_undulation(undulation_m)) {
6217 : // make the assumption that GLOBAL_POSITION_INT.alt is AMSL:
6218 13 : alt_mm = global_position_int_alt();
6219 13 : alt_mm -= undulation_m * 1000; // m -> mm
6220 13 : flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE;
6221 : }
6222 : #endif // AP_GPS_ENABLED
6223 :
6224 : #if AP_TERRAIN_AVAILABLE
6225 : // the message is specific about relative_alt being "Altitude above ground"
6226 13 : AP_Terrain *terrain = AP::terrain();
6227 13 : if (terrain != nullptr) {
6228 : float above_terrain_m;
6229 : // "true" here enables extrapolation
6230 10 : if (terrain->height_above_terrain(above_terrain_m, true)) {
6231 8 : relative_alt_mm = above_terrain_m * 1000;
6232 8 : flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE;
6233 : }
6234 : }
6235 : #endif // AP_TERRAIN_AVAILABLE
6236 :
6237 : // velocity
6238 13 : int16_t vx = 0;
6239 13 : int16_t vy = 0;
6240 13 : int16_t vz = 0;
6241 13 : Vector3f vel;
6242 13 : if (ahrs.get_velocity_NED(vel)) {
6243 13 : vx = vel.x * 100;
6244 13 : vy = vel.y * 100;
6245 13 : vz = vel.z * 100;
6246 13 : flags |= UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE;
6247 13 : flags |= UTM_DATA_AVAIL_FLAGS_VERTICAL_VELO_AVAILABLE;
6248 : }
6249 :
6250 : // next waypoint from vehicle-specific navigation
6251 13 : int32_t next_lat = 0;
6252 13 : int32_t next_lon = 0;
6253 13 : int32_t next_alt_mm = 0;
6254 : // the altitude must be wgs84, need undulation for that:
6255 : #if AP_GPS_ENABLED
6256 13 : Location next_loc;
6257 26 : if (!isnan(undulation_m) &&
6258 26 : get_target_location(next_loc) && next_loc.initialised()) {
6259 : float next_alt_m;
6260 8 : if (next_loc.get_alt_m(Location::AltFrame::ABSOLUTE, next_alt_m)) {
6261 8 : next_lat = next_loc.lat;
6262 8 : next_lon = next_loc.lng;
6263 8 : next_alt_mm = (int32_t)(next_alt_m * 1000.0f);
6264 8 : next_alt_mm -= undulation_m * 1000; // m -> mm
6265 8 : flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE;
6266 : }
6267 : }
6268 : #endif // AP_GPS_ENABLED
6269 :
6270 : // flight state mapped from landed_state()
6271 13 : uint8_t flight_state = UTM_FLIGHT_STATE_UNKNOWN;
6272 13 : switch (landed_state()) {
6273 5 : case MAV_LANDED_STATE_UNDEFINED:
6274 : case MAV_LANDED_STATE_ENUM_END:
6275 5 : flight_state = UTM_FLIGHT_STATE_UNKNOWN;
6276 5 : break;
6277 2 : case MAV_LANDED_STATE_ON_GROUND:
6278 2 : flight_state = UTM_FLIGHT_STATE_GROUND;
6279 2 : break;
6280 6 : case MAV_LANDED_STATE_IN_AIR:
6281 : case MAV_LANDED_STATE_TAKEOFF:
6282 : case MAV_LANDED_STATE_LANDING:
6283 6 : flight_state = UTM_FLIGHT_STATE_AIRBORNE;
6284 6 : break;
6285 : }
6286 :
6287 13 : uint16_t h_acc_mm = 0, v_acc_mm = 0, vel_acc_cms = 0;
6288 : {
6289 : float pos_horiz_m, pos_vert_m, vel_m_s;
6290 13 : if (ahrs.get_pos_vel_uncertainty(pos_horiz_m, pos_vert_m, vel_m_s)) {
6291 13 : h_acc_mm = (uint16_t)constrain_float(pos_horiz_m * 1000.0f, 0, UINT16_MAX);
6292 13 : v_acc_mm = (uint16_t)constrain_float(pos_vert_m * 1000.0f, 0, UINT16_MAX);
6293 13 : vel_acc_cms = (uint16_t)constrain_float(vel_m_s * 100.0f, 0, UINT16_MAX);
6294 : }
6295 : }
6296 :
6297 13 : mavlink_utm_global_position_t msg_utm {
6298 : time : time_usec,
6299 : lat : lat,
6300 : lon : lon,
6301 : alt : alt_mm,
6302 : relative_alt : relative_alt_mm,
6303 : next_lat : next_lat,
6304 : next_lon : next_lon,
6305 : next_alt : next_alt_mm,
6306 : vx : vx,
6307 : vy : vy,
6308 : vz : vz,
6309 : h_acc : h_acc_mm,
6310 : v_acc : v_acc_mm,
6311 : vel_acc : vel_acc_cms,
6312 : update_rate : 0, // unknown/data-driven
6313 : flight_state : flight_state,
6314 : flags : flags,
6315 13 : };
6316 13 : memcpy(msg_utm.uas_id, uas_id, sizeof(msg_utm.uas_id));
6317 13 : mavlink_msg_utm_global_position_send_struct(chan, &msg_utm);
6318 13 : }
6319 : #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
6320 :
6321 : #if HAL_MOUNT_ENABLED
6322 1117755 : void GCS_MAVLINK::send_gimbal_device_attitude_status() const
6323 : {
6324 1117755 : AP_Mount *mount = AP::mount();
6325 1117755 : if (mount == nullptr) {
6326 1760 : return;
6327 : }
6328 1115995 : mount->send_gimbal_device_attitude_status(chan);
6329 : }
6330 :
6331 0 : void GCS_MAVLINK::send_gimbal_manager_information() const
6332 : {
6333 0 : AP_Mount *mount = AP::mount();
6334 0 : if (mount == nullptr) {
6335 0 : return;
6336 : }
6337 0 : mount->send_gimbal_manager_information(chan);
6338 : }
6339 :
6340 25 : void GCS_MAVLINK::send_gimbal_manager_status() const
6341 : {
6342 25 : AP_Mount *mount = AP::mount();
6343 25 : if (mount == nullptr) {
6344 0 : return;
6345 : }
6346 25 : mount->send_gimbal_manager_status(chan);
6347 : }
6348 : #endif
6349 :
6350 0 : void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
6351 : {
6352 :
6353 0 : const uint16_t type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | \
6354 : POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | \
6355 : POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
6356 :
6357 : // convert altitude to relative to home
6358 : float rel_alt_m;
6359 0 : if (!loc.get_alt_m(Location::AltFrame::ABOVE_HOME, rel_alt_m)) {
6360 0 : return;
6361 : }
6362 :
6363 0 : mavlink_msg_set_position_target_global_int_send(
6364 : chan,
6365 : AP_HAL::millis(),
6366 : target_system,
6367 : target_component,
6368 : MAV_FRAME_GLOBAL_RELATIVE_ALT,
6369 : type_mask,
6370 0 : loc.lat,
6371 0 : loc.lng,
6372 : rel_alt_m,
6373 : 0,0,0, // vx, vy, vz
6374 : 0,0,0, // ax, ay, az
6375 : 0,0); // yaw, yaw_rate
6376 : }
6377 :
6378 1117087 : void GCS_MAVLINK::send_position_target_global_int()
6379 : {
6380 1117087 : Location target;
6381 1117087 : if (!get_target_location(target)) {
6382 558559 : return;
6383 : }
6384 572722 : if (!target.initialised()) {
6385 10051 : return;
6386 : }
6387 : float alt_amsl_m;
6388 562671 : if (!target.get_alt_m(Location::AltFrame::ABSOLUTE, alt_amsl_m)) {
6389 4143 : return;
6390 : }
6391 : static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
6392 : static constexpr uint16_t TYPE_MASK =
6393 : POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE |
6394 : POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE |
6395 : POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
6396 : POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE |
6397 : POSITION_TARGET_TYPEMASK_LAST_BYTE;
6398 558528 : mavlink_msg_position_target_global_int_send(
6399 : chan,
6400 : AP_HAL::millis(), // time_boot_ms
6401 : MAV_FRAME_GLOBAL, // targets are always global altitude
6402 : TYPE_MASK, // ignore everything except the x/y/z components
6403 : target.lat, // latitude as 1e7
6404 : target.lng, // longitude as 1e7
6405 : alt_amsl_m, // altitude AMSL in metres
6406 : 0.0f, // vx
6407 : 0.0f, // vy
6408 : 0.0f, // vz
6409 : 0.0f, // afx
6410 : 0.0f, // afy
6411 : 0.0f, // afz
6412 : 0.0f, // yaw
6413 : 0.0f); // yaw_rate
6414 : }
6415 :
6416 : #if HAL_GENERATOR_ENABLED
6417 1122504 : void GCS_MAVLINK::send_generator_status() const
6418 : {
6419 1122504 : AP_Generator *generator = AP::generator();
6420 1122504 : if (generator == nullptr) {
6421 0 : return;
6422 : }
6423 1122504 : generator->send_generator_status(*this);
6424 : }
6425 : #endif
6426 :
6427 : #if HAL_ADSB_ENABLED
6428 0 : void GCS_MAVLINK::send_uavionix_adsb_out_status() const
6429 : {
6430 0 : AP_ADSB *adsb = AP::ADSB();
6431 0 : if (adsb != nullptr) {
6432 0 : adsb->send_adsb_out_status(chan);
6433 : }
6434 0 : }
6435 : #endif
6436 :
6437 : #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
6438 377 : bool GCS_MAVLINK::send_relay_status() const
6439 : {
6440 377 : AP_Relay *relay = AP::relay();
6441 377 : if (relay == nullptr) {
6442 : // must only return false if out of space:
6443 0 : return true;
6444 : }
6445 :
6446 377 : return relay->send_relay_status(*this);
6447 : }
6448 : #endif // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
6449 :
6450 11602 : void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const
6451 : {
6452 : #if AP_AHRS_ENABLED
6453 : // get attitude
6454 11602 : const AP_AHRS &ahrs = AP::ahrs();
6455 11602 : Quaternion quat;
6456 11602 : if (!ahrs.get_quaternion(quat)) {
6457 0 : return;
6458 : }
6459 11602 : const float repr_offseq_q[] = {quat.q1, quat.q2, quat.q3, quat.q4};
6460 :
6461 : // get velocity
6462 11602 : Vector3f vel;
6463 11602 : if (!ahrs.get_velocity_NED(vel)) {
6464 0 : vel.zero();
6465 : }
6466 :
6467 : // get vehicle earth-frame rotation rate targets
6468 11602 : Vector3f rate_ef_targets;
6469 : #if AP_VEHICLE_ENABLED
6470 11602 : const AP_Vehicle *vehicle = AP::vehicle();
6471 11602 : if (vehicle != nullptr) {
6472 11602 : vehicle->get_rate_ef_targets(rate_ef_targets);
6473 : }
6474 : #endif
6475 :
6476 : // get estimator flags
6477 11602 : uint16_t est_status_flags = 0;
6478 : nav_filter_status nav_filt_status;
6479 11602 : if (ahrs.get_filter_status(nav_filt_status)) {
6480 11602 : est_status_flags = (uint16_t)(nav_filt_status.value & 0xFFFF);
6481 : }
6482 :
6483 11602 : mavlink_msg_autopilot_state_for_gimbal_device_send(
6484 11602 : chan,
6485 11602 : mavlink_system.sysid, // target system (this autopilot's gimbal)
6486 : 0, // target component (anything)
6487 11602 : AP_HAL::micros(), // time boot us
6488 : repr_offseq_q, // attitude as quaternion
6489 : 0, // attitude estimated delay in micros
6490 : vel.x, // x speed in NED (m/s)
6491 : vel.y, // y speed in NED (m/s)
6492 : vel.z, // z speed in NED (m/s)
6493 : 0, // velocity estimated delay in micros
6494 : rate_ef_targets.z, // feed forward angular velocity z
6495 : est_status_flags, // estimator status
6496 : 0, // landed_state (see MAV_LANDED_STATE)
6497 11602 : AP::ahrs().get_yaw_rate_earth()); // [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown
6498 : #endif // AP_AHRS_ENABLED
6499 : }
6500 :
6501 : #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
6502 0 : void GCS_MAVLINK::send_flight_information()
6503 : {
6504 0 : const uint64_t time_boot_micros = AP_HAL::micros64();
6505 0 : const uint32_t time_boot_ms = static_cast<uint32_t>(time_boot_micros / 1000);
6506 :
6507 : // This field is misnamed as `arming_time_utc` in MAVLink. However, it is
6508 : // not a UTC time, it is the microseconds since boot.
6509 0 : const uint64_t arm_time_us = AP::arming().arm_time_us();
6510 :
6511 0 : const MAV_LANDED_STATE current_landed_state = landed_state();
6512 0 : if (flight_info.last_landed_state != current_landed_state) {
6513 0 : switch (current_landed_state) {
6514 0 : case MAV_LANDED_STATE_IN_AIR:
6515 : case MAV_LANDED_STATE_TAKEOFF:
6516 : case MAV_LANDED_STATE_LANDING:
6517 0 : if (!flight_info.takeoff_time_us) {
6518 0 : flight_info.takeoff_time_us = time_boot_micros;
6519 : }
6520 0 : break;
6521 :
6522 0 : case MAV_LANDED_STATE_ON_GROUND:
6523 0 : flight_info.takeoff_time_us = 0;
6524 0 : break;
6525 :
6526 0 : case MAV_LANDED_STATE_UNDEFINED:
6527 : case MAV_LANDED_STATE_ENUM_END:
6528 0 : break;
6529 : }
6530 :
6531 0 : flight_info.last_landed_state = current_landed_state;
6532 : }
6533 :
6534 : // This field is misnamed as `takeoff_time_utc` in MAVLink. However, it is
6535 : // not a UTC time, it is the microseconds since boot.
6536 0 : uint64_t takeoff_time_us = flight_info.takeoff_time_us;
6537 :
6538 : // This field is misnamed as `flight_uuid` in MAVLink.
6539 0 : const uint64_t flight_number = 0;
6540 :
6541 0 : mavlink_msg_flight_information_send(
6542 : chan,
6543 : time_boot_ms,
6544 : arm_time_us,
6545 : takeoff_time_us,
6546 : flight_number,
6547 : 0 // landing_time, 0 meaning "not supported"
6548 : );
6549 0 : }
6550 : #endif // AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
6551 :
6552 0 : void GCS_MAVLINK::send_received_message_deprecation_warning(const char * message)
6553 : {
6554 : // we're not expecting very many of these ever, so a tiny bit of
6555 : // de-duping is probably OK:
6556 0 : if (message == last_deprecation_message) {
6557 0 : return;
6558 : }
6559 :
6560 0 : const uint32_t now_ms = AP_HAL::millis();
6561 0 : if (last_deprecation_warning_send_time_ms - now_ms < 30000) {
6562 0 : return;
6563 : }
6564 0 : last_deprecation_warning_send_time_ms = now_ms;
6565 0 : last_deprecation_message = message;
6566 :
6567 0 : send_text(MAV_SEVERITY_INFO, "Received message (%s) is deprecated", message);
6568 : }
6569 :
6570 705655 : bool GCS_MAVLINK::send_available_modes()
6571 : {
6572 705655 : if (!available_modes.should_send) {
6573 : // must only return false if out of space
6574 705655 : return true;
6575 : }
6576 :
6577 0 : CHECK_PAYLOAD_SIZE(AVAILABLE_MODES);
6578 :
6579 : // Zero is a special case for send all.
6580 0 : const bool send_all = available_modes.requested_index == 0;
6581 : uint8_t request_index;
6582 0 : if (!send_all) {
6583 : // Single request
6584 0 : request_index = available_modes.requested_index;
6585 0 : available_modes.should_send = false;
6586 :
6587 : } else {
6588 : // Request all modes
6589 0 : request_index = available_modes.next_index;
6590 0 : available_modes.next_index += 1;
6591 : }
6592 :
6593 0 : const uint8_t mode_count = send_available_mode(request_index);
6594 :
6595 0 : if (send_all && (available_modes.next_index > mode_count)) {
6596 : // Sending all and just sent the last
6597 0 : available_modes.should_send = false;
6598 : }
6599 :
6600 0 : return true;
6601 : }
6602 :
6603 0 : bool GCS_MAVLINK::send_available_mode_monitor()
6604 : {
6605 0 : CHECK_PAYLOAD_SIZE(AVAILABLE_MODES_MONITOR);
6606 :
6607 0 : mavlink_msg_available_modes_monitor_send(
6608 : chan,
6609 0 : available_modes.available_modes_sequence
6610 : );
6611 :
6612 0 : return true;
6613 : }
6614 :
6615 81 : void GCS_MAVLINK::available_modes_changed()
6616 : {
6617 : // Only increment the counter if this channel is tracking modes
6618 81 : if (!available_modes.requested) {
6619 81 : return;
6620 : }
6621 :
6622 0 : available_modes.available_modes_sequence += 1;
6623 :
6624 : // Stream the AVAILABLE_MODES_MONITOR message
6625 : // This allows the GCS to re-request modes
6626 0 : set_ap_message_interval(MSG_AVAILABLE_MODES_MONITOR, 5000);
6627 : }
6628 :
6629 137647384 : bool GCS_MAVLINK::try_send_message(const enum ap_message id)
6630 : {
6631 137647384 : bool ret = true;
6632 :
6633 137647384 : switch(id) {
6634 :
6635 : #if AP_AHRS_ENABLED
6636 1144486 : case MSG_ATTITUDE:
6637 1144486 : CHECK_PAYLOAD_SIZE(ATTITUDE);
6638 1144401 : send_attitude();
6639 1144401 : break;
6640 :
6641 111 : case MSG_ATTITUDE_QUATERNION:
6642 111 : CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION);
6643 111 : send_attitude_quaternion();
6644 111 : break;
6645 : #endif
6646 :
6647 84066992 : case MSG_NEXT_PARAM:
6648 84066992 : CHECK_PAYLOAD_SIZE(PARAM_VALUE);
6649 1625060 : queued_param_send();
6650 1625060 : break;
6651 :
6652 269141 : case MSG_HEARTBEAT:
6653 269141 : CHECK_PAYLOAD_SIZE(HEARTBEAT);
6654 170070 : last_heartbeat_time = AP_HAL::millis();
6655 170070 : send_heartbeat();
6656 170070 : break;
6657 :
6658 : #if AP_AHRS_ENABLED
6659 1552831 : case MSG_LOCATION:
6660 1552831 : CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
6661 1552828 : send_global_position_int();
6662 1552828 : break;
6663 :
6664 11357 : case MSG_HOME:
6665 11357 : CHECK_PAYLOAD_SIZE(HOME_POSITION);
6666 11357 : send_home_position();
6667 11357 : break;
6668 :
6669 12085 : case MSG_ORIGIN:
6670 12085 : CHECK_PAYLOAD_SIZE(GPS_GLOBAL_ORIGIN);
6671 11850 : send_gps_global_origin();
6672 11850 : break;
6673 : #endif // AP_AHRS_ENABLED
6674 :
6675 : #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
6676 13 : case MSG_UTM_GLOBAL_POSITION:
6677 13 : CHECK_PAYLOAD_SIZE(UTM_GLOBAL_POSITION);
6678 13 : send_utm_global_position();
6679 13 : break;
6680 : #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
6681 :
6682 : #if AP_RPM_ENABLED
6683 1120967 : case MSG_RPM:
6684 1120967 : CHECK_PAYLOAD_SIZE(RPM);
6685 1120967 : send_rpm();
6686 1120967 : break;
6687 : #endif
6688 :
6689 1117385 : case MSG_CURRENT_WAYPOINT:
6690 : case MSG_MISSION_ITEM_REACHED:
6691 : case MSG_NEXT_MISSION_REQUEST_WAYPOINTS:
6692 : case MSG_NEXT_MISSION_REQUEST_RALLY:
6693 : case MSG_NEXT_MISSION_REQUEST_FENCE:
6694 1117385 : ret = try_send_mission_message(id);
6695 1117385 : break;
6696 :
6697 : #if COMPASS_CAL_ENABLED
6698 1117755 : case MSG_MAG_CAL_PROGRESS:
6699 1117755 : ret = AP::compass().send_mag_cal_progress(*this);
6700 1117755 : break;
6701 1117755 : case MSG_MAG_CAL_REPORT:
6702 1117755 : ret = AP::compass().send_mag_cal_report(*this);
6703 1117755 : break;
6704 : #endif
6705 :
6706 : #if AP_BATTERY_ENABLED
6707 1117755 : case MSG_BATTERY_STATUS:
6708 1117755 : send_battery_status();
6709 1117755 : break;
6710 : #endif // AP_BATTERY_ENABLED
6711 :
6712 : #if AP_AHRS_ENABLED
6713 1117755 : case MSG_EKF_STATUS_REPORT:
6714 1117755 : CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
6715 1117755 : AP::ahrs().send_ekf_status_report(*this);
6716 1117755 : break;
6717 : #endif
6718 :
6719 1117088 : case MSG_MEMINFO:
6720 1117088 : CHECK_PAYLOAD_SIZE(MEMINFO);
6721 1117088 : send_meminfo();
6722 1117088 : break;
6723 :
6724 : #if AP_FENCE_ENABLED
6725 1117208 : case MSG_FENCE_STATUS:
6726 1117208 : CHECK_PAYLOAD_SIZE(FENCE_STATUS);
6727 1117208 : send_fence_status();
6728 1117208 : break;
6729 : #endif
6730 :
6731 : #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
6732 4400 : case MSG_RANGEFINDER:
6733 4400 : CHECK_PAYLOAD_SIZE(RANGEFINDER);
6734 4400 : send_rangefinder();
6735 4400 : break;
6736 : #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
6737 :
6738 1117775 : case MSG_DISTANCE_SENSOR:
6739 1117775 : send_distance_sensor();
6740 1117775 : break;
6741 :
6742 : #if AP_CAMERA_ENABLED
6743 2884 : case MSG_CAMERA_FEEDBACK:
6744 : case MSG_CAMERA_INFORMATION:
6745 : case MSG_CAMERA_SETTINGS:
6746 : #if AP_CAMERA_SEND_FOV_STATUS_ENABLED
6747 : case MSG_CAMERA_FOV_STATUS:
6748 : #endif
6749 : case MSG_CAMERA_CAPTURE_STATUS:
6750 : #if AP_CAMERA_SEND_THERMAL_RANGE_ENABLED
6751 : case MSG_CAMERA_THERMAL_RANGE:
6752 : #endif
6753 : #if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
6754 : case MSG_VIDEO_STREAM_INFORMATION:
6755 : #endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
6756 : {
6757 2884 : AP_Camera *camera = AP::camera();
6758 2884 : if (camera == nullptr) {
6759 0 : break;
6760 : }
6761 2884 : return camera->send_mavlink_message(*this, id);
6762 : }
6763 : #endif // AP_CAMERA_ENABLED
6764 :
6765 1117756 : case MSG_SYSTEM_TIME:
6766 1117756 : CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
6767 1117756 : send_system_time();
6768 1117756 : break;
6769 :
6770 : #if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
6771 1117205 : case MSG_GPS_RAW:
6772 1117205 : CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
6773 1117165 : AP::gps().send_mavlink_gps_raw(chan);
6774 1117165 : break;
6775 : #endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED
6776 : #if AP_GPS_GPS_RTK_SENDING_ENABLED
6777 1117087 : case MSG_GPS_RTK:
6778 1117087 : CHECK_PAYLOAD_SIZE(GPS_RTK);
6779 1117087 : AP::gps().send_mavlink_gps_rtk(chan, 0);
6780 1117087 : break;
6781 : #endif // AP_GPS_GPS_RTK_SENDING_ENABLED
6782 : #if AP_GPS_GPS2_RAW_SENDING_ENABLED
6783 1117087 : case MSG_GPS2_RAW:
6784 1117087 : CHECK_PAYLOAD_SIZE(GPS2_RAW);
6785 1117087 : AP::gps().send_mavlink_gps2_raw(chan);
6786 1117087 : break;
6787 : #endif // AP_GPS_GPS2_RAW_SENDING_ENABLED
6788 : #if AP_GPS_GPS2_RTK_SENDING_ENABLED
6789 1117087 : case MSG_GPS2_RTK:
6790 1117087 : CHECK_PAYLOAD_SIZE(GPS2_RTK);
6791 1117087 : AP::gps().send_mavlink_gps_rtk(chan, 1);
6792 1117087 : break;
6793 : #endif // AP_GPS_GPS2_RTK_SENDING_ENABLED
6794 :
6795 : #if AP_AHRS_ENABLED
6796 1119336 : case MSG_LOCAL_POSITION:
6797 1119336 : CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
6798 1119336 : send_local_position();
6799 1119336 : break;
6800 : #endif
6801 :
6802 : #if HAL_MOUNT_ENABLED
6803 1117867 : case MSG_GIMBAL_DEVICE_ATTITUDE_STATUS:
6804 1117867 : CHECK_PAYLOAD_SIZE(GIMBAL_DEVICE_ATTITUDE_STATUS);
6805 1117755 : send_gimbal_device_attitude_status();
6806 1117755 : break;
6807 11602 : case MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE:
6808 11602 : CHECK_PAYLOAD_SIZE(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE);
6809 11602 : send_autopilot_state_for_gimbal_device();
6810 11602 : break;
6811 0 : case MSG_GIMBAL_MANAGER_INFORMATION:
6812 0 : CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
6813 0 : send_gimbal_manager_information();
6814 0 : break;
6815 25 : case MSG_GIMBAL_MANAGER_STATUS:
6816 25 : CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS);
6817 25 : send_gimbal_manager_status();
6818 25 : break;
6819 : #endif // HAL_MOUNT_ENABLED
6820 :
6821 : #if AP_OPTICALFLOW_ENABLED
6822 1117756 : case MSG_OPTICAL_FLOW:
6823 1117756 : CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
6824 1117755 : send_opticalflow();
6825 1117755 : break;
6826 : #endif
6827 :
6828 14691 : case MSG_ATTITUDE_TARGET:
6829 14691 : CHECK_PAYLOAD_SIZE(ATTITUDE_TARGET);
6830 14690 : send_attitude_target();
6831 14690 : break;
6832 :
6833 1117087 : case MSG_POSITION_TARGET_GLOBAL_INT:
6834 1117087 : CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
6835 1117087 : send_position_target_global_int();
6836 1117087 : break;
6837 :
6838 4724 : case MSG_POSITION_TARGET_LOCAL_NED:
6839 4724 : CHECK_PAYLOAD_SIZE(POSITION_TARGET_LOCAL_NED);
6840 4724 : send_position_target_local_ned();
6841 4724 : break;
6842 :
6843 1117088 : case MSG_POWER_STATUS:
6844 1117088 : CHECK_PAYLOAD_SIZE(POWER_STATUS);
6845 1117088 : send_power_status();
6846 1117088 : break;
6847 :
6848 : #if HAL_WITH_MCU_MONITORING
6849 : case MSG_MCU_STATUS:
6850 : CHECK_PAYLOAD_SIZE(MCU_STATUS);
6851 : send_mcu_status();
6852 : break;
6853 : #endif
6854 :
6855 : #if AP_RC_CHANNEL_ENABLED
6856 1117302 : case MSG_RC_CHANNELS:
6857 1117302 : CHECK_PAYLOAD_SIZE(RC_CHANNELS);
6858 1117302 : send_rc_channels();
6859 1117302 : break;
6860 :
6861 : #if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
6862 1117302 : case MSG_RC_CHANNELS_RAW:
6863 1117302 : CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
6864 1117302 : send_rc_channels_raw();
6865 1117302 : break;
6866 : #endif // AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
6867 :
6868 : #endif
6869 :
6870 1117608 : case MSG_RAW_IMU:
6871 1117608 : CHECK_PAYLOAD_SIZE(RAW_IMU);
6872 1117608 : send_raw_imu();
6873 1117608 : break;
6874 :
6875 0 : case MSG_SCALED_IMU:
6876 0 : CHECK_PAYLOAD_SIZE(SCALED_IMU);
6877 0 : send_scaled_imu(0, mavlink_msg_scaled_imu_send);
6878 0 : break;
6879 :
6880 1117088 : case MSG_SCALED_IMU2:
6881 1117088 : CHECK_PAYLOAD_SIZE(SCALED_IMU2);
6882 1117088 : send_scaled_imu(1, mavlink_msg_scaled_imu2_send);
6883 1117088 : break;
6884 :
6885 1117088 : case MSG_SCALED_IMU3:
6886 1117088 : CHECK_PAYLOAD_SIZE(SCALED_IMU3);
6887 1117088 : send_scaled_imu(2, mavlink_msg_scaled_imu3_send);
6888 1117088 : break;
6889 : #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
6890 0 : case MSG_HIGHRES_IMU:
6891 0 : CHECK_PAYLOAD_SIZE(HIGHRES_IMU);
6892 0 : send_highres_imu();
6893 0 : break;
6894 : #endif
6895 :
6896 1117304 : case MSG_SCALED_PRESSURE:
6897 1117304 : CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
6898 1117303 : send_scaled_pressure();
6899 1117303 : break;
6900 :
6901 1117088 : case MSG_SCALED_PRESSURE2:
6902 1117088 : CHECK_PAYLOAD_SIZE(SCALED_PRESSURE2);
6903 1117088 : send_scaled_pressure2();
6904 1117088 : break;
6905 :
6906 1117088 : case MSG_SCALED_PRESSURE3:
6907 1117088 : CHECK_PAYLOAD_SIZE(SCALED_PRESSURE3);
6908 1117088 : send_scaled_pressure3();
6909 1117088 : break;
6910 :
6911 : #if AP_AIRSPEED_ENABLED
6912 1117087 : case MSG_AIRSPEED:
6913 1117087 : CHECK_PAYLOAD_SIZE(AIRSPEED);
6914 1117087 : send_airspeed();
6915 1117087 : break;
6916 : #endif
6917 :
6918 1131807 : case MSG_SERVO_OUTPUT_RAW:
6919 1131807 : CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
6920 1131807 : send_servo_output_raw();
6921 1131807 : break;
6922 :
6923 : #if AP_SIM_ENABLED
6924 1128799 : case MSG_SIMSTATE:
6925 1128799 : CHECK_PAYLOAD_SIZE(SIMSTATE);
6926 1127758 : send_simstate();
6927 1127758 : break;
6928 :
6929 2791 : case MSG_SIM_STATE:
6930 2791 : CHECK_PAYLOAD_SIZE(SIM_STATE);
6931 2791 : send_sim_state();
6932 2791 : break;
6933 : #endif
6934 :
6935 1117272 : case MSG_SYS_STATUS:
6936 1117272 : CHECK_PAYLOAD_SIZE(SYS_STATUS);
6937 1117266 : send_sys_status();
6938 1117266 : break;
6939 :
6940 : #if AP_AHRS_ENABLED
6941 1121064 : case MSG_AHRS2:
6942 1121064 : CHECK_PAYLOAD_SIZE(AHRS2);
6943 1120989 : send_ahrs2();
6944 1120989 : break;
6945 : #endif
6946 :
6947 1120967 : case MSG_PID_TUNING:
6948 1120967 : CHECK_PAYLOAD_SIZE(PID_TUNING);
6949 1120967 : send_pid_tuning();
6950 1120967 : break;
6951 :
6952 1117088 : case MSG_NAV_CONTROLLER_OUTPUT:
6953 1117088 : CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
6954 1117088 : send_nav_controller_output();
6955 1117088 : break;
6956 :
6957 : #if AP_AHRS_ENABLED
6958 1117821 : case MSG_AHRS:
6959 1117821 : CHECK_PAYLOAD_SIZE(AHRS);
6960 1117757 : send_ahrs();
6961 1117757 : break;
6962 : #endif
6963 :
6964 3716 : case MSG_EXTENDED_SYS_STATE:
6965 3716 : CHECK_PAYLOAD_SIZE(EXTENDED_SYS_STATE);
6966 3716 : send_extended_sys_state();
6967 3716 : break;
6968 :
6969 : #if AP_AHRS_ENABLED
6970 1119092 : case MSG_VFR_HUD:
6971 1119092 : CHECK_PAYLOAD_SIZE(VFR_HUD);
6972 1119088 : send_vfr_hud();
6973 1119088 : break;
6974 : #endif
6975 :
6976 1115995 : case MSG_VIBRATION:
6977 1115995 : CHECK_PAYLOAD_SIZE(VIBRATION);
6978 1115995 : send_vibration();
6979 1115995 : break;
6980 :
6981 : #if HAL_GENERATOR_ENABLED
6982 1122504 : case MSG_GENERATOR_STATUS:
6983 1122504 : CHECK_PAYLOAD_SIZE(GENERATOR_STATUS);
6984 1122504 : send_generator_status();
6985 1122504 : break;
6986 : #endif
6987 :
6988 30 : case MSG_AUTOPILOT_VERSION:
6989 30 : CHECK_PAYLOAD_SIZE(AUTOPILOT_VERSION);
6990 30 : send_autopilot_version();
6991 30 : break;
6992 :
6993 : #if HAL_WITH_ESC_TELEM
6994 1120967 : case MSG_ESC_TELEMETRY:
6995 1120967 : AP::esc_telem().send_esc_telemetry_mavlink(uint8_t(chan));
6996 1120967 : break;
6997 : #endif
6998 :
6999 : #if HAL_EFI_ENABLED
7000 1122206 : case MSG_EFI_STATUS: {
7001 1122206 : CHECK_PAYLOAD_SIZE(EFI_STATUS);
7002 1122204 : AP_EFI *efi = AP::EFI();
7003 1122204 : if (efi) {
7004 1122204 : efi->send_mavlink_status(chan);
7005 : }
7006 1122204 : break;
7007 : }
7008 : #endif
7009 :
7010 : #if AP_WINCH_ENABLED
7011 1120967 : case MSG_WINCH_STATUS:
7012 1120967 : CHECK_PAYLOAD_SIZE(WINCH_STATUS);
7013 1120967 : send_winch_status();
7014 1120967 : break;
7015 : #endif
7016 :
7017 : #if HAL_HIGH_LATENCY2_ENABLED
7018 1456 : case MSG_HIGH_LATENCY2:
7019 1456 : CHECK_PAYLOAD_SIZE(HIGH_LATENCY2);
7020 1456 : send_high_latency2();
7021 1456 : break;
7022 : #endif // HAL_HIGH_LATENCY2_ENABLED
7023 :
7024 : #if AP_AIS_ENABLED
7025 1114539 : case MSG_AIS_VESSEL: {
7026 1114539 : AP_AIS *ais = AP_AIS::get_singleton();
7027 1114539 : if (ais) {
7028 124162 : ais->send(chan);
7029 : }
7030 1114539 : break;
7031 : }
7032 : #endif
7033 :
7034 : #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
7035 0 : case MSG_UAVIONIX_ADSB_OUT_STATUS:
7036 0 : CHECK_PAYLOAD_SIZE(UAVIONIX_ADSB_OUT_STATUS);
7037 0 : send_uavionix_adsb_out_status();
7038 0 : break;
7039 : #endif
7040 :
7041 : #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
7042 377 : case MSG_RELAY_STATUS:
7043 377 : ret = send_relay_status();
7044 377 : break;
7045 : #endif
7046 :
7047 705655 : case MSG_AVAILABLE_MODES:
7048 705655 : ret = send_available_modes();
7049 705655 : break;
7050 :
7051 0 : case MSG_AVAILABLE_MODES_MONITOR:
7052 0 : ret = send_available_mode_monitor();
7053 0 : break;
7054 :
7055 : #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
7056 0 : case MSG_FLIGHT_INFORMATION:
7057 0 : CHECK_PAYLOAD_SIZE(FLIGHT_INFORMATION);
7058 0 : send_flight_information();
7059 0 : break;
7060 : #endif
7061 :
7062 : #if AP_TERRAIN_AVAILABLE
7063 1117756 : case MSG_TERRAIN_REQUEST: {
7064 1117756 : CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
7065 1117756 : AP_Terrain *terrain = AP::terrain();
7066 1117756 : if (terrain != nullptr) {
7067 991097 : terrain->send_request(*this);
7068 : }
7069 1117756 : break;
7070 : }
7071 1117756 : case MSG_TERRAIN_REPORT: {
7072 1117756 : CHECK_PAYLOAD_SIZE(TERRAIN_REPORT);
7073 1117756 : AP_Terrain *terrain = AP::terrain();
7074 1117756 : if (terrain != nullptr) {
7075 991097 : terrain->send_report(*this);
7076 : }
7077 1117756 : break;
7078 : }
7079 : #endif // AP_TERRAIN_AVAILABLE
7080 :
7081 : #if AP_AIRSPEED_HYGROMETER_ENABLE
7082 : // hygrometer should be in its down library, not called via
7083 : // Plane's functions
7084 619663 : case MSG_HYGROMETER:
7085 : #endif
7086 : // ADSB should be moved down into here
7087 : case MSG_ADSB_VEHICLE:
7088 619663 : break;
7089 :
7090 0 : default:
7091 : // try_send_message must always at some stage return true for
7092 : // a message, or we will attempt to infinitely retry the
7093 : // message as part of send_message.
7094 : // This message will be sent out at the same rate as the
7095 : // unknown message, so should be safe.
7096 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
7097 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
7098 0 : AP_HAL::panic("Sending unknown ap_message %u", id);
7099 : #endif
7100 0 : break;
7101 : }
7102 :
7103 55101827 : return ret;
7104 : }
7105 :
7106 82485 : uint16_t GCS_MAVLINK::get_interval_for_stream(GCS_MAVLINK::streams id) const
7107 : {
7108 82485 : const int16_t frate = streamRates[id].get();
7109 82485 : if (frate == 0) {
7110 32173 : return 0;
7111 : }
7112 50312 : const uint32_t ret = 1000/frate;
7113 50312 : if (ret > 60000) {
7114 0 : return 60000;
7115 : }
7116 50312 : return ret;
7117 : }
7118 :
7119 82480 : void GCS_MAVLINK::initialise_message_intervals_for_stream(GCS_MAVLINK::streams id)
7120 : {
7121 446215 : for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
7122 446215 : const GCS_MAVLINK::stream_entries &entries = all_stream_entries[i];
7123 446215 : if (entries.stream_id != id) {
7124 363735 : continue;
7125 : }
7126 : // found it!
7127 82480 : const uint16_t interval_ms = get_interval_for_stream(id);
7128 517649 : for (uint8_t j=0; j<entries.num_ap_message_ids; j++) {
7129 435169 : set_ap_message_interval(entries.ap_message_ids[j], interval_ms);
7130 : }
7131 82480 : break;
7132 : }
7133 82480 : }
7134 :
7135 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
7136 : // open and read contents of path, setting message intervals from each
7137 : // line
7138 2 : DefaultIntervalsFromFiles::DefaultIntervalsFromFiles(uint16_t max_num)
7139 : {
7140 2 : if (max_num == 0) {
7141 0 : return;
7142 : }
7143 2 : _intervals = NEW_NOTHROW from_file_default_interval[max_num];
7144 2 : _max_intervals = max_num;
7145 : }
7146 :
7147 1 : DefaultIntervalsFromFiles::~DefaultIntervalsFromFiles()
7148 : {
7149 1 : delete[] (_intervals);
7150 1 : }
7151 :
7152 4 : void DefaultIntervalsFromFiles::set(ap_message id, uint16_t interval)
7153 : {
7154 4 : if (_intervals == nullptr) {
7155 0 : return;
7156 : }
7157 :
7158 : // update any existing interval (last-one-in wins)
7159 6 : for (uint8_t i=0; i<_num_intervals; i++) {
7160 2 : if (_intervals[i].id == id) {
7161 0 : _intervals[i].interval = interval;
7162 0 : return;
7163 : }
7164 : }
7165 :
7166 : // store an interval we've not seen before:
7167 4 : if (_num_intervals == _max_intervals) {
7168 0 : return;
7169 : }
7170 :
7171 4 : _intervals[_num_intervals].id = id;
7172 4 : _intervals[_num_intervals].interval = interval;
7173 4 : _num_intervals++;
7174 : }
7175 :
7176 0 : bool DefaultIntervalsFromFiles::get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const
7177 : {
7178 0 : for (uint16_t i=0; i<_num_intervals; i++) {
7179 0 : if (_intervals[i].id == id) {
7180 0 : interval = _intervals[i].interval;
7181 0 : return true;
7182 : }
7183 : }
7184 0 : return false;
7185 : }
7186 :
7187 2 : ap_message DefaultIntervalsFromFiles::id_at(uint8_t ofs) const
7188 : {
7189 2 : if (_intervals == nullptr || ofs >= _num_intervals) {
7190 0 : return MSG_LAST;
7191 : }
7192 2 : return _intervals[ofs].id;
7193 : }
7194 :
7195 2 : uint16_t DefaultIntervalsFromFiles::interval_at(uint8_t ofs) const
7196 : {
7197 2 : if (_intervals == nullptr || ofs >= _num_intervals) {
7198 0 : return -1; // unsigned-integer wrap
7199 : }
7200 2 : return _intervals[ofs].interval;
7201 : }
7202 :
7203 4 : void GCS_MAVLINK::get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &intervals)
7204 : {
7205 4 : const int f = AP::FS().open(path, O_RDONLY);
7206 4 : if (f == -1) {
7207 2 : return;
7208 : }
7209 :
7210 : char line[20];
7211 8 : while (AP::FS().fgets(line, sizeof(line), f)) {
7212 6 : char *saveptr = nullptr;
7213 6 : const char *mavlink_id_str = strtok_r(line, " ", &saveptr);
7214 6 : if (mavlink_id_str == nullptr || strlen(mavlink_id_str) == 0) {
7215 2 : continue;
7216 : }
7217 6 : const uint32_t mavlink_id = atoi(mavlink_id_str);
7218 :
7219 6 : const ap_message msg_id = mavlink_id_to_ap_message_id(mavlink_id);
7220 6 : if (msg_id == MSG_LAST) {
7221 2 : continue;
7222 : }
7223 :
7224 4 : const char *interval_str = strtok_r(nullptr, "\r\n", &saveptr);
7225 4 : if (interval_str == nullptr || strlen(interval_str) == 0) {
7226 0 : continue;
7227 : }
7228 4 : const uint16_t interval = atoi(interval_str);
7229 :
7230 4 : intervals.set(msg_id, interval);
7231 : }
7232 :
7233 2 : AP::FS().close(f);
7234 : }
7235 :
7236 6340 : void GCS_MAVLINK::initialise_message_intervals_from_config_files()
7237 : {
7238 : static const char *path_templates[] {
7239 : "@ROMFS/message-intervals-chan%u.txt",
7240 : "message-intervals-chan%u.txt"
7241 : };
7242 :
7243 : // don't do anything at all if no files exist:
7244 6340 : bool exists = false;
7245 19019 : for (const char * path_template : path_templates) {
7246 : struct stat stats;
7247 : char *path;
7248 12680 : if (asprintf(&path, path_template, chan) == -1) {
7249 12679 : continue;
7250 : }
7251 12680 : if (AP::FS().stat(path, &stats) < 0) {
7252 12679 : free(path);
7253 12679 : continue;
7254 : }
7255 1 : free(path);
7256 1 : if (stats.st_size == 0) {
7257 0 : continue;
7258 : }
7259 1 : exists = true;
7260 1 : break;
7261 : }
7262 6340 : if (!exists) {
7263 6339 : return;
7264 : }
7265 :
7266 : // first over-allocate:
7267 1 : DefaultIntervalsFromFiles *overallocated = NEW_NOTHROW DefaultIntervalsFromFiles(128);
7268 1 : if (overallocated == nullptr) {
7269 0 : return;
7270 : }
7271 3 : for (const char * path_template : path_templates) {
7272 : char *path;
7273 2 : if (asprintf(&path, path_template, chan) == -1) {
7274 0 : continue;
7275 : }
7276 2 : get_intervals_from_filepath(path, *overallocated);
7277 2 : free(path);
7278 : }
7279 :
7280 : // then allocate just the right number and redo all of the work:
7281 1 : const uint16_t num_required = overallocated->num_intervals();
7282 1 : delete overallocated;
7283 1 : overallocated = nullptr;
7284 :
7285 1 : default_intervals_from_files = NEW_NOTHROW DefaultIntervalsFromFiles(num_required);
7286 1 : if (default_intervals_from_files == nullptr) {
7287 0 : return;
7288 : }
7289 3 : for (const char * path_template : path_templates) {
7290 : char *path;
7291 2 : if (asprintf(&path, path_template, chan) == -1) {
7292 0 : continue;
7293 : }
7294 2 : get_intervals_from_filepath(path, *default_intervals_from_files);
7295 2 : free(path);
7296 : }
7297 :
7298 : // now actually initialise the intervals:
7299 3 : for (uint8_t i=0; i<default_intervals_from_files->num_intervals(); i++) {
7300 2 : const ap_message id = default_intervals_from_files->id_at(i);
7301 2 : if (id == MSG_LAST) {
7302 : // internal error
7303 0 : break;
7304 : }
7305 2 : const uint16_t interval = default_intervals_from_files->interval_at(i);
7306 2 : set_ap_message_interval(id, interval);
7307 : }
7308 : }
7309 : #endif
7310 :
7311 6340 : void GCS_MAVLINK::initialise_message_intervals_from_streamrates()
7312 : {
7313 : #if HAL_HIGH_LATENCY2_ENABLED
7314 6340 : if (!is_high_latency_link) {
7315 : // this is O(n^2), but it's once at boot and across a 10-entry list...
7316 69718 : for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
7317 63380 : initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
7318 : }
7319 6338 : set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
7320 : } else {
7321 2 : set_mavlink_message_id_interval(MAVLINK_MSG_ID_HIGH_LATENCY2, 5000);
7322 : }
7323 : #else
7324 : // this is O(n^2), but it's once at boot and across a 10-entry list...
7325 : for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
7326 : initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
7327 : }
7328 : set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
7329 : #endif
7330 6340 : }
7331 :
7332 43 : bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const
7333 : {
7334 43 : if (id == MSG_HEARTBEAT) {
7335 : // handle heartbeat requests as a special case because heartbeat is not "streamed"
7336 0 : interval = 1000;
7337 0 : return true;
7338 : }
7339 :
7340 : #if HAL_HIGH_LATENCY2_ENABLED
7341 43 : if (id == MSG_HIGH_LATENCY2) {
7342 : // handle HL2 requests as a special case because HL2 is not "streamed"
7343 2 : interval = 5000;
7344 2 : return true;
7345 : }
7346 : #endif
7347 :
7348 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
7349 : // a user can specify default rates in files, which are read close
7350 : // to vehicle startup
7351 41 : if (default_intervals_from_files != nullptr &&
7352 0 : default_intervals_from_files->get_interval_for_ap_message_id(id, interval)) {
7353 0 : return true;
7354 : }
7355 : #endif
7356 :
7357 : // find which stream this ap_message is in
7358 430 : for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
7359 394 : const GCS_MAVLINK::stream_entries &entries = all_stream_entries[i];
7360 2425 : for (uint8_t j=0; j<entries.num_ap_message_ids; j++) {
7361 2036 : if (entries.ap_message_ids[j] == id) {
7362 5 : interval = get_interval_for_stream(all_stream_entries[i].stream_id);
7363 5 : return true;
7364 : }
7365 : }
7366 : }
7367 36 : return false;
7368 : }
7369 :
7370 : /*
7371 : correct an offboard timestamp in microseconds into a local timestamp
7372 : since boot in milliseconds. See the JitterCorrection code for details
7373 :
7374 : Return a value in milliseconds since boot (for use by the EKF)
7375 : */
7376 15583 : uint32_t GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size)
7377 : {
7378 : uint64_t local_us;
7379 : // if the HAL supports it then constrain the latest possible time
7380 : // the packet could have been sent by the uart receive time and
7381 : // the baudrate and packet size.
7382 15583 : uint64_t uart_receive_time = _port->receive_time_constraint_us(payload_size);
7383 15583 : if (uart_receive_time != 0) {
7384 15583 : local_us = uart_receive_time;
7385 : } else {
7386 0 : local_us = AP_HAL::micros64();
7387 : }
7388 15583 : uint64_t corrected_us = lag_correction.correct_offboard_timestamp_usec(offboard_usec, local_us);
7389 :
7390 15583 : return corrected_us / 1000U;
7391 : }
7392 :
7393 : /*
7394 : return true if we will accept this packet. Used to implement MAV_GCS_ENFRCE
7395 : */
7396 2784435 : bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status,
7397 : const mavlink_message_t &msg) const
7398 : {
7399 2784435 : if (msg.sysid == mavlink_system.sysid) {
7400 : // accept packets from our own components
7401 : // (e.g. mavlink-connected companion computers)
7402 21798 : return true;
7403 : }
7404 :
7405 2762637 : if (gcs().sysid_is_gcs(msg.sysid)) {
7406 37517 : return true;
7407 : }
7408 :
7409 2725120 : if (msg.msgid == MAVLINK_MSG_ID_RADIO ||
7410 2725120 : msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
7411 0 : return true;
7412 : }
7413 :
7414 2725120 : if (!gcs().option_is_enabled(GCS::Option::GCS_SYSID_ENFORCE)) {
7415 2725052 : return true;
7416 : }
7417 :
7418 68 : return false;
7419 : }
7420 :
7421 : /*
7422 : update UART pass-thru, if enabled
7423 : */
7424 41931458 : void GCS::update_passthru(void)
7425 : {
7426 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
7427 : // examples don't have AP::serialmanager
7428 0 : return;
7429 : #endif
7430 :
7431 41931458 : WITH_SEMAPHORE(_passthru.sem);
7432 41931458 : uint32_t now = AP_HAL::millis();
7433 : uint32_t baud1, baud2;
7434 41931458 : bool enabled = AP::serialmanager().get_passthru(_passthru.port1, _passthru.port2, _passthru.timeout_s,
7435 : baud1, baud2);
7436 41931458 : if (enabled && !_passthru.enabled) {
7437 0 : _passthru.start_ms = now;
7438 0 : _passthru.last_ms = 0;
7439 0 : _passthru.enabled = true;
7440 0 : _passthru.last_port1_data_ms = now;
7441 0 : _passthru.baud1 = baud1;
7442 0 : _passthru.baud2 = baud2;
7443 0 : _passthru.parity1 = _passthru.port1->get_parity();
7444 0 : _passthru.parity2 = _passthru.port2->get_parity();
7445 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru enabled");
7446 0 : if (!_passthru.timer_installed) {
7447 0 : _passthru.timer_installed = true;
7448 0 : hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&GCS::passthru_timer, void));
7449 : }
7450 41931458 : } else if (!enabled && _passthru.enabled) {
7451 0 : _passthru.enabled = false;
7452 0 : _passthru.port1->lock_port(0, 0);
7453 0 : _passthru.port2->lock_port(0, 0);
7454 : // Restore original baudrates
7455 0 : if (_passthru.baud1 != baud1) {
7456 0 : _passthru.port1->end();
7457 0 : _passthru.port1->begin(baud1);
7458 : }
7459 0 : if (_passthru.baud2 != baud2) {
7460 0 : _passthru.port2->end();
7461 0 : _passthru.port2->begin(baud2);
7462 : }
7463 : // Restore original parity
7464 0 : if (_passthru.parity1 != _passthru.port1->get_parity()) {
7465 0 : _passthru.port1->configure_parity(_passthru.parity1);
7466 : }
7467 0 : if (_passthru.parity2 != _passthru.port2->get_parity()) {
7468 0 : _passthru.port2->configure_parity(_passthru.parity2);
7469 : }
7470 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru disabled");
7471 41931458 : } else if (enabled &&
7472 0 : _passthru.timeout_s &&
7473 0 : now - _passthru.last_port1_data_ms > uint32_t(_passthru.timeout_s)*1000U) {
7474 : // timed out, disable
7475 0 : _passthru.enabled = false;
7476 0 : _passthru.port1->lock_port(0, 0);
7477 0 : _passthru.port2->lock_port(0, 0);
7478 0 : AP::serialmanager().disable_passthru();
7479 : // Restore original baudrates
7480 0 : if (_passthru.baud1 != baud1) {
7481 0 : _passthru.port1->end();
7482 0 : _passthru.port1->begin(baud1);
7483 : }
7484 0 : if (_passthru.baud2 != baud2) {
7485 0 : _passthru.port2->end();
7486 0 : _passthru.port2->begin(baud2);
7487 : }
7488 : // Restore original parity
7489 0 : if (_passthru.parity1 != _passthru.port1->get_parity()) {
7490 0 : _passthru.port1->configure_parity(_passthru.parity1);
7491 : }
7492 0 : if (_passthru.parity2 != _passthru.port2->get_parity()) {
7493 0 : _passthru.port2->configure_parity(_passthru.parity2);
7494 : }
7495 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Passthru timed out");
7496 : }
7497 41931458 : }
7498 :
7499 : /*
7500 : called at 1kHz to handle pass-thru between SERIAL_PASS1 and SERIAL_PASS2 ports
7501 : */
7502 0 : void GCS::passthru_timer(void)
7503 : {
7504 0 : WITH_SEMAPHORE(_passthru.sem);
7505 :
7506 0 : if (!_passthru.enabled) {
7507 : // it has been disabled after starting
7508 0 : return;
7509 : }
7510 0 : if (_passthru.start_ms != 0) {
7511 0 : uint32_t now = AP_HAL::millis();
7512 0 : if (now - _passthru.start_ms < 1000) {
7513 : // delay for 1s so the reply for the SERIAL_PASS2 param set can be seen by GCS
7514 0 : return;
7515 : }
7516 0 : _passthru.start_ms = 0;
7517 0 : _passthru.port1->begin(_passthru.baud1);
7518 0 : _passthru.port2->begin(_passthru.baud2);
7519 : }
7520 :
7521 : // while pass-thru is enabled lock both ports. They remain
7522 : // locked until disabled again, or reboot
7523 0 : const uint32_t lock_key = 0x3256AB9F;
7524 0 : _passthru.port1->lock_port(lock_key, lock_key);
7525 0 : _passthru.port2->lock_port(lock_key, lock_key);
7526 :
7527 : // Check for requested Baud rates and parity over USB
7528 0 : uint32_t baud = _passthru.port1->get_usb_baud();
7529 0 : uint8_t parity = _passthru.port1->get_usb_parity();
7530 0 : if (baud != 0) { // port1 is USB
7531 0 : if (_passthru.baud2 != baud) {
7532 0 : _passthru.baud2 = baud;
7533 0 : _passthru.port2->end();
7534 0 : _passthru.port2->begin_locked(baud, 0, 0, lock_key);
7535 : }
7536 :
7537 0 : if (_passthru.parity2 != parity) {
7538 0 : _passthru.parity2 = parity;
7539 0 : _passthru.port2->configure_parity(parity);
7540 : }
7541 : }
7542 :
7543 0 : baud = _passthru.port2->get_usb_baud();
7544 0 : parity = _passthru.port2->get_usb_parity();
7545 0 : if (baud != 0) { // port2 is USB
7546 0 : if (_passthru.baud1 != baud) {
7547 0 : _passthru.baud1 = baud;
7548 0 : _passthru.port1->end();
7549 0 : _passthru.port1->begin_locked(baud, 0, 0, lock_key);
7550 : }
7551 :
7552 0 : if (_passthru.parity1 != parity) {
7553 0 : _passthru.parity1 = parity;
7554 0 : _passthru.port1->configure_parity(parity);
7555 : }
7556 : }
7557 :
7558 : uint8_t buf[64];
7559 :
7560 : // read from port1, and write to port2
7561 0 : int16_t nbytes = _passthru.port1->read_locked(buf, MIN(sizeof(buf),_passthru.port2->txspace()), lock_key);
7562 0 : if (nbytes > 0) {
7563 0 : _passthru.last_port1_data_ms = AP_HAL::millis();
7564 0 : _passthru.port2->write_locked(buf, nbytes, lock_key);
7565 : }
7566 :
7567 : // read from port2, and write to port1
7568 0 : nbytes = _passthru.port2->read_locked(buf, MIN(sizeof(buf),_passthru.port1->txspace()), lock_key);
7569 0 : if (nbytes > 0) {
7570 0 : _passthru.port1->write_locked(buf, nbytes, lock_key);
7571 : }
7572 0 : }
7573 :
7574 634 : bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME coordinate_frame, Location::AltFrame &frame)
7575 : {
7576 634 : switch (coordinate_frame) {
7577 174 : case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
7578 : case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
7579 174 : frame = Location::AltFrame::ABOVE_HOME;
7580 174 : return true;
7581 123 : case MAV_FRAME_GLOBAL_TERRAIN_ALT:
7582 : case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
7583 123 : frame = Location::AltFrame::ABOVE_TERRAIN;
7584 123 : return true;
7585 337 : case MAV_FRAME_GLOBAL:
7586 : case MAV_FRAME_GLOBAL_INT:
7587 337 : frame = Location::AltFrame::ABSOLUTE;
7588 337 : return true;
7589 0 : default:
7590 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
7591 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unknown mavlink coordinate frame %u", coordinate_frame);
7592 : #endif
7593 0 : return false;
7594 : }
7595 : }
7596 :
7597 30 : uint64_t GCS_MAVLINK::capabilities() const
7598 : {
7599 30 : uint64_t ret = MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
7600 : MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION;
7601 :
7602 30 : const auto mavlink_protocol = uartstate->get_protocol();
7603 30 : if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
7604 30 : ret |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
7605 : }
7606 :
7607 : #if AP_ADVANCEDFAILSAFE_ENABLED
7608 30 : AP_AdvancedFailsafe *failsafe = AP::advancedfailsafe();
7609 30 : if (failsafe != nullptr && failsafe->enabled()) {
7610 : // Copter and Sub may also set this bit as they can always terminate
7611 1 : ret |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
7612 : }
7613 : #endif
7614 :
7615 : #if HAL_RALLY_ENABLED
7616 30 : if (AP::rally()) {
7617 27 : ret |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
7618 : }
7619 : #endif
7620 :
7621 : #if AP_FENCE_ENABLED
7622 30 : if (AP::fence()) {
7623 27 : ret |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
7624 : }
7625 : #endif
7626 :
7627 : #if AP_MAVLINK_FTP_ENABLED
7628 30 : if (!AP_BoardConfig::ftp_disabled()){ //if ftp disable board option is not set
7629 30 : ret |= MAV_PROTOCOL_CAPABILITY_FTP;
7630 : }
7631 : #endif
7632 :
7633 30 : return ret;
7634 : }
7635 :
7636 :
7637 : #if AP_RC_CHANNEL_ENABLED
7638 134 : void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
7639 : {
7640 134 : if (c == nullptr) {
7641 0 : return;
7642 : }
7643 134 : int16_t override_value = 0;
7644 134 : if (value_in != INT16_MAX) {
7645 54 : const int16_t radio_min = c->get_radio_min();
7646 54 : const int16_t radio_max = c->get_radio_max();
7647 54 : if (reversed) {
7648 2 : value_in *= -1;
7649 : }
7650 54 : override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
7651 : }
7652 134 : c->set_override(override_value, tnow);
7653 : }
7654 :
7655 56 : void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg)
7656 : {
7657 56 : if (!gcs().sysid_is_gcs(msg.sysid)) {
7658 0 : return; // only accept control from our gcs
7659 : }
7660 :
7661 : mavlink_manual_control_t packet;
7662 56 : mavlink_msg_manual_control_decode(&msg, &packet);
7663 :
7664 56 : if (packet.target != gcs().sysid_this_mav()) {
7665 0 : return; // only accept control aimed at us
7666 : }
7667 :
7668 56 : uint32_t tnow = AP_HAL::millis();
7669 :
7670 56 : handle_manual_control_axes(packet, tnow);
7671 :
7672 : // a manual control message is considered to be a 'heartbeat'
7673 : // from the ground station for failsafe purposes
7674 56 : sysid_mygcs_seen(tnow);
7675 : }
7676 : #endif // AP_RC_CHANNEL_ENABLED
7677 :
7678 : // called when valid traffic has been seen from our GCS
7679 14919 : void GCS_MAVLINK::sysid_mygcs_seen(uint32_t seen_time_ms)
7680 : {
7681 14919 : gcs().sysid_mygcs_seen(seen_time_ms);
7682 14919 : _sysid_gcs_last_seen_time_ms = seen_time_ms;
7683 14919 : }
7684 :
7685 :
7686 : #if AP_RSSI_ENABLED
7687 1117406 : uint8_t GCS_MAVLINK::receiver_rssi() const
7688 : {
7689 1117406 : AP_RSSI *aprssi = AP::rssi();
7690 1117406 : if (aprssi == nullptr) {
7691 0 : return UINT8_MAX;
7692 : }
7693 :
7694 1117406 : if (!aprssi->enabled()) {
7695 1117406 : return UINT8_MAX;
7696 : }
7697 :
7698 : // scale across the full valid range
7699 0 : return aprssi->read_receiver_rssi() * 254;
7700 : }
7701 : #endif
7702 :
7703 1144549328 : GCS &gcs()
7704 : {
7705 1144549328 : return *GCS::get_singleton();
7706 : }
7707 :
7708 : /*
7709 : send HIGH_LATENCY2 message
7710 : */
7711 : #if HAL_HIGH_LATENCY2_ENABLED
7712 1456 : void GCS_MAVLINK::send_high_latency2() const
7713 : {
7714 : #if AP_AHRS_ENABLED
7715 1456 : AP_AHRS &ahrs = AP::ahrs();
7716 1456 : Location global_position_current;
7717 1456 : UNUSED_RESULT(ahrs.get_location(global_position_current));
7718 : #if AP_BATTERY_ENABLED
7719 1456 : const int8_t battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE);
7720 : #endif
7721 :
7722 1456 : uint16_t current_waypoint = 0;
7723 : #if AP_MISSION_ENABLED
7724 1456 : current_waypoint = AP::mission().get_current_nav_index();
7725 : #endif
7726 :
7727 : uint32_t present;
7728 : uint32_t enabled;
7729 : uint32_t health;
7730 1456 : gcs().get_sensor_status_flags(present, enabled, health);
7731 : // Remap HL_FAILURE_FLAG from system status flags
7732 : static const struct PACKED status_map_t {
7733 : MAV_SYS_STATUS_SENSOR sensor;
7734 : HL_FAILURE_FLAG failure_flag;
7735 : } status_map[] {
7736 : { MAV_SYS_STATUS_SENSOR_GPS, HL_FAILURE_FLAG_GPS },
7737 : { MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE },
7738 : { MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, HL_FAILURE_FLAG_ABSOLUTE_PRESSURE },
7739 : { MAV_SYS_STATUS_SENSOR_3D_ACCEL, HL_FAILURE_FLAG_3D_ACCEL },
7740 : { MAV_SYS_STATUS_SENSOR_3D_GYRO, HL_FAILURE_FLAG_3D_GYRO },
7741 : { MAV_SYS_STATUS_SENSOR_3D_MAG, HL_FAILURE_FLAG_3D_MAG },
7742 : { MAV_SYS_STATUS_TERRAIN, HL_FAILURE_FLAG_TERRAIN },
7743 : { MAV_SYS_STATUS_SENSOR_BATTERY, HL_FAILURE_FLAG_BATTERY },
7744 : { MAV_SYS_STATUS_SENSOR_RC_RECEIVER, HL_FAILURE_FLAG_RC_RECEIVER },
7745 : { MAV_SYS_STATUS_GEOFENCE, HL_FAILURE_FLAG_GEOFENCE },
7746 : { MAV_SYS_STATUS_AHRS, HL_FAILURE_FLAG_ESTIMATOR },
7747 : };
7748 :
7749 1456 : uint16_t failure_flags = 0;
7750 17472 : for (auto &map_entry : status_map) {
7751 16016 : if ((health & map_entry.sensor) == 0) {
7752 744 : failure_flags |= map_entry.failure_flag;
7753 : }
7754 : }
7755 :
7756 1456 : mavlink_msg_high_latency2_send(chan,
7757 : AP_HAL::millis(), //[ms] Timestamp (milliseconds since boot or Unix epoch)
7758 1456 : gcs().frame_type(), // Type of the MAV (quadrotor, helicopter, etc.)
7759 : MAV_AUTOPILOT_ARDUPILOTMEGA, // Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers.
7760 1456 : gcs().custom_mode(), // A bitfield for use for autopilot-specific flags (2 byte version).
7761 : global_position_current.lat, // [degE7] Latitude
7762 : global_position_current.lng, // [degE7] Longitude
7763 1456 : global_position_current.alt * 0.01f, // [m] Altitude above mean sea level
7764 1456 : high_latency_target_altitude(), // [m] Altitude setpoint
7765 1456 : uint16_t(ahrs.get_yaw_deg()) / 2, // [deg/2] Heading
7766 1456 : high_latency_tgt_heading(), // [deg/2] Heading setpoint
7767 1456 : high_latency_tgt_dist_dam(), // [dam] Distance to target waypoint or position
7768 1456 : abs(vfr_hud_throttle()), // [%] Throttle
7769 1456 : MIN(vfr_hud_airspeed() * 5, UINT8_MAX), // [m/s*5] Airspeed
7770 1456 : high_latency_tgt_airspeed(), // [m/s*5] Airspeed setpoint
7771 1456 : MIN(ahrs.groundspeed() * 5, UINT8_MAX), // [m/s*5] Groundspeed
7772 1456 : high_latency_wind_speed(), // [m/s*5] Windspeed
7773 1456 : high_latency_wind_direction(), // [deg/2] Wind heading
7774 : 0, // [dm] Maximum error horizontal position since last message
7775 : 0, // [dm] Maximum error vertical position since last message
7776 1456 : high_latency_air_temperature(), // [degC] Air temperature from airspeed sensor
7777 : 0, // [dm/s] Maximum climb rate magnitude since last message
7778 : #if AP_BATTERY_ENABLED
7779 : battery_remaining, // [%] Battery level (-1 if field not provided).
7780 : #else
7781 : -1,
7782 : #endif
7783 : current_waypoint, // Current waypoint number
7784 : failure_flags, // Bitmap of failure flags.
7785 1456 : base_mode(), // Field for custom payload. base mode (arming status) in ArduPilot's case
7786 : 0, // Field for custom payload.
7787 : 0); // Field for custom payload.
7788 : #endif
7789 1456 : }
7790 :
7791 1456 : int8_t GCS_MAVLINK::high_latency_air_temperature() const
7792 : {
7793 : #if AP_AIRSPEED_ENABLED
7794 : // return units are degC
7795 1456 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
7796 : float air_temperature;
7797 1456 : if (airspeed != nullptr && airspeed->enabled() && airspeed->get_temperature(air_temperature)) {
7798 1456 : return air_temperature;
7799 : }
7800 : #endif
7801 :
7802 0 : return INT8_MIN;
7803 : }
7804 :
7805 : /*
7806 : handle a MAV_CMD_CONTROL_HIGH_LATENCY command
7807 :
7808 : Enable or disable any marked (via SERIALn_PROTOCOL) high latency connections
7809 : */
7810 16 : MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_int_t &packet)
7811 : {
7812 : // high latency mode is enabled if param1=1 or disabled if param1=0
7813 16 : if (is_equal(packet.param1, 0.0f)) {
7814 8 : gcs().enable_high_latency_connections(false);
7815 8 : } else if (is_equal(packet.param1, 1.0f)) {
7816 8 : gcs().enable_high_latency_connections(true);
7817 : } else {
7818 0 : return MAV_RESULT_FAILED;
7819 : }
7820 :
7821 : // send to all other mavlink components with same sysid
7822 16 : mavlink_command_long_t hl_msg{};
7823 16 : hl_msg.command = MAV_CMD_CONTROL_HIGH_LATENCY;
7824 16 : hl_msg.param1 = packet.param1;
7825 16 : GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&hl_msg, sizeof(hl_msg));
7826 :
7827 16 : return MAV_RESULT_ACCEPTED;
7828 : }
7829 : #endif // HAL_HIGH_LATENCY2_ENABLED
7830 :
7831 : #if AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
7832 0 : void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg)
7833 : {
7834 : mavlink_radio_rc_channels_t packet;
7835 0 : mavlink_msg_radio_rc_channels_decode(&msg, &packet);
7836 :
7837 0 : AP::RC().handle_radio_rc_channels(&packet);
7838 0 : }
7839 : #endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED
7840 :
7841 : #endif // HAL_GCS_ENABLED
|
