Line data Source code
1 : #include "GCS_config.h"
2 :
3 : #if HAL_GCS_ENABLED
4 :
5 : #include "GCS.h"
6 :
7 : #include <AC_Fence/AC_Fence.h>
8 : #include <AP_BoardConfig/AP_BoardConfig.h>
9 : #include <AP_Logger/AP_Logger.h>
10 : #include <AP_BattMonitor/AP_BattMonitor.h>
11 : #include <AP_Scheduler/AP_Scheduler.h>
12 : #include <AP_Baro/AP_Baro.h>
13 : #include <AP_AHRS/AP_AHRS.h>
14 : #include <AP_Compass/AP_Compass.h>
15 : #include <AP_GPS/AP_GPS.h>
16 : #include <AP_Arming/AP_Arming.h>
17 : #include <AP_VisualOdom/AP_VisualOdom.h>
18 : #include <AP_Notify/AP_Notify.h>
19 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
20 : #include <RC_Channel/RC_Channel.h>
21 : #include <AP_Vehicle/AP_Vehicle_Type.h>
22 :
23 : #include "MissionItemProtocol_Waypoints.h"
24 : #include "MissionItemProtocol_Rally.h"
25 : #include "MissionItemProtocol_Fence.h"
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : #ifndef MAV_SYSID_DEFAULT
30 : #if APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
31 : #define MAV_SYSID_DEFAULT 2
32 : #else
33 : #define MAV_SYSID_DEFAULT 1
34 : #endif // APM_BUILD_TYPE(APM_BUILD_AntennaTracker)
35 : #endif // defined(MAV_SYSID_DEFAULT)
36 :
37 : const AP_Param::GroupInfo GCS::var_info[] {
38 : // @Param: _SYSID
39 : // @DisplayName: MAVLink system ID of this vehicle
40 : // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network.
41 : // @Range: 1 255
42 : // @User: Advanced
43 : AP_GROUPINFO("_SYSID", 1, GCS, sysid, MAV_SYSID_DEFAULT),
44 :
45 : // @Param: _GCS_SYSID
46 : // @DisplayName: My ground station number
47 : // @Description: This sets what MAVLink source system IDs are accepted for GCS failsafe handling, RC overrides and manual control. When MAV_GCS_SYSID_HI is less than MAV_GCS_SYSID then only this value is considered to be a GCS. When MAV_GCS_SYSID_HI is greater than or equal to MAV_GCS_SYSID then the range of values between MAV_GCS_SYSID and MAV_GCS_SYSID_HI (inclusive) are all treated as valid GCS MAVLink system IDs
48 : // @Range: 1 255
49 : // @Increment: 1
50 : // @User: Advanced
51 : AP_GROUPINFO("_GCS_SYSID", 2, GCS, mav_gcs_sysid, 255),
52 :
53 : // @Param: _GCS_SYSID_HI
54 : // @DisplayName: ground station system ID, maximum
55 : // @Description: Upper limit of MAVLink source system IDs considered to be from the GCS. When this is less than MAV_GCS_SYSID then only MAV_GCS_SYSID is used as GCS ID. When this is greater than or equal to MAV_GCS_SYSID then the range of values from MAV_GCS_SYSID to MAV_GCS_SYSID_HI (inclusive) is treated as a GCS ID.
56 : // @Range: 0 255
57 : // @Increment: 1
58 : // @User: Advanced
59 : AP_GROUPINFO("_GCS_SYSID_HI", 5, GCS, mav_gcs_sysid_high, 0),
60 :
61 : // @Param: _OPTIONS
62 : // @DisplayName: MAVLink Options
63 : // @Description: Alters various behaviour of the MAVLink interface
64 : // @Bitmask: 0:Accept MAVLink only from system IDs given by MAV_SYSID_GCS and MAV_SYSID_GCS_HI
65 : // @User: Advanced
66 : AP_GROUPINFO("_OPTIONS", 3, GCS, mav_options, 0),
67 :
68 : // @Param: _TELEM_DELAY
69 : // @DisplayName: Telemetry startup delay
70 : // @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up
71 : // @User: Advanced
72 : // @Units: s
73 : // @Range: 0 30
74 : // @Increment: 1
75 : AP_GROUPINFO("_TELEM_DELAY", 4, GCS, mav_telem_delay, 0),
76 :
77 : #if MAVLINK_COMM_NUM_BUFFERS > 0
78 : // @Group: 1
79 : // @Path: GCS_MAVLink_Parameters.cpp
80 : AP_SUBGROUPVARPTR(_chan[0], "1", 11, GCS, _chan_var_info[0]),
81 : #endif
82 : #if MAVLINK_COMM_NUM_BUFFERS > 1
83 : // @Group: 2
84 : // @Path: GCS_MAVLink_Parameters.cpp
85 : AP_SUBGROUPVARPTR(_chan[1], "2", 12, GCS, _chan_var_info[1]),
86 : #endif
87 : #if MAVLINK_COMM_NUM_BUFFERS > 2
88 : // @Group: 3
89 : // @Path: GCS_MAVLink_Parameters.cpp
90 : AP_SUBGROUPVARPTR(_chan[2], "3", 13, GCS, _chan_var_info[2]),
91 : #endif
92 : #if MAVLINK_COMM_NUM_BUFFERS > 3
93 : // @Group: 4
94 : // @Path: GCS_MAVLink_Parameters.cpp
95 : AP_SUBGROUPVARPTR(_chan[3], "4", 14, GCS, _chan_var_info[3]),
96 : #endif
97 : #if MAVLINK_COMM_NUM_BUFFERS > 4
98 : // @Group: 5
99 : // @Path: GCS_MAVLink_Parameters.cpp
100 : AP_SUBGROUPVARPTR(_chan[4], "5", 15, GCS, _chan_var_info[4]),
101 : #endif
102 : #if MAVLINK_COMM_NUM_BUFFERS > 5
103 : // @Group: 6
104 : // @Path: GCS_MAVLink_Parameters.cpp
105 : AP_SUBGROUPVARPTR(_chan[5], "6", 16, GCS, _chan_var_info[5]),
106 : #endif
107 : #if MAVLINK_COMM_NUM_BUFFERS > 6
108 : // @Group: 7
109 : // @Path: GCS_MAVLink_Parameters.cpp
110 : AP_SUBGROUPVARPTR(_chan[6], "7", 17, GCS, _chan_var_info[6]),
111 : #endif
112 : #if MAVLINK_COMM_NUM_BUFFERS > 7
113 : // @Group: 8
114 : // @Path: GCS_MAVLink_Parameters.cpp
115 : AP_SUBGROUPVARPTR(_chan[7], "8", 18, GCS, _chan_var_info[7]),
116 : #endif
117 : #if MAVLINK_COMM_NUM_BUFFERS > 8
118 : // @Group: 9
119 : // @Path: GCS_MAVLink_Parameters.cpp
120 : AP_SUBGROUPVARPTR(_chan[8], "9", 19, GCS, _chan_var_info[8]),
121 : #endif
122 : #if MAVLINK_COMM_NUM_BUFFERS > 9
123 : // @Group: 10
124 : // @Path: GCS_MAVLink_Parameters.cpp
125 : AP_SUBGROUPVARPTR(_chan[9], "10", 20, GCS, _chan_var_info[9]),
126 : #endif
127 : #if MAVLINK_COMM_NUM_BUFFERS > 10
128 : // @Group: 11
129 : // @Path: GCS_MAVLink_Parameters.cpp
130 : AP_SUBGROUPVARPTR(_chan[10], "11", 21, GCS, _chan_var_info[10]),
131 : #endif
132 : #if MAVLINK_COMM_NUM_BUFFERS > 11
133 : // @Group: 12
134 : // @Path: GCS_MAVLink_Parameters.cpp
135 : AP_SUBGROUPVARPTR(_chan[11], "12", 22, GCS, _chan_var_info[11]),
136 : #endif
137 : #if MAVLINK_COMM_NUM_BUFFERS > 12
138 : // @Group: 13
139 : // @Path: GCS_MAVLink_Parameters.cpp
140 : AP_SUBGROUPVARPTR(_chan[12], "13", 23, GCS, _chan_var_info[12]),
141 : #endif
142 : #if MAVLINK_COMM_NUM_BUFFERS > 13
143 : // @Group: 14
144 : // @Path: GCS_MAVLink_Parameters.cpp
145 : AP_SUBGROUPVARPTR(_chan[13], "14", 24, GCS, _chan_var_info[13]),
146 : #endif
147 : #if MAVLINK_COMM_NUM_BUFFERS > 14
148 : // @Group: 15
149 : // @Path: GCS_MAVLink_Parameters.cpp
150 : AP_SUBGROUPVARPTR(_chan[14], "15", 25, GCS, _chan_var_info[14]),
151 : #endif
152 : #if MAVLINK_COMM_NUM_BUFFERS > 15
153 : // @Group: 16
154 : // @Path: GCS_MAVLink_Parameters.cpp
155 : AP_SUBGROUPVARPTR(_chan[15], "16", 26, GCS, _chan_var_info[15]),
156 : #endif
157 : #if MAVLINK_COMM_NUM_BUFFERS > 16
158 : // @Group: 17
159 : // @Path: GCS_MAVLink_Parameters.cpp
160 : AP_SUBGROUPVARPTR(_chan[16], "17", 27, GCS, _chan_var_info[16]),
161 : #endif
162 : #if MAVLINK_COMM_NUM_BUFFERS > 17
163 : // @Group: 18
164 : // @Path: GCS_MAVLink_Parameters.cpp
165 : AP_SUBGROUPVARPTR(_chan[17], "18", 28, GCS, _chan_var_info[17]),
166 : #endif
167 : #if MAVLINK_COMM_NUM_BUFFERS > 18
168 : // @Group: 19
169 : // @Path: GCS_MAVLink_Parameters.cpp
170 : AP_SUBGROUPVARPTR(_chan[18], "19", 29, GCS, _chan_var_info[18]),
171 : #endif
172 : #if MAVLINK_COMM_NUM_BUFFERS > 19
173 : // @Group: 20
174 : // @Path: GCS_MAVLink_Parameters.cpp
175 : AP_SUBGROUPVARPTR(_chan[19], "20", 30, GCS, _chan_var_info[19]),
176 : #endif
177 : #if MAVLINK_COMM_NUM_BUFFERS > 20
178 : // @Group: 21
179 : // @Path: GCS_MAVLink_Parameters.cpp
180 : AP_SUBGROUPVARPTR(_chan[20], "21", 31, GCS, _chan_var_info[20]),
181 : #endif
182 : #if MAVLINK_COMM_NUM_BUFFERS > 21
183 : // @Group: 22
184 : // @Path: GCS_MAVLink_Parameters.cpp
185 : AP_SUBGROUPVARPTR(_chan[21], "22", 32, GCS, _chan_var_info[21]),
186 : #endif
187 : #if MAVLINK_COMM_NUM_BUFFERS > 22
188 : // @Group: 23
189 : // @Path: GCS_MAVLink_Parameters.cpp
190 : AP_SUBGROUPVARPTR(_chan[22], "23", 33, GCS, _chan_var_info[22]),
191 : #endif
192 : #if MAVLINK_COMM_NUM_BUFFERS > 23
193 : // @Group: 24
194 : // @Path: GCS_MAVLink_Parameters.cpp
195 : AP_SUBGROUPVARPTR(_chan[23], "24", 34, GCS, _chan_var_info[23]),
196 : #endif
197 : #if MAVLINK_COMM_NUM_BUFFERS > 24
198 : // @Group: 25
199 : // @Path: GCS_MAVLink_Parameters.cpp
200 : AP_SUBGROUPVARPTR(_chan[24], "25", 35, GCS, _chan_var_info[24]),
201 : #endif
202 : #if MAVLINK_COMM_NUM_BUFFERS > 25
203 : // @Group: 26
204 : // @Path: GCS_MAVLink_Parameters.cpp
205 : AP_SUBGROUPVARPTR(_chan[25], "26", 36, GCS, _chan_var_info[25]),
206 : #endif
207 : #if MAVLINK_COMM_NUM_BUFFERS > 26
208 : // @Group: 27
209 : // @Path: GCS_MAVLink_Parameters.cpp
210 : AP_SUBGROUPVARPTR(_chan[26], "27", 37, GCS, _chan_var_info[26]),
211 : #endif
212 : #if MAVLINK_COMM_NUM_BUFFERS > 27
213 : // @Group: 28
214 : // @Path: GCS_MAVLink_Parameters.cpp
215 : AP_SUBGROUPVARPTR(_chan[27], "28", 38, GCS, _chan_var_info[27]),
216 : #endif
217 : #if MAVLINK_COMM_NUM_BUFFERS > 28
218 : // @Group: 29
219 : // @Path: GCS_MAVLink_Parameters.cpp
220 : AP_SUBGROUPVARPTR(_chan[28], "29", 39, GCS, _chan_var_info[28]),
221 : #endif
222 : #if MAVLINK_COMM_NUM_BUFFERS > 29
223 : // @Group: 30
224 : // @Path: GCS_MAVLink_Parameters.cpp
225 : AP_SUBGROUPVARPTR(_chan[29], "30", 40, GCS, _chan_var_info[29]),
226 : #endif
227 : #if MAVLINK_COMM_NUM_BUFFERS > 30
228 : // @Group: 31
229 : // @Path: GCS_MAVLink_Parameters.cpp
230 : AP_SUBGROUPVARPTR(_chan[30], "31", 41, GCS, _chan_var_info[30]),
231 : #endif
232 : #if MAVLINK_COMM_NUM_BUFFERS > 31
233 : // @Group: 32
234 : // @Path: GCS_MAVLink_Parameters.cpp
235 : AP_SUBGROUPVARPTR(_chan[31], "32", 42, GCS, _chan_var_info[31]),
236 : #endif
237 :
238 : AP_GROUPEND
239 : };
240 :
241 1121120 : void GCS::get_sensor_status_flags(uint32_t &present,
242 : uint32_t &enabled,
243 : uint32_t &health)
244 : {
245 : // if this assert fails then fix it and the comment in GCS.h where
246 : // _statustext_queue is declared
247 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
248 : ASSERT_STORAGE_SIZE(GCS::statustext_t, 60);
249 : #endif
250 :
251 1121120 : WITH_SEMAPHORE(control_sensors_sem);
252 :
253 1121120 : update_sensor_status_flags();
254 :
255 1121120 : present = control_sensors_present;
256 1121120 : enabled = control_sensors_enabled;
257 1121120 : health = control_sensors_health;
258 1121120 : }
259 :
260 : MissionItemProtocol *GCS::missionitemprotocols[3];
261 :
262 2094 : void GCS::init()
263 : {
264 2094 : mavlink_system.sysid = sysid_this_mav();
265 2094 : }
266 :
267 : /*
268 : * returns a mask of channels that statustexts should be sent to
269 : */
270 521942 : mavlink_channel_mask_t GCS::statustext_send_channel_mask() const
271 : {
272 521942 : mavlink_channel_mask_t ret = 0;
273 521942 : ret |= GCS_MAVLINK::active_channel_mask();
274 521942 : ret |= GCS_MAVLINK::streaming_channel_mask();
275 521942 : ret &= ~GCS_MAVLINK::private_channel_mask();
276 521942 : return ret;
277 : }
278 :
279 : /*
280 : send a text message to all GCS
281 : */
282 259116 : void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
283 : {
284 259116 : mavlink_channel_mask_t mask = statustext_send_channel_mask();
285 259116 : if (!update_send_has_been_called) {
286 : // we have not yet initialised the streaming-channel-mask,
287 : // which is done as part of the update() call. So just send
288 : // it to all channels:
289 102 : mask = (1<<_num_gcs)-1;
290 : }
291 259116 : send_textv(severity, fmt, arg_list, mask);
292 259116 : }
293 :
294 58514 : void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...)
295 : {
296 : va_list arg_list;
297 58514 : va_start(arg_list, fmt);
298 58514 : send_textv(severity, fmt, arg_list);
299 58514 : va_end(arg_list);
300 58514 : }
301 :
302 119265 : void GCS::send_to_active_channels(uint32_t msgid, const char *pkt)
303 : {
304 119265 : const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
305 119265 : if (entry == nullptr) {
306 0 : return;
307 : }
308 474116 : for (uint8_t i=0; i<num_gcs(); i++) {
309 354851 : GCS_MAVLINK &c = *chan(i);
310 354851 : if (c.is_private()) {
311 502 : continue;
312 : }
313 354349 : if (!c.is_active()) {
314 239374 : continue;
315 : }
316 : #if HAL_HIGH_LATENCY2_ENABLED
317 114975 : if (c.is_high_latency_link) {
318 118 : continue;
319 : }
320 : #endif
321 : // size checks done by this method:
322 114857 : c.send_message(pkt, entry);
323 : }
324 : }
325 :
326 31063 : void GCS::send_named_float(const char *name, float value) const
327 : {
328 :
329 31063 : mavlink_named_value_float_t packet {};
330 31063 : packet.time_boot_ms = AP_HAL::millis();
331 31063 : packet.value = value;
332 31063 : memcpy(packet.name, name, MIN(strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN));
333 :
334 31063 : gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT,
335 : (const char *)&packet);
336 :
337 : #if HAL_LOGGING_ENABLED
338 : // @LoggerMessage: NVF
339 : // @Description: Named Value Float messages; messages sent to GCS via NAMED_VALUE_FLOAT
340 : // @Field: TimeUS: Time since system startup
341 : // @Field: Name: Name of float
342 : // @Field: Value: Value of float
343 31063 : AP::logger().Write(
344 : "NVF",
345 : "TimeUS," "Name," "Value",
346 : "s" "#" "-",
347 : "F" "-" "-",
348 : "Q" "N" "f",
349 : AP_HAL::micros64(),
350 : name,
351 : value
352 : );
353 : #endif // HAL_LOGGING_ENABLED
354 31063 : }
355 :
356 92 : void GCS::send_named_int(const char *name, int32_t value) const
357 : {
358 92 : mavlink_named_value_int_t packet {};
359 92 : packet.time_boot_ms = AP_HAL::millis();
360 92 : packet.value = value;
361 92 : memcpy(packet.name, name, MIN(strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN));
362 :
363 92 : gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_INT,
364 : (const char *)&packet);
365 : #if HAL_LOGGING_ENABLED
366 : // @LoggerMessage: NVI
367 : // @Description: Named Value Int messages; messages sent to GCS via NAMED_VALUE_INT
368 : // @Field: TimeUS: Time since system startup
369 : // @Field: Name: Name of int
370 : // @Field: Value: Value of int
371 92 : AP::logger().Write(
372 : "NVI",
373 : "TimeUS," "Name," "Value",
374 : "s" "#" "-",
375 : "F" "-" "-",
376 : "Q" "N" "f",
377 : AP_HAL::micros64(),
378 : name,
379 : value
380 : );
381 : #endif // HAL_LOGGING_ENABLED
382 92 : }
383 :
384 48 : void GCS::send_named_string(const char *name, const char *value) const
385 : {
386 48 : mavlink_named_value_string_t packet {};
387 48 : packet.time_boot_ms = AP_HAL::millis();
388 48 : strncpy_noterm(packet.name, name, ARRAY_SIZE(packet.name));
389 48 : strncpy_noterm(packet.value, value, ARRAY_SIZE(packet.value));
390 :
391 48 : gcs().send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_STRING,
392 : (const char *)&packet);
393 :
394 : #if HAL_LOGGING_ENABLED
395 : // NVS is also emitted in GCS_Common.cpp
396 : // @LoggerMessage: NVS
397 : // @Description: Named Value String messages; messages sent to GCS via NAMED_VALUE_STRING
398 : // @Field: TimeUS: Time since system startup
399 : // @Field: Name: Name of string
400 : // @Field: Value: Value of string
401 48 : AP::logger().WriteStreaming(
402 : "NVS",
403 : "TimeUS," "Name," "Value",
404 : "s" "#" "-",
405 : "F" "-" "-",
406 : "Q" "N" "Z",
407 : AP_HAL::micros64(),
408 : name,
409 : value
410 : );
411 : #endif // HAL_LOGGING_ENABLED
412 48 : }
413 :
414 :
415 : #if HAL_HIGH_LATENCY2_ENABLED
416 16 : void GCS::enable_high_latency_connections(bool enabled)
417 : {
418 16 : high_latency_link_enabled = enabled;
419 16 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "High Latency %s", enabled ? "enabled" : "disabled");
420 16 : }
421 :
422 20523 : bool GCS::get_high_latency_status()
423 : {
424 20523 : return high_latency_link_enabled;
425 : }
426 : #endif // HAL_HIGH_LATENCY2_ENABLED
427 :
428 : /*
429 : install an alternative protocol handler. This allows another
430 : protocol to take over the link if MAVLink goes idle. It is used to
431 : allow for the AP_BLHeli pass-thru protocols to run on hal.serial(0)
432 : */
433 0 : bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protocol_handler_fn_t handler)
434 : {
435 0 : GCS_MAVLINK *link = chan(c);
436 0 : if (link == nullptr) {
437 0 : return false;
438 : }
439 0 : if (link->alternative.handler && handler) {
440 : // already have one installed - we may need to add support for
441 : // multiple alternative handlers
442 0 : return false;
443 : }
444 0 : link->alternative.handler = handler;
445 0 : return true;
446 : }
447 :
448 : // note that control_sensors_present and friends are protected by
449 : // control_sensors_sem. There is currently only one caller to this
450 : // method, and it does the protection for us.
451 1121120 : void GCS::update_sensor_status_flags()
452 : {
453 1121120 : control_sensors_present = 0;
454 1121120 : control_sensors_enabled = 0;
455 1121120 : control_sensors_health = 0;
456 :
457 : #if AP_INERTIALSENSOR_ENABLED
458 1121120 : const AP_InertialSensor &ins = AP::ins();
459 : #endif
460 :
461 : #if AP_AHRS_ENABLED && AP_INERTIALSENSOR_ENABLED
462 1121120 : AP_AHRS &ahrs = AP::ahrs();
463 :
464 1121120 : control_sensors_present |= MAV_SYS_STATUS_AHRS;
465 1121120 : if (ahrs.initialised()) {
466 975231 : control_sensors_enabled |= MAV_SYS_STATUS_AHRS;
467 975231 : if (ahrs.healthy()) {
468 967304 : if (!ahrs.have_inertial_nav() || ins.accel_calibrated_ok_all()) {
469 965471 : control_sensors_health |= MAV_SYS_STATUS_AHRS;
470 : }
471 : }
472 : }
473 : #endif
474 :
475 : #if AP_COMPASS_ENABLED
476 1121120 : const Compass &compass = AP::compass();
477 1121120 : if (AP::compass().available()) {
478 1120745 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
479 1120745 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
480 : }
481 1121120 : if (compass.available() && compass.healthy()) {
482 1119259 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
483 : }
484 : #endif
485 :
486 : #if AP_BARO_ENABLED
487 1121120 : const AP_Baro &barometer = AP::baro();
488 1121120 : if (barometer.num_instances() > 0) {
489 1120954 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
490 1120954 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
491 1120954 : if (barometer.all_healthy()) {
492 1120066 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
493 : }
494 : }
495 : #endif
496 :
497 : #if AP_GPS_ENABLED
498 1121120 : const AP_GPS &gps = AP::gps();
499 1121120 : if (gps.status() > AP_GPS_FixType::NO_GPS) {
500 1049521 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
501 1049521 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
502 : }
503 1121120 : if (gps.is_healthy() && gps.status() >= min_status_for_gps_healthy()) {
504 1043032 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
505 : }
506 : #endif
507 :
508 : #if AP_BATTERY_ENABLED
509 1121120 : const AP_BattMonitor &battery = AP::battery();
510 1121120 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
511 1121120 : if (battery.num_instances() > 0) {
512 871617 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
513 : }
514 1121120 : if (battery.healthy() && !battery.has_failsafed()) {
515 1115435 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_BATTERY;
516 : }
517 : #endif
518 :
519 : #if AP_INERTIALSENSOR_ENABLED
520 1121120 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
521 1121120 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
522 1121120 : if (!ins.calibrating()) {
523 1120597 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
524 1120597 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
525 1120597 : if (ins.get_accel_health_all()) {
526 1120555 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
527 : }
528 1120597 : if (ins.get_gyro_health_all() && ins.gyro_calibrated_ok_all()) {
529 1120385 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
530 : }
531 : }
532 : #endif
533 :
534 : #if HAL_LOGGING_ENABLED
535 1121120 : const AP_Logger &logger = AP::logger();
536 1121120 : bool logging_present = logger.logging_present();
537 1121120 : bool logging_enabled = logger.logging_enabled();
538 1121120 : bool logging_healthy = !logger.logging_failed();
539 : #if AP_GPS_ENABLED
540 : // some GPS units do logging, so they have to be healthy too:
541 1121120 : logging_present |= gps.logging_present();
542 1121120 : logging_enabled |= gps.logging_enabled();
543 1121120 : logging_healthy &= !gps.logging_failed();
544 : #endif
545 1121120 : if (logging_present) {
546 1121120 : control_sensors_present |= MAV_SYS_STATUS_LOGGING;
547 : }
548 1121120 : if (logging_enabled) {
549 1075235 : control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
550 : }
551 1121120 : if (logging_healthy) {
552 1120282 : control_sensors_health |= MAV_SYS_STATUS_LOGGING;
553 : }
554 : #endif // HAL_LOGGING_ENABLED
555 :
556 : // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
557 : #if !defined(HAL_BUILD_AP_PERIPH)
558 1121120 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
559 1121120 : if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
560 1120039 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
561 : }
562 1121120 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
563 : #endif
564 :
565 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_AHRS_ENABLED
566 1121120 : if (ahrs.configured_ekf_type() == AP_AHRS::EKFType::SIM) {
567 : // always show EKF type 10 as healthy. This prevents spurious error
568 : // messages in xplane and other simulators that use EKF type 10
569 78254 : control_sensors_health |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
570 : }
571 : #endif
572 :
573 : #if AP_FENCE_ENABLED
574 1121120 : const AC_Fence *fence = AP::fence();
575 1121120 : if (fence != nullptr) {
576 1119335 : if (fence->sys_status_enabled()) {
577 50013 : control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
578 : }
579 1119335 : if (fence->sys_status_present()) {
580 620493 : control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
581 : }
582 1119335 : if (!fence->sys_status_failed()) {
583 1111387 : control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
584 : }
585 : }
586 : #endif
587 :
588 : // airspeed
589 : #if AP_AIRSPEED_ENABLED
590 1121120 : const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
591 1121120 : if (airspeed && airspeed->enabled()) {
592 564382 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
593 564382 : const bool use = airspeed->use();
594 : #if AP_AHRS_ENABLED
595 564382 : const bool enabled = AP::ahrs().airspeed_sensor_enabled();
596 : #else
597 : const AP_Airspeed *_airspeed = AP::airspeed();
598 : const bool enabled = (_airspeed != nullptr && _airspeed->use());
599 : #endif
600 564382 : if (use) {
601 539172 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
602 : }
603 564382 : if (airspeed->all_healthy() && (!use || enabled)) {
604 560198 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
605 : }
606 : }
607 : #endif
608 :
609 : #if AP_OPTICALFLOW_ENABLED
610 1121120 : const AP_OpticalFlow *optflow = AP::opticalflow();
611 1121120 : if (optflow && optflow->enabled()) {
612 5102 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
613 5102 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
614 : }
615 1121120 : if (optflow && optflow->healthy()) {
616 5068 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
617 : }
618 : #endif
619 :
620 : #if HAL_VISUALODOM_ENABLED
621 1121120 : const AP_VisualOdom *visual_odom = AP::visualodom();
622 1121120 : if (visual_odom && visual_odom->enabled()) {
623 4461 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
624 4461 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
625 4461 : if (visual_odom->healthy()) {
626 4131 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
627 : }
628 : }
629 : #endif
630 :
631 : // give GCS status of prearm checks. This is enabled if any arming checks are enabled.
632 : // it is healthy if armed or checks are passing
633 : #if AP_ARMING_ENABLED
634 1121120 : control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK;
635 1121120 : if (AP::arming().get_enabled_checks()) {
636 1120578 : control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK;
637 1120578 : if (hal.util->get_soft_armed() || AP_Notify::flags.pre_arm_check) {
638 926875 : control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK;
639 : }
640 : }
641 : #endif
642 :
643 : #if AP_RC_CHANNEL_ENABLED
644 1121120 : if (rc().has_ever_seen_rc_input()) {
645 1120031 : control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
646 1120031 : control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
647 1120031 : if (!rc().in_rc_failsafe()) { // should this be has_valid_input?
648 1114383 : control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
649 : }
650 : }
651 : #endif
652 :
653 1121120 : update_vehicle_sensor_status_flags();
654 1121120 : }
655 :
656 186618194 : bool GCS::out_of_time() const
657 : {
658 : #if defined(HAL_BUILD_AP_PERIPH)
659 : // we are never out of time for AP_Periph
660 : // as we don't have concept of AP_Scheduler in AP_Periph
661 : return false;
662 : #endif
663 : // while we are in the delay callback we are never out of time:
664 186618194 : if (hal.scheduler->in_delay_callback()) {
665 1335045 : return false;
666 : }
667 :
668 : // we always want to be able to send messages out while in the error loop:
669 185283149 : if (AP_BoardConfig::in_config_error()) {
670 100254 : return false;
671 : }
672 :
673 : #if AP_SCHEDULER_ENABLED
674 185182895 : if (min_loop_time_remaining_for_message_send_us() <= AP::scheduler().time_available_usec()) {
675 185182796 : return false;
676 : }
677 : #endif
678 :
679 99 : return true;
680 : }
681 :
682 158354184 : void gcs_out_of_space_to_send(mavlink_channel_t chan)
683 : {
684 158354184 : GCS_MAVLINK *link = gcs().chan(chan);
685 158354184 : if (link == nullptr) {
686 0 : return;
687 : }
688 158354184 : link->out_of_space_to_send();
689 : }
690 :
691 : /*
692 : check there is enough space for a message
693 : */
694 135107089 : bool GCS_MAVLINK::check_payload_size(uint16_t max_payload_len)
695 : {
696 135107089 : if (txspace() < unsigned(packet_overhead()+max_payload_len)) {
697 82571410 : gcs_out_of_space_to_send(chan);
698 82571410 : return false;
699 : }
700 52535679 : return true;
701 : }
702 :
703 : #if AP_SCRIPTING_ENABLED
704 : /*
705 : lua access to command_int
706 :
707 : Note that this is called with the AP_Scheduler lock, ensuring the
708 : main thread does not race with a lua command_int
709 : */
710 16 : MAV_RESULT GCS::lua_command_int_packet(const mavlink_command_int_t &packet)
711 : {
712 : // for now we assume channel 0. In the future we may create a dedicated channel
713 16 : auto *ch = chan(0);
714 16 : if (ch == nullptr) {
715 0 : return MAV_RESULT_UNSUPPORTED;
716 : }
717 : // we need a dummy message for some calls
718 16 : mavlink_message_t msg {};
719 :
720 16 : return ch->handle_command_int_packet(packet, msg);
721 : }
722 : #endif // AP_SCRIPTING_ENABLED
723 :
724 : /*
725 : return true if a MAVLink system ID is a GCS for this vehicle
726 : */
727 4281748 : bool GCS::sysid_is_gcs(uint8_t _sysid) const
728 : {
729 4281748 : if (mav_gcs_sysid_high <= mav_gcs_sysid) {
730 4281748 : return mav_gcs_sysid == _sysid;
731 : }
732 0 : return _sysid >= mav_gcs_sysid && _sysid <= mav_gcs_sysid_high;
733 : }
734 :
735 : // Increment the available modes sequence number for each channel
736 27 : void GCS::available_modes_changed() {
737 108 : for (uint8_t i=0; i<num_gcs(); i++) {
738 81 : GCS_MAVLINK &c = *chan(i);
739 81 : c.available_modes_changed();
740 : }
741 27 : }
742 :
743 : #endif // HAL_GCS_ENABLED
|
