Line data Source code
1 : /// @file GCS.h
2 : /// @brief Interface definition for the various Ground Control System
3 : // protocols.
4 : #pragma once
5 :
6 : #include "GCS_config.h"
7 :
8 : #if HAL_GCS_ENABLED
9 :
10 : #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe_config.h>
11 : #include <AP_HAL/AP_HAL.h>
12 : #include <AP_Common/AP_Common.h>
13 : #include "GCS_MAVLink.h"
14 : #include <AP_Mission/AP_Mission.h>
15 : #include <stdint.h>
16 : #include "MAVLink_routing.h"
17 : #include <AP_RTC/JitterCorrection.h>
18 : #include <AP_Common/Bitmask.h>
19 : #include <AP_LTM_Telem/AP_LTM_Telem.h>
20 : #include <AP_Devo_Telem/AP_Devo_Telem.h>
21 : #include <AP_Filesystem/AP_Filesystem_config.h>
22 : #include <AP_Frsky_Telem/AP_Frsky_config.h>
23 : #include <AP_GPS/AP_GPS.h>
24 : #include <AP_Mount/AP_Mount_config.h>
25 : #include <AP_SerialManager/AP_SerialManager.h>
26 : #include <AP_RangeFinder/AP_RangeFinder_config.h>
27 : #include <AP_Winch/AP_Winch_config.h>
28 : #include <AP_AHRS/AP_AHRS_config.h>
29 : #include <AP_Arming/AP_Arming_config.h>
30 : #include <AP_Airspeed/AP_Airspeed_config.h>
31 : #include <AP_Follow/AP_Follow.h>
32 :
33 : #include "ap_message.h"
34 :
35 : #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
36 :
37 : #ifndef HAL_GCS_ALLOW_PARAM_SET_DEFAULT
38 : #define HAL_GCS_ALLOW_PARAM_SET_DEFAULT 1
39 : #endif // HAL_GCS_ALLOW_PARAM_SET_DEFAULT
40 :
41 : // macros used to determine if a message will fit in the space available.
42 :
43 : void gcs_out_of_space_to_send(mavlink_channel_t chan);
44 : bool check_payload_size(mavlink_channel_t chan, uint16_t max_payload_len);
45 :
46 : // important note: despite the names, these messages do NOT check to
47 : // see if the payload will fit in the buffer. They check to see if
48 : // the packed message along with any channel overhead will fit.
49 :
50 : // PAYLOAD_SIZE returns the amount of space required to send the
51 : // mavlink message with id id on channel chan. Mavlink2 has higher
52 : // overheads than mavlink1, for example.
53 :
54 : // check if a message will fit in the payload space available
55 : #define PAYLOAD_SIZE(chan, id) (unsigned(GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))
56 :
57 : // HAVE_PAYLOAD_SPACE evaluates to an expression that can be used
58 : // anywhere in the code to determine if the mavlink message with ID id
59 : // can currently fit in the output of _chan. Note the use of the ","
60 : // operator here to increment a counter.
61 : #define HAVE_PAYLOAD_SPACE(_chan, id) (comm_get_txspace(_chan) >= PAYLOAD_SIZE(_chan, id) ? true : (gcs_out_of_space_to_send(_chan), false))
62 :
63 : // CHECK_PAYLOAD_SIZE - macro which may only be used within a
64 : // GCS_MAVLink object's methods. It inserts code which will
65 : // immediately return false from the current function if there is no
66 : // room to fit the mavlink message with id id on the current object's
67 : // output
68 : #define CHECK_PAYLOAD_SIZE(id) if (!check_payload_size(MAVLINK_MSG_ID_ ## id ## _LEN)) return false
69 :
70 : // CHECK_PAYLOAD_SIZE2 - macro which inserts code which will
71 : // immediately return false from the current function if there is no
72 : // room to fit the mavlink message with id id on the mavlink output
73 : // channel "chan". It is expecting there to be a "chan" variable in
74 : // scope.
75 : #define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
76 :
77 : // CHECK_PAYLOAD_SIZE2_VOID - macro which inserts code which will
78 : // immediately return from the current (void) function if there is no
79 : // room to fit the mavlink message with id id on the mavlink output
80 : // channel "chan".
81 : #define CHECK_PAYLOAD_SIZE2_VOID(chan, id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return
82 :
83 : // code generation; avoid each subclass duplicating these two methods
84 : // and just changing the name. These methods allow retrieval of
85 : // objects specific to the vehicle's subclass, which the vehicle can
86 : // then call its own specific methods on
87 : #define GCS_MAVLINK_CHAN_METHOD_DEFINITIONS(subclass_name) \
88 : subclass_name *chan(const uint8_t ofs) override { \
89 : if (ofs >= _num_gcs) { \
90 : return nullptr; \
91 : } \
92 : return (subclass_name *)_chan[ofs]; \
93 : } \
94 : \
95 : const subclass_name *chan(const uint8_t ofs) const override { \
96 : if (ofs >= _num_gcs) { \
97 : return nullptr; \
98 : } \
99 : return (subclass_name *)_chan[ofs]; \
100 : }
101 :
102 :
103 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
104 : class DefaultIntervalsFromFiles
105 : {
106 :
107 : public:
108 :
109 : DefaultIntervalsFromFiles(uint16_t max_num);
110 : ~DefaultIntervalsFromFiles();
111 :
112 : void set(ap_message id, uint16_t interval);
113 4 : uint16_t num_intervals() const {
114 4 : return _num_intervals;
115 : }
116 : bool get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const;
117 : ap_message id_at(uint8_t ofs) const;
118 : uint16_t interval_at(uint8_t ofs) const;
119 :
120 : private:
121 :
122 : struct from_file_default_interval {
123 : ap_message id;
124 : uint16_t interval;
125 : };
126 :
127 : from_file_default_interval *_intervals;
128 :
129 : uint16_t _num_intervals;
130 : uint16_t _max_intervals;
131 : };
132 : #endif
133 :
134 : class GCS_MAVLINK_InProgress
135 : {
136 : public:
137 : enum class Type {
138 : NONE,
139 : AIRSPEED_CAL,
140 : SD_FORMAT,
141 : };
142 :
143 : // these can fail if there's no space on the channel to send the ack:
144 : bool conclude(MAV_RESULT result);
145 : bool send_in_progress();
146 : // abort task without sending any further ACKs:
147 0 : void abort() { task = Type::NONE; }
148 :
149 : Type task;
150 : MAV_CMD mav_cmd;
151 :
152 : static class GCS_MAVLINK_InProgress *get_task(MAV_CMD cmd, Type t, uint8_t sysid, uint8_t compid, mavlink_channel_t chan);
153 :
154 : static void check_tasks();
155 :
156 : private:
157 :
158 : uint8_t requesting_sysid;
159 : uint8_t requesting_compid;
160 : mavlink_channel_t chan;
161 :
162 : bool send_ack(MAV_RESULT result);
163 :
164 : static GCS_MAVLINK_InProgress in_progress_tasks[1];
165 :
166 : // allocate a task-tracking ID
167 : static uint32_t last_check_ms;
168 : };
169 :
170 : ///
171 : /// @class GCS_MAVLINK
172 : /// @brief MAVLink transport control class
173 : ///
174 : class GCS_MAVLINK
175 : {
176 : public:
177 : friend class GCS;
178 : #if AP_MAVLINK_FTP_ENABLED
179 : friend class GCS_FTP;
180 : #endif
181 : friend class MAVLink_routing;
182 :
183 : GCS_MAVLINK(AP_HAL::UARTDriver &uart);
184 0 : virtual ~GCS_MAVLINK() {}
185 :
186 : static const struct AP_Param::GroupInfo var_info[];
187 :
188 : // accessors used to retrieve objects used for parsing incoming messages:
189 118867078 : mavlink_message_t *channel_buffer() { return &_channel_buffer; }
190 415865978 : mavlink_status_t *channel_status() { return &_channel_status; }
191 :
192 : void update_receive(uint32_t max_time_us=1000);
193 : void update_send();
194 : bool init(uint8_t instance);
195 : void send_message(enum ap_message id);
196 : void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
197 : void queued_param_send();
198 : void queued_mission_request_send();
199 :
200 : bool sending_mavlink1() const;
201 :
202 : // returns true if we are requesting any items from the GCS:
203 : bool requesting_mission_items() const;
204 :
205 : /// Check for available transmit space
206 272445186 : uint16_t txspace() const {
207 272445186 : if (_locked) {
208 0 : return 0;
209 : }
210 : // there were concerns over return a too-large value for
211 : // txspace (in case we tried to do too much with the space in
212 : // a single loop):
213 272445186 : return MIN(_port->txspace(), 8192U);
214 : }
215 :
216 : bool check_payload_size(uint16_t max_payload_len);
217 :
218 : // this is called when we discover we'd like to send something but can't:
219 158354184 : void out_of_space_to_send() { out_of_space_to_send_count++; }
220 :
221 8 : void send_mission_ack(const mavlink_message_t &msg,
222 : MAV_MISSION_TYPE mission_type,
223 : MAV_MISSION_RESULT result) const {
224 8 : CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
225 8 : mavlink_msg_mission_ack_send(chan,
226 8 : msg.sysid,
227 8 : msg.compid,
228 : result,
229 : mission_type);
230 : }
231 :
232 : // packetReceived is called on any successful decode of a mavlink
233 : // message where the framing is correct (so CRC matches, for
234 : // example).
235 : virtual void packetReceived(const mavlink_status_t &status,
236 : const mavlink_message_t &msg);
237 :
238 : // raw_packetReceived is called on any successful decode of a mavlink
239 : // message.
240 : void raw_packetReceived(uint8_t framing_status,
241 : const mavlink_status_t &status,
242 : const mavlink_message_t &msg);
243 :
244 : // send a mavlink_message_t out this GCS_MAVLINK connection.
245 17220 : void send_message(uint32_t msgid, const char *pkt) {
246 17220 : const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
247 17220 : if (entry == nullptr) {
248 0 : return;
249 : }
250 17220 : send_message(pkt, entry);
251 : }
252 132077 : void send_message(const char *pkt, const mavlink_msg_entry_t *entry) {
253 132077 : if (!check_payload_size(entry->max_msg_len)) {
254 1291 : return;
255 : }
256 130786 : _mav_finalize_message_chan_send(chan,
257 130786 : entry->msgid,
258 : pkt,
259 130786 : entry->min_msg_len,
260 130786 : entry->max_msg_len,
261 130786 : entry->crc_extra);
262 : }
263 :
264 : // accessor for uart
265 0 : AP_HAL::UARTDriver *get_uart() { return _port; }
266 :
267 : // cap the MAVLink message rate. It can't be greater than 0.8 * SCHED_LOOP_RATE
268 : uint16_t cap_message_interval(uint16_t interval_ms) const;
269 :
270 : // NOTE: param_name here must point to a 16+1 byte buffer - so do
271 : // NOT try to pass in a static-char-* unless it does have that
272 : // length!
273 : void send_parameter_value(const char *param_name,
274 : ap_var_type param_type,
275 : float param_value);
276 :
277 : // NOTE! The streams enum below and the
278 : // set of AP_Int16 stream rates _must_ be
279 : // kept in the same order
280 : // ... and "default_rates[..]" in GCS_MAVLINK_Parameters.cpp
281 : // should also be kept in mind.
282 : enum streams : uint8_t {
283 : STREAM_RAW_SENSORS,
284 : STREAM_EXTENDED_STATUS,
285 : STREAM_RC_CHANNELS,
286 : STREAM_RAW_CONTROLLER,
287 : STREAM_POSITION,
288 : STREAM_EXTRA1,
289 : STREAM_EXTRA2,
290 : STREAM_EXTRA3,
291 : STREAM_PARAMS,
292 : STREAM_ADSB,
293 : NUM_STREAMS
294 : };
295 :
296 0 : bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
297 : // return true if this channel has hardware flow control
298 : bool have_flow_control();
299 :
300 127420835 : bool is_active() const {
301 127420835 : return GCS_MAVLINK::active_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
302 : }
303 82247181 : bool is_streaming() const {
304 82247181 : return sending_bucket_id != no_bucket_to_send;
305 : }
306 :
307 31905644 : mavlink_channel_t get_chan() const { return chan; }
308 368939 : uint32_t get_last_heartbeat_time() const { return last_heartbeat_time; };
309 :
310 : uint32_t last_heartbeat_time; // milliseconds
311 :
312 : // called when valid traffic has been seen from our GCS
313 : void sysid_mygcs_seen(uint32_t seen_time_ms);
314 :
315 0 : static uint32_t last_radio_status_remrssi_ms() {
316 0 : return last_radio_status.remrssi_ms;
317 : }
318 : static float telemetry_radio_rssi(); // 0==no signal, 1==full signal
319 : static bool last_txbuf_is_greater(uint8_t txbuf_limit);
320 :
321 : // mission item index to be sent on queued msg, delayed or not
322 : uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE;
323 :
324 : // generate a MISSION_STATE enumeration value for where the
325 : // mission is up to:
326 : virtual MISSION_STATE mission_state(const class AP_Mission &mission) const;
327 : // send a mission_current message for the supplied waypoint
328 : void send_mission_current(const class AP_Mission &mission, uint16_t seq);
329 :
330 : // common send functions
331 : void send_heartbeat(void) const;
332 : void send_meminfo(void);
333 : void send_fence_status() const;
334 : void send_power_status(void);
335 : #if HAL_WITH_MCU_MONITORING
336 : void send_mcu_status(void);
337 : #endif
338 : void send_battery_status(const uint8_t instance) const;
339 : bool send_battery_status();
340 : void send_distance_sensor();
341 : // send_rangefinder sends only if a downward-facing instance is
342 : // found. Rover overrides this!
343 : #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED
344 : virtual void send_rangefinder() const;
345 : #endif
346 : void send_proximity();
347 : virtual void send_nav_controller_output() const = 0;
348 : virtual void send_pid_tuning() = 0;
349 : void send_ahrs2();
350 : void send_system_time() const;
351 : void send_rc_channels() const;
352 : void send_rc_channels_raw() const;
353 : void send_raw_imu();
354 : void send_highres_imu();
355 :
356 : void 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));
357 : void send_scaled_pressure();
358 : void send_scaled_pressure2();
359 : virtual void send_scaled_pressure3(); // allow sub to override this
360 : #if AP_AIRSPEED_ENABLED
361 : // Send per instance airspeed message
362 : // last index is used to rotate through sensors
363 : void send_airspeed();
364 : uint8_t last_airspeed_idx;
365 : #endif
366 : void send_simstate() const;
367 : void send_sim_state() const;
368 : void send_ahrs();
369 : void send_opticalflow();
370 : virtual void send_attitude() const;
371 : virtual void send_attitude_quaternion() const;
372 : void send_autopilot_version() const;
373 : void send_extended_sys_state() const;
374 : void send_local_position() const;
375 : void send_vfr_hud();
376 : void send_vibration() const;
377 : void send_gimbal_device_attitude_status() const;
378 : void send_gimbal_manager_information() const;
379 : void send_gimbal_manager_status() const;
380 : void send_named_float(const char *name, float value) const;
381 : void send_named_int(const char *name, int32_t value) const;
382 : void send_home_position() const;
383 : void send_gps_global_origin() const;
384 0 : virtual void send_attitude_target() {};
385 : void send_position_target_global_int();
386 : // returns a Location to which the vehicle is currently heading,
387 : // or would head to in an autonomous mode
388 1760 : virtual bool get_target_location(Location &loc) const { return false; }
389 0 : virtual void send_position_target_local_ned() { };
390 : void send_servo_output_raw();
391 : void send_accelcal_vehicle_position(uint32_t position);
392 : void 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));
393 : void send_sys_status();
394 : void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
395 : void send_rpm() const;
396 : void send_generator_status() const;
397 : #if AP_WINCH_ENABLED
398 735015 : virtual void send_winch_status() const {};
399 : #endif
400 : int8_t battery_remaining_pct(const uint8_t instance) const;
401 :
402 : #if HAL_HIGH_LATENCY2_ENABLED
403 : void send_high_latency2() const;
404 : #endif // HAL_HIGH_LATENCY2_ENABLED
405 : void send_uavionix_adsb_out_status() const;
406 : void send_autopilot_state_for_gimbal_device() const;
407 :
408 : // Send the mode with the given index (not mode number!) return the total number of modes
409 : // Index starts at 1
410 : virtual uint8_t send_available_mode(uint8_t index) const = 0;
411 :
412 : #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
413 : struct {
414 : MAV_LANDED_STATE last_landed_state;
415 : uint64_t takeoff_time_us;
416 : } flight_info;
417 :
418 : void send_flight_information();
419 : #endif
420 :
421 : // lock a channel, preventing use by MAVLink
422 0 : void lock(bool _lock) {
423 0 : _locked = _lock;
424 0 : }
425 : // returns true if this channel isn't available for MAVLink
426 127068125 : bool locked() const {
427 127068125 : return _locked;
428 : }
429 :
430 : // return a bitmap of active channels. Used by libraries to loop
431 : // over active channels to send to all active channels
432 129006752 : static mavlink_channel_mask_t active_channel_mask(void) { return mavlink_active; }
433 :
434 : // return a bitmap of streaming channels
435 521942 : static mavlink_channel_mask_t streaming_channel_mask(void) { return chan_is_streaming; }
436 :
437 : // return a bitmap of private channels
438 1585918 : static mavlink_channel_mask_t private_channel_mask(void) { return mavlink_private; }
439 :
440 : // set a channel as private. Private channels get sent heartbeats, but
441 : // don't get broadcast packets or forwarded packets
442 : static void set_channel_private(mavlink_channel_t chan);
443 :
444 : // return true if channel is private
445 82600377 : static bool is_private(mavlink_channel_t _chan) {
446 82600377 : return (mavlink_private & (1U<<(unsigned)_chan)) != 0;
447 : }
448 :
449 : // return true if channel is private
450 82600377 : bool is_private(void) const { return is_private(chan); }
451 :
452 : #if HAL_HIGH_LATENCY2_ENABLED
453 : // true if this is a high latency link
454 : bool is_high_latency_link;
455 : #endif
456 :
457 : /*
458 : send a MAVLink message to all components with this vehicle's system id
459 : This is a no-op if no routes to components have been learned
460 : */
461 21 : static void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { routing.send_to_components(msgid, pkt, pkt_len); }
462 :
463 : /*
464 : allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic
465 : */
466 1023 : static void disable_channel_routing(mavlink_channel_t chan) { routing.no_route_mask |= (1U<<(chan-MAVLINK_COMM_0)); }
467 :
468 : /*
469 : search for a component in the routing table with given mav_type and retrieve it's sysid, compid and channel
470 : returns if a matching component is found
471 : */
472 10 : static bool find_by_mavtype(uint8_t mav_type, uint8_t &sysid, uint8_t &compid, mavlink_channel_t &channel) { return routing.find_by_mavtype(mav_type, sysid, compid, channel); }
473 :
474 : /*
475 : search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
476 : returns true if a match is found
477 : */
478 0 : static bool find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid, mavlink_channel_t &channel) { return routing.find_by_mavtype_and_compid(mav_type, compid, sysid, channel); }
479 : // same as above, but returns a pointer to the GCS_MAVLINK object
480 : // corresponding to the channel
481 : static GCS_MAVLINK *find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid);
482 :
483 : #if AP_MAVLINK_SIGNING_ENABLED
484 : // update signing timestamp on GPS lock
485 : static void update_signing_timestamp(uint64_t timestamp_usec);
486 : #endif // AP_MAVLINK_SIGNING_ENABLED
487 :
488 : // return current packet overhead for a channel
489 : static uint8_t packet_overhead_chan(mavlink_channel_t chan);
490 :
491 : // alternative protocol function handler
492 : FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
493 :
494 : struct stream_entries {
495 : const streams stream_id;
496 : const ap_message *ap_message_ids;
497 : const uint8_t num_ap_message_ids;
498 : };
499 : // vehicle subclass cpp files should define this:
500 : static const struct stream_entries all_stream_entries[];
501 :
502 : virtual uint64_t capabilities() const;
503 9325 : uint16_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
504 :
505 : MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
506 :
507 : protected:
508 :
509 : bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame,
510 : Location::AltFrame &frame);
511 :
512 : // overridable method to check for packet acceptance. Allows for
513 : // enforcement of GCS sysid
514 : bool accept_packet(const mavlink_status_t &status, const mavlink_message_t &msg) const;
515 : MAV_RESULT set_ekf_origin(const Location& loc);
516 :
517 : virtual uint8_t base_mode() const = 0;
518 : MAV_STATE system_status() const;
519 : virtual MAV_STATE vehicle_system_status() const = 0;
520 :
521 0 : virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
522 5 : virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }
523 :
524 : // return a MAVLink parameter type given a AP_Param type
525 : static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t);
526 :
527 : AP_Param * _queued_parameter; ///< next parameter to
528 : // be sent in queue
529 : mavlink_channel_t chan;
530 135114793 : uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
531 :
532 : // saveable rate of each stream
533 : AP_Int16 streamRates[NUM_STREAMS];
534 : AP_Int32 devid; // ID for device using this mavlink channel
535 :
536 : void handle_heartbeat(const mavlink_message_t &msg);
537 :
538 12746 : virtual bool persist_streamrates() const { return false; }
539 : void handle_request_data_stream(const mavlink_message_t &msg);
540 :
541 : AP_Int16 options;
542 : enum class Option : uint16_t {
543 : MAVLINK2_SIGNING_DISABLED = (1U << 0),
544 : NO_FORWARD = (1U << 1), // don't forward MAVLink data to or from this device
545 : NOSTREAMOVERRIDE = (1U << 2), // ignore REQUEST_DATA_STREAM messages (eg. from GCSs)
546 : FORWARD_BAD_CRC = (1U << 3), // forward mavlink packets that don't pass CRC
547 : };
548 14795 : bool option_enabled(Option option) const {
549 14795 : return options & static_cast<uint16_t>(option);
550 : }
551 0 : void enable_option(Option option) {
552 0 : options.set_and_save(static_cast<uint16_t>(options) | static_cast<uint16_t>(option));
553 0 : }
554 : void disable_option(Option option) {
555 : options.set_and_save(static_cast<uint16_t>(options) & (~ static_cast<uint16_t>(option)));
556 : }
557 : AP_Int8 options_were_converted;
558 :
559 : virtual void handle_command_ack(const mavlink_message_t &msg);
560 : void handle_set_mode(const mavlink_message_t &msg);
561 : void handle_command_int(const mavlink_message_t &msg);
562 :
563 : MAV_RESULT handle_command_do_follow(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
564 : virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
565 : MAV_RESULT handle_command_int_external_position_estimate(const mavlink_command_int_t &packet);
566 : MAV_RESULT handle_command_int_external_wind_estimate(const mavlink_command_int_t &packet);
567 :
568 : #if AP_HOME_ENABLED
569 : MAV_RESULT handle_command_do_set_home(const mavlink_command_int_t &packet);
570 : bool set_home_to_current_location(bool lock);
571 : bool set_home(const Location& loc, bool lock);
572 : #endif
573 :
574 : #if AP_ARMING_ENABLED
575 : virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_int_t &packet);
576 : #endif
577 : MAV_RESULT handle_command_do_aux_function(const mavlink_command_int_t &packet);
578 : MAV_RESULT handle_command_storage_format(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
579 : void handle_mission_request_list(const mavlink_message_t &msg);
580 : #if AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
581 : void handle_mission_request(const mavlink_message_t &msg);
582 : #endif
583 : void handle_mission_request_int(const mavlink_message_t &msg);
584 : void handle_mission_clear_all(const mavlink_message_t &msg);
585 :
586 : #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
587 : // Note that there exists a relatively new mavlink DO command,
588 : // MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
589 : // that the command has been received, rather than the GCS having to
590 : // rely on getting back an identical sequence number as some currently
591 : // do.
592 : virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg);
593 : #endif
594 :
595 : void handle_mission_count(const mavlink_message_t &msg);
596 : void handle_mission_write_partial_list(const mavlink_message_t &msg);
597 : void handle_mission_item(const mavlink_message_t &msg);
598 :
599 : void handle_distance_sensor(const mavlink_message_t &msg);
600 : void handle_obstacle_distance(const mavlink_message_t &msg);
601 : void handle_obstacle_distance_3d(const mavlink_message_t &msg);
602 :
603 : void handle_adsb_message(const mavlink_message_t &msg);
604 :
605 : void handle_osd_param_config(const mavlink_message_t &msg) const;
606 :
607 : void handle_common_param_message(const mavlink_message_t &msg);
608 : void handle_param_set(const mavlink_message_t &msg);
609 : void handle_param_request_list(const mavlink_message_t &msg);
610 : void handle_param_request_read(const mavlink_message_t &msg);
611 562 : virtual bool params_ready() const { return true; }
612 : void handle_rc_channels_override(const mavlink_message_t &msg);
613 : void handle_system_time_message(const mavlink_message_t &msg);
614 : void handle_common_rally_message(const mavlink_message_t &msg);
615 : void handle_rally_fetch_point(const mavlink_message_t &msg);
616 : void handle_rally_point(const mavlink_message_t &msg) const;
617 : #if HAL_MOUNT_ENABLED
618 : void handle_mount_message(const mavlink_message_t &msg);
619 : #endif
620 : void handle_fence_message(const mavlink_message_t &msg);
621 : void handle_param_value(const mavlink_message_t &msg);
622 : #if HAL_LOGGING_ENABLED
623 0 : virtual uint32_t log_radio_bit() const { return 0; }
624 : #endif
625 : void handle_radio_status(const mavlink_message_t &msg);
626 : void handle_serial_control(const mavlink_message_t &msg);
627 : void handle_vision_position_delta(const mavlink_message_t &msg);
628 :
629 : virtual void handle_message(const mavlink_message_t &msg);
630 : #if AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED
631 : void handle_set_gps_global_origin(const mavlink_message_t &msg);
632 : #endif // AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED
633 : void handle_setup_signing(const mavlink_message_t &msg) const;
634 : virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
635 : #if AP_MAVLINK_FAILURE_CREATION_ENABLED
636 : struct {
637 : HAL_Semaphore sem;
638 : bool taken;
639 : } _deadlock_sem;
640 : void deadlock_sem(void);
641 : #endif
642 :
643 : MAV_RESULT handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
644 :
645 : // reset a message interval via mavlink:
646 : MAV_RESULT handle_command_set_message_interval(const mavlink_command_int_t &packet);
647 : MAV_RESULT handle_command_get_message_interval(const mavlink_command_int_t &packet);
648 : bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const;
649 : MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet);
650 :
651 : MAV_RESULT handle_START_RX_PAIR(const mavlink_command_int_t &packet);
652 :
653 : virtual MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet);
654 :
655 : #if AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED
656 : void handle_send_autopilot_version(const mavlink_message_t &msg);
657 : #endif
658 : #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
659 : MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_int_t &packet);
660 : #endif
661 :
662 : virtual void send_banner();
663 :
664 : // send a (textual) message to the GCS that a received message has
665 : // been deprecated
666 : void send_received_message_deprecation_warning(const char *message);
667 :
668 : void handle_device_op_read(const mavlink_message_t &msg);
669 : void handle_device_op_write(const mavlink_message_t &msg);
670 :
671 : void send_timesync();
672 : // returns the time a timesync message was most likely received:
673 : uint64_t timesync_receive_timestamp_ns() const;
674 : // returns a timestamp suitable for packing into the ts1 field of TIMESYNC:
675 : uint64_t timesync_timestamp_ns() const;
676 : void handle_timesync(const mavlink_message_t &msg);
677 : struct {
678 : int64_t sent_ts1;
679 : uint32_t last_sent_ms;
680 : const uint16_t interval_ms = 10000;
681 : } _timesync_request;
682 :
683 : void handle_statustext(const mavlink_message_t &msg);
684 : struct {
685 : uint8_t last_src_system;
686 : uint8_t last_src_component;
687 : uint8_t last_id; // ID from the mavlink packet
688 : uint8_t msg_id; // ID used in our logs
689 : } statustext_chunking;
690 : void handle_named_value(const mavlink_message_t &msg) const;
691 :
692 : bool telemetry_delayed() const;
693 :
694 : MAV_RESULT handle_command_run_prearm_checks(const mavlink_command_int_t &packet);
695 : MAV_RESULT handle_command_flash_bootloader(const mavlink_command_int_t &packet);
696 :
697 : // generally this should not be overridden; Plane overrides it to ensure
698 : // failsafe isn't triggered during calibration
699 : virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
700 :
701 : virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
702 : virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg);
703 :
704 : #if AP_MISSION_ENABLED
705 : virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_int_t &packet);
706 : MAV_RESULT handle_command_do_jump_tag(const mavlink_command_int_t &packet);
707 : #endif
708 : MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet);
709 : void handle_command_long(const mavlink_message_t &msg);
710 : MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet);
711 :
712 : #if HAL_MOUNT_ENABLED
713 : virtual MAV_RESULT handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
714 : #endif
715 :
716 : MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet);
717 : MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet);
718 :
719 : MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet);
720 : MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
721 : virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
722 : MAV_RESULT handle_command_do_gripper(const mavlink_command_int_t &packet);
723 : MAV_RESULT handle_command_do_sprayer(const mavlink_command_int_t &packet);
724 : MAV_RESULT handle_command_do_set_mode(const mavlink_command_int_t &packet);
725 : #if AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
726 : MAV_RESULT handle_command_get_home_position(const mavlink_command_int_t &packet);
727 : #endif // AP_MAVLINK_MAV_CMD_GET_HOME_POSITION_ENABLED
728 : MAV_RESULT handle_command_do_fence_enable(const mavlink_command_int_t &packet);
729 : MAV_RESULT handle_command_debug_trap(const mavlink_command_int_t &packet);
730 : MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_int_t &packet);
731 : MAV_RESULT handle_command_airframe_configuration(const mavlink_command_int_t &packet);
732 :
733 : /*
734 : handle MAV_CMD_CAN_FORWARD and CAN_FRAME messages for CAN over MAVLink
735 : */
736 : void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &);
737 : #if AP_MAVLINKCAN_ENABLED
738 : MAV_RESULT handle_can_forward(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
739 : void handle_can_frame(const mavlink_message_t &msg) const;
740 : #endif // AP_MAVLINKCAN_ENABLED
741 :
742 : void handle_optical_flow(const mavlink_message_t &msg);
743 :
744 : void handle_manual_control(const mavlink_message_t &msg);
745 : void handle_radio_rc_channels(const mavlink_message_t &msg);
746 :
747 : // default empty handling of LANDING_TARGET
748 0 : virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { }
749 : // vehicle-overridable message send function
750 : virtual bool try_send_message(enum ap_message id);
751 : virtual void send_global_position_int();
752 :
753 : #if AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
754 : void send_utm_global_position() const;
755 : #endif // AP_MAVLINK_UTM_GLOBAL_POSITION_SENDING_ENABLED
756 :
757 : // message sending functions:
758 : bool try_send_mission_message(enum ap_message id);
759 : void handle_data_packet(const mavlink_message_t &msg);
760 :
761 : // these two methods are called after current_loc is updated:
762 : virtual int32_t global_position_int_alt() const;
763 : virtual int32_t global_position_int_relative_alt() const;
764 :
765 : virtual float vfr_hud_climbrate() const;
766 : virtual float vfr_hud_airspeed() const;
767 0 : virtual int16_t vfr_hud_throttle() const { return 0; }
768 : #if AP_AHRS_ENABLED
769 : virtual float vfr_hud_alt() const;
770 : MAV_RESULT handle_command_do_set_global_origin(const mavlink_command_int_t &packet);
771 : #endif
772 :
773 : #if HAL_HIGH_LATENCY2_ENABLED
774 727 : virtual int16_t high_latency_target_altitude() const { return 0; }
775 0 : virtual uint8_t high_latency_tgt_heading() const { return 0; }
776 0 : virtual uint16_t high_latency_tgt_dist_dam() const { return 0; }
777 0 : virtual uint8_t high_latency_tgt_airspeed() const { return 0; }
778 0 : virtual uint8_t high_latency_wind_speed() const { return 0; }
779 0 : virtual uint8_t high_latency_wind_direction() const { return 0; }
780 : int8_t high_latency_air_temperature() const;
781 :
782 : MAV_RESULT handle_control_high_latency(const mavlink_command_int_t &packet);
783 :
784 : #endif // HAL_HIGH_LATENCY2_ENABLED
785 :
786 : // note that the mavlink specification only uses 21196 as the
787 : // magic value to force and arm or disarm. We continue to support
788 : // 2989 for force-arming for backwards-compatability reasons.
789 : static constexpr const float magic_force_arm_value = 2989.0f;
790 : static constexpr const float magic_force_arm_disarm_value = 21196.0f;
791 :
792 : void manual_override(class RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false);
793 :
794 : uint8_t receiver_rssi() const;
795 :
796 : /*
797 : correct an offboard timestamp in microseconds to a local time
798 : since boot in milliseconds
799 : */
800 : uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size);
801 :
802 : #if AP_MAVLINK_COMMAND_LONG_ENABLED
803 : // converts a COMMAND_LONG packet to a COMMAND_INT packet, where
804 : // the command-long packet is assumed to be in the supplied frame.
805 : // If location is not present in the command then just omit frame.
806 : // this method ensures the passed-in structure is entirely
807 : // initialised.
808 : virtual void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT);
809 : virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const;
810 : MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
811 : #endif
812 :
813 : // methods to extract a Location object from a command_int
814 : bool location_from_command_t(const mavlink_command_int_t &in, Location &out);
815 :
816 : private:
817 :
818 : // define the two objects used for parsing incoming messages:
819 : mavlink_message_t _channel_buffer;
820 : mavlink_status_t _channel_status;
821 :
822 : const AP_SerialManager::UARTState *uartstate;
823 :
824 : // last time we got a non-zero RSSI from RADIO_STATUS
825 : static struct LastRadioStatus {
826 : uint32_t remrssi_ms;
827 : uint8_t rssi;
828 : uint32_t received_ms; // time RADIO_STATUS received
829 : uint8_t txbuf = 100;
830 : } last_radio_status;
831 :
832 : enum class Flags {
833 : USING_SIGNING = (1<<0),
834 : ACTIVE = (1<<1),
835 : STREAMING = (1<<2),
836 : PRIVATE = (1<<3),
837 : LOCKED = (1<<4),
838 : };
839 : void log_mavlink_stats();
840 :
841 : MAV_RESULT _set_mode_common(const uint8_t base_mode, const uint32_t custom_mode);
842 :
843 : // send a (textual) message to the GCS that a received message has
844 : // been deprecated
845 : uint32_t last_deprecation_warning_send_time_ms;
846 : const char *last_deprecation_message;
847 :
848 : // time we last saw traffic from our GCS. Note that there is an
849 : // identically named field in GCS:: which is the most recent of
850 : // each of the GCS_MAVLINK backends
851 : uint32_t _sysid_gcs_last_seen_time_ms;
852 :
853 : void service_statustext(void);
854 :
855 : MAV_RESULT handle_servorelay_message(const mavlink_command_int_t &packet);
856 : bool send_relay_status() const;
857 :
858 : static bool command_long_stores_location(const MAV_CMD command);
859 :
860 : bool calibrate_gyros();
861 :
862 : /// The stream we are communicating over
863 : AP_HAL::UARTDriver *_port;
864 :
865 : /// Perform queued sending operations
866 : ///
867 : enum ap_var_type _queued_parameter_type; ///< type of the next
868 : // parameter
869 : AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
870 : // next() call
871 : uint16_t _queued_parameter_index; ///< next queued
872 : // parameter's index
873 : uint16_t _queued_parameter_count; ///< saved count of
874 : // parameters for
875 : // queued send
876 : uint32_t _queued_parameter_send_time_ms;
877 :
878 : // number of extra ms to add to slow things down for the radio
879 : uint16_t stream_slowdown_ms;
880 :
881 : // outbound ("deferred message") queue.
882 :
883 : // "special" messages such as heartbeat, next_param etc are stored
884 : // separately to stream-rated messages like AHRS2 etc. If these
885 : // were to be stored in buckets then they would be slowed down
886 : // based on stream_slowdown, which we have not traditionally done.
887 : struct deferred_message_t {
888 : const ap_message id;
889 : uint16_t interval_ms;
890 : uint16_t last_sent_ms; // from AP_HAL::millis16()
891 : } deferred_message[3] = {
892 : { MSG_HEARTBEAT, },
893 : { MSG_NEXT_PARAM, },
894 : #if HAL_HIGH_LATENCY2_ENABLED
895 : { MSG_HIGH_LATENCY2, },
896 : #endif
897 : };
898 : // returns index of id in deferred_message[] or -1 if not present
899 : int8_t get_deferred_message_index(const ap_message id) const;
900 : // returns index of a message in deferred_message[] which should
901 : // be sent (or -1 if none to send at the moment)
902 : int8_t deferred_message_to_send_index(uint16_t now16_ms);
903 : // cache of which deferred message should be sent next:
904 : int8_t next_deferred_message_to_send_cache = -1;
905 :
906 : struct deferred_message_bucket_t {
907 : Bitmask<MSG_LAST> ap_message_ids;
908 : uint16_t interval_ms;
909 : uint16_t last_sent_ms; // from AP_HAL::millis16()
910 : };
911 : deferred_message_bucket_t deferred_message_bucket[10];
912 : static const uint8_t no_bucket_to_send = -1;
913 : static const ap_message no_message_to_send = (ap_message)-1;
914 : uint8_t sending_bucket_id = no_bucket_to_send;
915 : Bitmask<MSG_LAST> bucket_message_ids_to_send;
916 :
917 : ap_message next_deferred_bucket_message_to_send(uint16_t now16_ms);
918 : void find_next_bucket_to_send(uint16_t now16_ms);
919 : void remove_message_from_bucket(int8_t bucket, ap_message id);
920 :
921 : // bitmask of IDs the code has spontaneously decided it wants to
922 : // send out. Examples include HEARTBEAT (gcs_send_heartbeat)
923 : Bitmask<MSG_LAST> pushed_ap_message_ids;
924 :
925 : // returns true if it is OK to send a message while we are in
926 : // delay callback. In particular, when we are doing sensor init
927 : // we still send heartbeats.
928 : bool should_send_message_in_delay_callback(const ap_message id) const;
929 :
930 : // if true is returned, interval will contain the default interval for id
931 : bool get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const;
932 : // if true is returned, interval will contain the default interval for id
933 : // returns an interval in milliseconds for any ap_message in stream id
934 : uint16_t get_interval_for_stream(GCS_MAVLINK::streams id) const;
935 : // set an inverval for a specific mavlink message. Returns false
936 : // on failure (typically because there is no mapping from that
937 : // mavlink ID to an ap_message)
938 : bool set_mavlink_message_id_interval(const uint32_t mavlink_id,
939 : const uint16_t interval_ms);
940 : // map a mavlink ID to an ap_message which, if passed to
941 : // try_send_message, will cause a mavlink message with that id to
942 : // be emitted. Returns MSG_LAST if no such mapping exists.
943 : ap_message mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const;
944 : // set the interval at which an ap_message should be emitted (in ms)
945 : bool set_ap_message_interval(enum ap_message id, uint16_t interval_ms);
946 : // call set_ap_message_interval for each entry in a stream,
947 : // the interval being based on the stream's rate
948 : void initialise_message_intervals_for_stream(GCS_MAVLINK::streams id);
949 : // call initialise_message_intervals_for_stream on every stream:
950 : void initialise_message_intervals_from_streamrates();
951 : // boolean that indicated that message intervals have been set
952 : // from streamrates:
953 : bool deferred_messages_initialised;
954 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
955 : // read configuration files from (e.g.) SD and ROMFS, set
956 : // intervals from same
957 : void initialise_message_intervals_from_config_files();
958 : // read file, set message intervals from it:
959 : void get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &);
960 : #endif
961 : // return interval deferred message bucket should be sent after.
962 : // When sending parameters and waypoints this may be longer than
963 : // the interval specified in "deferred"
964 : uint16_t get_reschedule_interval_ms(const deferred_message_bucket_t &deferred) const;
965 :
966 : bool do_try_send_message(const ap_message id);
967 :
968 : // time when we missed sending a parameter for GCS
969 : static uint32_t reserve_param_space_start_ms;
970 :
971 : // bitmask of what mavlink channels are active
972 : static mavlink_channel_mask_t mavlink_active;
973 :
974 : // bitmask of what mavlink channels are private
975 : static mavlink_channel_mask_t mavlink_private;
976 :
977 : // bitmask of what mavlink channels are streaming
978 : static mavlink_channel_mask_t chan_is_streaming;
979 :
980 : // mavlink routing object
981 : static MAVLink_routing routing;
982 :
983 : struct pending_param_request {
984 : mavlink_channel_t chan;
985 : int16_t param_index;
986 : char param_name[AP_MAX_NAME_SIZE+1];
987 : uint8_t src_system_id;
988 : uint8_t src_component_id;
989 : };
990 :
991 : struct pending_param_reply {
992 : mavlink_channel_t chan;
993 : float value;
994 : enum ap_var_type p_type;
995 : int16_t param_index;
996 : uint16_t count;
997 : char param_name[AP_MAX_NAME_SIZE+1];
998 : uint8_t src_system_id;
999 : uint8_t src_component_id;
1000 : MAV_PARAM_ERROR param_error;
1001 : };
1002 :
1003 : // queue of pending parameter requests and replies
1004 : static ObjectBuffer<pending_param_request> param_requests;
1005 : static ObjectBuffer<pending_param_reply> param_replies;
1006 :
1007 : // have we registered the IO timer callback?
1008 : static bool param_timer_registered;
1009 :
1010 : // IO timer callback for parameters
1011 : void param_io_timer(void);
1012 :
1013 : // support for returning explicit error for parameter protocol
1014 : void send_param_error(const mavlink_message_t &msg, const mavlink_param_set_t ¶m_set, MAV_PARAM_ERROR error);
1015 : void send_param_error(const pending_param_reply &msg, MAV_PARAM_ERROR error);
1016 : uint8_t send_parameter_async_replies();
1017 :
1018 : void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const;
1019 :
1020 0 : virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) { return false; };
1021 0 : virtual void handle_change_alt_request(Location &location) {};
1022 : void handle_common_mission_message(const mavlink_message_t &msg);
1023 :
1024 0 : virtual void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) {};
1025 :
1026 : void handle_vicon_position_estimate(const mavlink_message_t &msg);
1027 : void handle_vision_position_estimate(const mavlink_message_t &msg);
1028 : void handle_global_vision_position_estimate(const mavlink_message_t &msg);
1029 : void handle_att_pos_mocap(const mavlink_message_t &msg);
1030 : void handle_odometry(const mavlink_message_t &msg);
1031 : void handle_common_vision_position_estimate_data(const uint64_t usec,
1032 : const float x,
1033 : const float y,
1034 : const float z,
1035 : const float roll,
1036 : const float pitch,
1037 : const float yaw,
1038 : const float covariance[21],
1039 : const uint8_t reset_counter,
1040 : const uint16_t payload_size);
1041 : void handle_vision_speed_estimate(const mavlink_message_t &msg);
1042 : void handle_landing_target(const mavlink_message_t &msg);
1043 : void handle_generator_message(const mavlink_message_t &msg);
1044 :
1045 : void lock_channel(const mavlink_channel_t chan, bool lock);
1046 :
1047 : #if AP_MAVLINK_SIGNING_ENABLED
1048 : mavlink_signing_t signing;
1049 : static mavlink_signing_streams_t signing_streams;
1050 : static uint32_t last_signing_save_ms;
1051 :
1052 : static StorageAccess _signing_storage;
1053 : static bool signing_key_save(const struct SigningKey &key);
1054 : static bool signing_key_load(struct SigningKey &key);
1055 : void load_signing_key(void);
1056 : bool signing_enabled(void) const;
1057 : static void save_signing_timestamp(bool force_save_now);
1058 : #endif // AP_MAVLINK_SIGNING_ENABLED
1059 :
1060 : #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
1061 : // structure containing default intervals read from files for this
1062 : // link:
1063 : DefaultIntervalsFromFiles *default_intervals_from_files;
1064 : #endif
1065 :
1066 : // alternative protocol handler support
1067 : struct {
1068 : GCS_MAVLINK::protocol_handler_fn_t handler;
1069 : uint32_t last_mavlink_ms;
1070 : uint32_t last_alternate_ms;
1071 : bool active;
1072 : } alternative;
1073 :
1074 : JitterCorrection lag_correction;
1075 :
1076 : // we cache the current location and send it even if the AHRS has
1077 : // no idea where we are:
1078 : Location global_position_current_loc;
1079 :
1080 : uint8_t last_tx_seq;
1081 : uint16_t send_packet_count;
1082 : uint16_t out_of_space_to_send_count; // number of times HAVE_PAYLOAD_SPACE and friends have returned false
1083 :
1084 : #if GCS_DEBUG_SEND_MESSAGE_TIMINGS
1085 : struct {
1086 : uint32_t longest_time_us;
1087 : ap_message longest_id;
1088 : uint32_t no_space_for_message;
1089 : uint16_t statustext_last_sent_ms;
1090 : uint32_t behind;
1091 : uint32_t out_of_time;
1092 : uint16_t fnbts_maxtime;
1093 : uint32_t max_retry_deferred_body_us;
1094 : uint8_t max_retry_deferred_body_type;
1095 : } try_send_message_stats;
1096 : uint16_t max_slowdown_ms;
1097 : #endif
1098 :
1099 : uint32_t last_mavlink_stats_logged;
1100 :
1101 : uint8_t last_battery_status_idx;
1102 :
1103 : // if we've ever sent a DISTANCE_SENSOR message out of an
1104 : // orientation we continue to send it out, even if it is not
1105 : // longer valid.
1106 : uint8_t proximity_ever_valid_bitmask;
1107 :
1108 : // true if we should NOT do MAVLink on this port (usually because
1109 : // someone's doing SERIAL_CONTROL over mavlink)
1110 : bool _locked;
1111 :
1112 : // Handling of AVAILABLE_MODES
1113 : struct {
1114 : bool requested;
1115 : bool should_send;
1116 : // Note these start at 1
1117 : uint8_t requested_index;
1118 : uint8_t next_index;
1119 : // Sequence number should be incremented when available modes changes
1120 : // Sent in AVAILABLE_MODES_MONITOR msg
1121 : uint8_t available_modes_sequence;
1122 : } available_modes;
1123 : bool send_available_modes();
1124 : bool send_available_mode_monitor();
1125 : void available_modes_changed();
1126 :
1127 : };
1128 :
1129 : /// @class GCS
1130 : /// @brief global GCS object
1131 : class GCS
1132 : {
1133 :
1134 : public:
1135 :
1136 2098 : GCS() {
1137 2098 : if (_singleton == nullptr) {
1138 2098 : _singleton = this;
1139 : } else {
1140 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1141 : // this is a serious problem, but we don't need to kill a
1142 : // real vehicle
1143 0 : AP_HAL::panic("GCS must be singleton");
1144 : #endif
1145 : }
1146 :
1147 2098 : AP_Param::setup_object_defaults(this, var_info);
1148 2098 : };
1149 :
1150 1144552908 : static class GCS *get_singleton() {
1151 1144552908 : return _singleton;
1152 : }
1153 :
1154 : static const struct AP_Param::GroupInfo var_info[];
1155 :
1156 : virtual uint32_t custom_mode() const = 0;
1157 : virtual MAV_TYPE frame_type() const = 0;
1158 4 : virtual const char* frame_string() const { return nullptr; }
1159 :
1160 : struct statustext_t {
1161 : mavlink_statustext_t msg;
1162 : uint16_t entry_created_ms;
1163 : mavlink_channel_mask_t bitmask;
1164 : };
1165 : class StatusTextQueue : public ObjectArray<statustext_t> {
1166 : public:
1167 : using ObjectArray::ObjectArray;
1168 42199882 : HAL_Semaphore &semaphore() { return _sem; }
1169 : void prune();
1170 : private:
1171 : // a lock for the statustext queue, to make it safe to use send_text()
1172 : // from multiple threads
1173 : HAL_Semaphore _sem;
1174 :
1175 : uint32_t last_prune_ms;
1176 : };
1177 :
1178 15398458 : StatusTextQueue &statustext_queue() {
1179 15398458 : return _statustext_queue;
1180 : }
1181 :
1182 : /*
1183 : return true if a MAVLink system ID is a GCS
1184 : */
1185 : bool sysid_is_gcs(uint8_t sysid) const;
1186 :
1187 : // last time traffic was seen from my designated GCS. traffic
1188 : // includes heartbeats and some manual control messages.
1189 174792 : uint32_t sysid_mygcs_last_seen_time_ms() const {
1190 174792 : return _sysid_gcs_last_seen_time_ms;
1191 : }
1192 : // called when valid traffic has been seen from our GCS. This is
1193 : // usually only called from GCS_MAVLINK::sysid_mygcs_seen(..)!
1194 15610 : void sysid_mygcs_seen(uint32_t seen_time_ms) {
1195 15610 : _sysid_gcs_last_seen_time_ms = seen_time_ms;
1196 15610 : }
1197 :
1198 : void send_to_active_channels(uint32_t msgid, const char *pkt);
1199 :
1200 : void send_text(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(3, 4);
1201 : void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list);
1202 : virtual void send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, mavlink_channel_mask_t mask);
1203 : mavlink_channel_mask_t statustext_send_channel_mask() const;
1204 :
1205 : virtual GCS_MAVLINK *chan(const uint8_t ofs) = 0;
1206 : virtual const GCS_MAVLINK *chan(const uint8_t ofs) const = 0;
1207 : // return the number of valid GCS objects
1208 352827664 : uint8_t num_gcs() const { return _num_gcs; };
1209 : void send_message(enum ap_message id);
1210 : void send_mission_item_reached_message(uint16_t mission_index);
1211 : void send_named_float(const char *name, float value) const;
1212 : void send_named_int(const char *name, int32_t value) const;
1213 : void send_named_string(const char *name, const char *value) const;
1214 :
1215 : void send_parameter_value(const char *param_name,
1216 : ap_var_type param_type,
1217 : float param_value);
1218 :
1219 : // an array of objects used to handle each of the different
1220 : // protocol types we support. This is indexed by the enumeration
1221 : // MAV_MISSION_TYPE, taking advantage of the fact that fence,
1222 : // mission and rally have values 0, 1 and 2. Indexing should be via
1223 : // get_prot_for_mission_type to do bounds checking.
1224 : static class MissionItemProtocol *missionitemprotocols[3];
1225 : class MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const;
1226 : void try_send_queued_message_for_type(MAV_MISSION_TYPE type) const;
1227 :
1228 : void update_send();
1229 : void update_receive();
1230 :
1231 : // minimum amount of time (in microseconds) that must remain in
1232 : // the main scheduler loop before we are allowed to send any
1233 : // mavlink messages. We want to prioritise the main flight
1234 : // control loop over communications
1235 63644056 : virtual uint16_t min_loop_time_remaining_for_message_send_us() const {
1236 63644056 : return 200;
1237 : }
1238 :
1239 : void init();
1240 : void setup_console();
1241 : void setup_uarts();
1242 :
1243 : enum class Option {
1244 : GCS_SYSID_ENFORCE = (1U << 0),
1245 : };
1246 2725120 : bool option_is_enabled(Option option) const {
1247 2725120 : return (mav_options & (uint16_t)option) != 0;
1248 : }
1249 :
1250 : // returns true if attempts to set parameters via PARAM_SET or via
1251 : // file upload in mavftp should be honoured:
1252 10144 : bool get_allow_param_set() const {
1253 10144 : return allow_param_set;
1254 : }
1255 : // can be used to force sets via PARAM_SET or via mavftp file
1256 : // upload to be ignored by the GCS library:
1257 5 : void set_allow_param_set(bool new_allowed) {
1258 5 : allow_param_set = new_allowed;
1259 5 : }
1260 :
1261 : bool out_of_time() const;
1262 :
1263 : #if AP_FRSKY_TELEM_ENABLED
1264 : // frsky backend
1265 : class AP_Frsky_Telem *frsky;
1266 : #endif
1267 :
1268 : #if AP_LTM_TELEM_ENABLED
1269 : // LTM backend
1270 : AP_LTM_Telem ltm_telemetry;
1271 : #endif
1272 :
1273 : #if AP_DEVO_TELEM_ENABLED
1274 : // Devo backend
1275 : AP_DEVO_Telem devo_telemetry;
1276 : #endif
1277 :
1278 : // install an alternative protocol handler
1279 : bool install_alternative_protocol(mavlink_channel_t chan, GCS_MAVLINK::protocol_handler_fn_t handler);
1280 :
1281 : // get the VFR_HUD throttle
1282 568 : int16_t get_hud_throttle(void) const {
1283 568 : const GCS_MAVLINK *link = chan(0);
1284 568 : if (link == nullptr) {
1285 0 : return 0;
1286 : }
1287 568 : return link->vfr_hud_throttle();
1288 : }
1289 :
1290 : // update uart pass-thru
1291 : void update_passthru();
1292 :
1293 : void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
1294 1131129 : virtual bool vehicle_initialised() const { return true; }
1295 :
1296 37 : virtual bool simple_input_active() const { return false; }
1297 37 : virtual bool supersimple_input_active() const { return false; }
1298 :
1299 : // set message interval for a given serial port and message id
1300 : // this function is for use by lua scripts, most consumers should use the channel level function
1301 : MAV_RESULT set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us);
1302 :
1303 : uint8_t get_channel_from_port_number(uint8_t port_num);
1304 :
1305 : #if HAL_HIGH_LATENCY2_ENABLED
1306 : bool high_latency_link_enabled;
1307 : void enable_high_latency_connections(bool enabled);
1308 : bool get_high_latency_status();
1309 : #endif // HAL_HIGH_LATENCY2_ENABLED
1310 :
1311 2150 : uint8_t sysid_this_mav() const { return sysid; }
1312 141783846 : uint32_t telem_delay() const { return mav_telem_delay; }
1313 :
1314 : #if AP_SCRIPTING_ENABLED
1315 : // lua access to command_int
1316 : MAV_RESULT lua_command_int_packet(const mavlink_command_int_t &packet);
1317 : #endif
1318 :
1319 : // Sequence number should be incremented when available modes changes
1320 : // Sent in AVAILABLE_MODES_MONITOR msg
1321 : void available_modes_changed();
1322 :
1323 : protected:
1324 :
1325 : virtual GCS_MAVLINK *new_gcs_mavlink_backend(AP_HAL::UARTDriver &uart) = 0;
1326 :
1327 : HAL_Semaphore control_sensors_sem; // protects the three bitmasks
1328 : uint32_t control_sensors_present;
1329 : uint32_t control_sensors_enabled;
1330 : uint32_t control_sensors_health;
1331 0 : virtual void update_vehicle_sensor_status_flags() {}
1332 :
1333 : static const struct AP_Param::GroupInfo *_chan_var_info[MAVLINK_COMM_NUM_BUFFERS];
1334 : uint8_t _num_gcs;
1335 : GCS_MAVLINK *_chan[MAVLINK_COMM_NUM_BUFFERS];
1336 :
1337 : // parameters
1338 : AP_Int16 sysid;
1339 : AP_Int16 mav_gcs_sysid;
1340 : AP_Int16 mav_gcs_sysid_high;
1341 : AP_Enum16<Option> mav_options;
1342 : AP_Int8 mav_telem_delay;
1343 :
1344 : private:
1345 :
1346 : static GCS *_singleton;
1347 :
1348 : bool create_gcs_mavlink_backend(AP_HAL::UARTDriver &uart) WARN_IF_UNUSED;
1349 :
1350 : char statustext_printf_buffer[256+1];
1351 :
1352 : #if AP_GPS_ENABLED
1353 525694 : virtual AP_GPS_FixType min_status_for_gps_healthy() const {
1354 : // NO_FIX simply excludes NO_GPS
1355 525694 : return AP_GPS_FixType::NONE;
1356 : }
1357 : #endif
1358 :
1359 : void update_sensor_status_flags();
1360 :
1361 : // time we last saw traffic from our GCS. Note that there is an
1362 : // identically named field in GCS_MAVLINK:: which is the most
1363 : // recent time that backend saw traffic from MAV_GCS_SYSID
1364 : uint32_t _sysid_gcs_last_seen_time_ms;
1365 :
1366 : void service_statustext(void);
1367 : #if HAL_MEM_CLASS <= HAL_MEM_CLASS_192
1368 : static const uint8_t _status_capacity = 10;
1369 : #else
1370 : static const uint8_t _status_capacity = 30;
1371 : #endif
1372 :
1373 : // ephemeral state indicating whether the GCS (including via
1374 : // PARAM_SET and upload of param values via FTP) should be allowed
1375 : // to change parameter values:
1376 : bool allow_param_set = HAL_GCS_ALLOW_PARAM_SET_DEFAULT;
1377 :
1378 : // queue of outgoing statustext messages. Each entry consumes 58
1379 : // bytes of RAM on stm32
1380 : StatusTextQueue _statustext_queue{_status_capacity};
1381 :
1382 : // true if we have already allocated protocol objects:
1383 : bool initialised_missionitemprotocol_objects;
1384 :
1385 : // true if update_send has ever been called:
1386 : bool update_send_has_been_called;
1387 :
1388 : // handle passthru between two UARTs
1389 : struct {
1390 : bool enabled;
1391 : bool timer_installed;
1392 : AP_HAL::UARTDriver *port1;
1393 : AP_HAL::UARTDriver *port2;
1394 : uint32_t start_ms;
1395 : uint32_t last_ms;
1396 : uint32_t last_port1_data_ms;
1397 : uint32_t baud1;
1398 : uint32_t baud2;
1399 : uint8_t parity1;
1400 : uint8_t parity2;
1401 : uint8_t timeout_s;
1402 : HAL_Semaphore sem;
1403 : } _passthru;
1404 :
1405 : // timer called to implement pass-thru
1406 : void passthru_timer();
1407 :
1408 : // this contains the index of the GCS_MAVLINK backend we will
1409 : // first call update_send on. It is incremented each time
1410 : // GCS::update_send is called so we don't starve later links of
1411 : // time in which they are permitted to send messages.
1412 : uint8_t first_backend_to_send;
1413 :
1414 : };
1415 :
1416 : GCS &gcs();
1417 :
1418 : // send text when we do have a GCS
1419 : #if !defined(HAL_BUILD_AP_PERIPH)
1420 : #define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args)
1421 : #define AP_HAVE_GCS_SEND_TEXT 1
1422 : #else
1423 : extern "C" {
1424 : void can_printf_severity(uint8_t severity, const char *fmt, ...);
1425 : }
1426 : #define GCS_SEND_TEXT(severity, format, args...) can_printf_severity(severity, format, ##args)
1427 : #define AP_HAVE_GCS_SEND_TEXT 1
1428 : #endif
1429 :
1430 : #define GCS_SEND_MESSAGE(msg) gcs().send_message(msg)
1431 :
1432 : #elif defined(HAL_BUILD_AP_PERIPH) && !defined(STM32F1)
1433 :
1434 : // map send text to can_printf() on larger AP_Periph boards
1435 : extern "C" {
1436 : void can_printf_severity(uint8_t severity, const char *fmt, ...);
1437 : }
1438 : #define GCS_SEND_TEXT(severity, format, args...) can_printf_severity(severity, format, ##args)
1439 : #define GCS_SEND_MESSAGE(msg)
1440 : #define AP_HAVE_GCS_SEND_TEXT 1
1441 :
1442 : /*
1443 : we need a severity enum for the can_printf_severity function with no GCS present
1444 : */
1445 : #ifndef HAVE_ENUM_MAV_SEVERITY
1446 : enum MAV_SEVERITY
1447 : {
1448 : MAV_SEVERITY_EMERGENCY=0,
1449 : MAV_SEVERITY_ALERT=1,
1450 : MAV_SEVERITY_CRITICAL=2,
1451 : MAV_SEVERITY_ERROR=3,
1452 : MAV_SEVERITY_WARNING=4,
1453 : MAV_SEVERITY_NOTICE=5,
1454 : MAV_SEVERITY_INFO=6,
1455 : MAV_SEVERITY_DEBUG=7,
1456 : MAV_SEVERITY_ENUM_END=8,
1457 : };
1458 : #define HAVE_ENUM_MAV_SEVERITY
1459 : #endif
1460 :
1461 : #else // HAL_GCS_ENABLED
1462 : // empty send text when we have no GCS
1463 : #define GCS_SEND_TEXT(severity, format, args...)
1464 : #define GCS_SEND_MESSAGE(msg)
1465 : #define AP_HAVE_GCS_SEND_TEXT 0
1466 :
1467 : #endif // HAL_GCS_ENABLED
|
