/
GCS.h
1402 lines (1148 loc) · 53.2 KB
/
GCS.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/// @file GCS.h
/// @brief Interface definition for the various Ground Control System
// protocols.
#pragma once
#include "GCS_config.h"
#if HAL_GCS_ENABLED
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe_config.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>
#include "GCS_MAVLink.h"
#include <AP_Mission/AP_Mission.h>
#include <stdint.h>
#include "MAVLink_routing.h"
#include <AP_RTC/JitterCorrection.h>
#include <AP_Common/Bitmask.h>
#include <AP_LTM_Telem/AP_LTM_Telem.h>
#include <AP_Devo_Telem/AP_Devo_Telem.h>
#include <AP_Filesystem/AP_Filesystem_config.h>
#include <AP_Frsky_Telem/AP_Frsky_config.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Mount/AP_Mount_config.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_RangeFinder/AP_RangeFinder_config.h>
#include <AP_Winch/AP_Winch_config.h>
#include <AP_AHRS/AP_AHRS_config.h>
#include <AP_Arming/AP_Arming_config.h>
#include "ap_message.h"
#define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0
// macros used to determine if a message will fit in the space available.
void gcs_out_of_space_to_send(mavlink_channel_t chan);
bool check_payload_size(mavlink_channel_t chan, uint16_t max_payload_len);
// important note: despite the names, these messages do NOT check to
// see if the payload will fit in the buffer. They check to see if
// the packed message along with any channel overhead will fit.
// PAYLOAD_SIZE returns the amount of space required to send the
// mavlink message with id id on channel chan. Mavlink2 has higher
// overheads than mavlink1, for example.
// check if a message will fit in the payload space available
#define PAYLOAD_SIZE(chan, id) (unsigned(GCS_MAVLINK::packet_overhead_chan(chan)+MAVLINK_MSG_ID_ ## id ## _LEN))
// HAVE_PAYLOAD_SPACE evaluates to an expression that can be used
// anywhere in the code to determine if the mavlink message with ID id
// can currently fit in the output of _chan. Note the use of the ","
// operator here to increment a counter.
#define HAVE_PAYLOAD_SPACE(_chan, id) (comm_get_txspace(_chan) >= PAYLOAD_SIZE(_chan, id) ? true : (gcs_out_of_space_to_send(_chan), false))
// CHECK_PAYLOAD_SIZE - macro which may only be used within a
// GCS_MAVLink object's methods. It inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the current object's
// output
#define CHECK_PAYLOAD_SIZE(id) if (!check_payload_size(MAVLINK_MSG_ID_ ## id ## _LEN)) return false
// CHECK_PAYLOAD_SIZE2 - macro which inserts code which will
// immediately return false from the current function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan". It is expecting there to be a "chan" variable in
// scope.
#define CHECK_PAYLOAD_SIZE2(id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return false
// CHECK_PAYLOAD_SIZE2_VOID - macro which inserts code which will
// immediately return from the current (void) function if there is no
// room to fit the mavlink message with id id on the mavlink output
// channel "chan".
#define CHECK_PAYLOAD_SIZE2_VOID(chan, id) if (!HAVE_PAYLOAD_SPACE(chan, id)) return
// convenience macros for defining which ap_message ids are in which streams:
#define MAV_STREAM_ENTRY(stream_name) \
{ \
GCS_MAVLINK::stream_name, \
stream_name ## _msgs, \
ARRAY_SIZE(stream_name ## _msgs) \
}
#define MAV_STREAM_TERMINATOR { (streams)0, nullptr, 0 }
// code generation; avoid each subclass duplicating these two methods
// and just changing the name. These methods allow retrieval of
// objects specific to the vehicle's subclass, which the vehicle can
// then call its own specific methods on
#define GCS_MAVLINK_CHAN_METHOD_DEFINITIONS(subclass_name) \
subclass_name *chan(const uint8_t ofs) override { \
if (ofs >= _num_gcs) { \
return nullptr; \
} \
return (subclass_name *)_chan[ofs]; \
} \
\
const subclass_name *chan(const uint8_t ofs) const override { \
if (ofs >= _num_gcs) { \
return nullptr; \
} \
return (subclass_name *)_chan[ofs]; \
}
#define GCS_MAVLINK_NUM_STREAM_RATES 10
class GCS_MAVLINK_Parameters
{
public:
GCS_MAVLINK_Parameters();
static const struct AP_Param::GroupInfo var_info[];
// saveable rate of each stream
AP_Int16 streamRates[GCS_MAVLINK_NUM_STREAM_RATES];
};
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
class DefaultIntervalsFromFiles
{
public:
DefaultIntervalsFromFiles(uint16_t max_num);
~DefaultIntervalsFromFiles();
void set(ap_message id, uint16_t interval);
uint16_t num_intervals() const {
return _num_intervals;
}
bool get_interval_for_ap_message_id(ap_message id, uint16_t &interval) const;
ap_message id_at(uint8_t ofs) const;
uint16_t interval_at(uint8_t ofs) const;
private:
struct from_file_default_interval {
ap_message id;
uint16_t interval;
};
from_file_default_interval *_intervals;
uint16_t _num_intervals;
uint16_t _max_intervals;
};
#endif
class GCS_MAVLINK_InProgress
{
public:
enum class Type {
NONE,
AIRSPEED_CAL,
SD_FORMAT,
};
// these can fail if there's no space on the channel to send the ack:
bool conclude(MAV_RESULT result);
bool send_in_progress();
// abort task without sending any further ACKs:
void abort() { task = Type::NONE; }
Type task;
MAV_CMD mav_cmd;
static class GCS_MAVLINK_InProgress *get_task(MAV_CMD cmd, Type t, uint8_t sysid, uint8_t compid, mavlink_channel_t chan);
static void check_tasks();
private:
uint8_t requesting_sysid;
uint8_t requesting_compid;
mavlink_channel_t chan;
bool send_ack(MAV_RESULT result);
static GCS_MAVLINK_InProgress in_progress_tasks[1];
// allocate a task-tracking ID
static uint32_t last_check_ms;
};
///
/// @class GCS_MAVLINK
/// @brief MAVLink transport control class
///
class GCS_MAVLINK
{
public:
friend class GCS;
GCS_MAVLINK(GCS_MAVLINK_Parameters ¶meters, AP_HAL::UARTDriver &uart);
virtual ~GCS_MAVLINK() {}
// accessors used to retrieve objects used for parsing incoming messages:
mavlink_message_t *channel_buffer() { return &_channel_buffer; }
mavlink_status_t *channel_status() { return &_channel_status; }
void update_receive(uint32_t max_time_us=1000);
void update_send();
bool init(uint8_t instance);
void send_message(enum ap_message id);
void send_text(MAV_SEVERITY severity, const char *fmt, ...) const FMT_PRINTF(3, 4);
void queued_param_send();
void queued_mission_request_send();
bool sending_mavlink1() const;
// returns true if we are requesting any items from the GCS:
bool requesting_mission_items() const;
/// Check for available transmit space
uint16_t txspace() const {
if (_locked) {
return 0;
}
// there were concerns over return a too-large value for
// txspace (in case we tried to do too much with the space in
// a single loop):
return MIN(_port->txspace(), 8192U);
}
bool check_payload_size(uint16_t max_payload_len);
// this is called when we discover we'd like to send something but can't:
void out_of_space_to_send() { out_of_space_to_send_count++; }
void send_mission_ack(const mavlink_message_t &msg,
MAV_MISSION_TYPE mission_type,
MAV_MISSION_RESULT result) const {
CHECK_PAYLOAD_SIZE2_VOID(chan, MISSION_ACK);
mavlink_msg_mission_ack_send(chan,
msg.sysid,
msg.compid,
result,
mission_type);
}
// packetReceived is called on any successful decode of a mavlink message
virtual void packetReceived(const mavlink_status_t &status,
const mavlink_message_t &msg);
// send a mavlink_message_t out this GCS_MAVLINK connection.
void send_message(uint32_t msgid, const char *pkt) {
const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
if (entry == nullptr) {
return;
}
send_message(pkt, entry);
}
void send_message(const char *pkt, const mavlink_msg_entry_t *entry) {
if (!check_payload_size(entry->max_msg_len)) {
return;
}
_mav_finalize_message_chan_send(chan,
entry->msgid,
pkt,
entry->min_msg_len,
entry->max_msg_len,
entry->crc_extra);
}
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
virtual uint8_t sysid_my_gcs() const = 0;
virtual bool sysid_enforce() const { return false; }
// NOTE: param_name here must point to a 16+1 byte buffer - so do
// NOT try to pass in a static-char-* unless it does have that
// length!
void send_parameter_value(const char *param_name,
ap_var_type param_type,
float param_value);
// NOTE! The streams enum below and the
// set of AP_Int16 stream rates _must_ be
// kept in the same order
enum streams : uint8_t {
STREAM_RAW_SENSORS,
STREAM_EXTENDED_STATUS,
STREAM_RC_CHANNELS,
STREAM_RAW_CONTROLLER,
STREAM_POSITION,
STREAM_EXTRA1,
STREAM_EXTRA2,
STREAM_EXTRA3,
STREAM_PARAMS,
STREAM_ADSB,
NUM_STREAMS
};
// streams must be moved out into the top level for
// GCS_MAVLINK_Parameters to be able to use it. This is an
// extensive change, so we 'll just keep them in sync with a
// static assert for now:
static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
// return true if this channel has hardware flow control
bool have_flow_control();
bool is_active() const {
return GCS_MAVLINK::active_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
}
bool is_streaming() const {
return sending_bucket_id != no_bucket_to_send;
}
mavlink_channel_t get_chan() const { return chan; }
uint32_t get_last_heartbeat_time() const { return last_heartbeat_time; };
uint32_t last_heartbeat_time; // milliseconds
static uint32_t last_radio_status_remrssi_ms() {
return last_radio_status.remrssi_ms;
}
static float telemetry_radio_rssi(); // 0==no signal, 1==full signal
// mission item index to be sent on queued msg, delayed or not
uint16_t mission_item_reached_index = AP_MISSION_CMD_INDEX_NONE;
// generate a MISSION_STATE enumeration value for where the
// mission is up to:
virtual MISSION_STATE mission_state(const class AP_Mission &mission) const;
// send a mission_current message for the supplied waypoint
void send_mission_current(const class AP_Mission &mission, uint16_t seq);
// common send functions
void send_heartbeat(void) const;
void send_meminfo(void);
void send_fence_status() const;
void send_power_status(void);
#if HAL_WITH_MCU_MONITORING
void send_mcu_status(void);
#endif
void send_battery_status(const uint8_t instance) const;
bool send_battery_status();
void send_distance_sensor();
// send_rangefinder sends only if a downward-facing instance is
// found. Rover overrides this!
#if AP_RANGEFINDER_ENABLED
virtual void send_rangefinder() const;
#endif
void send_proximity();
virtual void send_nav_controller_output() const = 0;
virtual void send_pid_tuning() = 0;
void send_ahrs2();
void send_system_time() const;
void send_rc_channels() const;
void send_rc_channels_raw() const;
void send_raw_imu();
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));
void send_scaled_pressure();
void send_scaled_pressure2();
virtual void send_scaled_pressure3(); // allow sub to override this
void send_simstate() const;
void send_sim_state() const;
void send_ahrs();
#if AP_MAVLINK_BATTERY2_ENABLED
void send_battery2();
#endif
void send_opticalflow();
virtual void send_attitude() const;
virtual void send_attitude_quaternion() const;
void send_autopilot_version() const;
void send_extended_sys_state() const;
void send_local_position() const;
void send_vfr_hud();
void send_vibration() const;
void send_gimbal_device_attitude_status() const;
void send_gimbal_manager_information() const;
void send_gimbal_manager_status() const;
void send_named_float(const char *name, float value) const;
void send_home_position() const;
void send_gps_global_origin() const;
virtual void send_attitude_target() {};
virtual void send_position_target_global_int() { };
virtual void send_position_target_local_ned() { };
void send_servo_output_raw();
void send_accelcal_vehicle_position(uint32_t position);
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));
void send_sys_status();
void send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc);
void send_rpm() const;
void send_generator_status() const;
#if AP_WINCH_ENABLED
virtual void send_winch_status() const {};
#endif
void send_water_depth() const;
int8_t battery_remaining_pct(const uint8_t instance) const;
#if HAL_HIGH_LATENCY2_ENABLED
void send_high_latency2() const;
#endif // HAL_HIGH_LATENCY2_ENABLED
void send_uavionix_adsb_out_status() const;
void send_autopilot_state_for_gimbal_device() const;
// lock a channel, preventing use by MAVLink
void lock(bool _lock) {
_locked = _lock;
}
// returns true if this channel isn't available for MAVLink
bool locked() const {
return _locked;
}
// return a bitmap of active channels. Used by libraries to loop
// over active channels to send to all active channels
static uint8_t active_channel_mask(void) { return mavlink_active; }
// return a bitmap of streaming channels
static uint8_t streaming_channel_mask(void) { return chan_is_streaming; }
// return a bitmap of private channels
static uint8_t private_channel_mask(void) { return mavlink_private; }
// set a channel as private. Private channels get sent heartbeats, but
// don't get broadcast packets or forwarded packets
static void set_channel_private(mavlink_channel_t chan);
// return true if channel is private
static bool is_private(mavlink_channel_t _chan) {
return (mavlink_private & (1U<<(unsigned)_chan)) != 0;
}
// return true if channel is private
bool is_private(void) const { return is_private(chan); }
#if HAL_HIGH_LATENCY2_ENABLED
// true if this is a high latency link
bool is_high_latency_link;
#endif
/*
send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned
*/
static void send_to_components(uint32_t msgid, const char *pkt, uint8_t pkt_len) { routing.send_to_components(msgid, pkt, pkt_len); }
/*
allow forwarding of packets / heartbeats to be blocked as required by some components to reduce traffic
*/
static void disable_channel_routing(mavlink_channel_t chan) { routing.no_route_mask |= (1U<<(chan-MAVLINK_COMM_0)); }
/*
search for a component in the routing table with given mav_type and retrieve it's sysid, compid and channel
returns if a matching component is found
*/
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); }
/*
search for the first vehicle or component in the routing table with given mav_type and component id and retrieve its sysid and channel
returns true if a match is found
*/
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); }
// same as above, but returns a pointer to the GCS_MAVLINK object
// corresponding to the channel
static GCS_MAVLINK *find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid);
// update signing timestamp on GPS lock
static void update_signing_timestamp(uint64_t timestamp_usec);
// return current packet overhead for a channel
static uint8_t packet_overhead_chan(mavlink_channel_t chan);
// alternative protocol function handler
FUNCTOR_TYPEDEF(protocol_handler_fn_t, bool, uint8_t, AP_HAL::UARTDriver *);
struct stream_entries {
const streams stream_id;
const ap_message *ap_message_ids;
const uint8_t num_ap_message_ids;
};
// vehicle subclass cpp files should define this:
static const struct stream_entries all_stream_entries[];
virtual uint64_t capabilities() const;
uint16_t get_stream_slowdown_ms() const { return stream_slowdown_ms; }
uint8_t get_last_txbuf() const { return last_txbuf; }
MAV_RESULT set_message_interval(uint32_t msg_id, int32_t interval_us);
protected:
bool mavlink_coordinate_frame_to_location_alt_frame(MAV_FRAME coordinate_frame,
Location::AltFrame &frame);
// overridable method to check for packet acceptance. Allows for
// enforcement of GCS sysid
bool accept_packet(const mavlink_status_t &status, const mavlink_message_t &msg) const;
void set_ekf_origin(const Location& loc);
virtual MAV_MODE base_mode() const = 0;
MAV_STATE system_status() const;
virtual MAV_STATE vehicle_system_status() const = 0;
virtual MAV_VTOL_STATE vtol_state() const { return MAV_VTOL_STATE_UNDEFINED; }
virtual MAV_LANDED_STATE landed_state() const { return MAV_LANDED_STATE_UNDEFINED; }
// return a MAVLink parameter type given a AP_Param type
static MAV_PARAM_TYPE mav_param_type(enum ap_var_type t);
AP_Param * _queued_parameter; ///< next parameter to
// be sent in queue
mavlink_channel_t chan;
uint8_t packet_overhead(void) const { return packet_overhead_chan(chan); }
// saveable rate of each stream
AP_Int16 *streamRates;
void handle_heartbeat(const mavlink_message_t &msg) const;
virtual bool persist_streamrates() const { return false; }
void handle_request_data_stream(const mavlink_message_t &msg);
virtual void handle_command_ack(const mavlink_message_t &msg);
void handle_set_mode(const mavlink_message_t &msg);
void handle_command_int(const mavlink_message_t &msg);
MAV_RESULT handle_command_do_set_home(const mavlink_command_int_t &packet);
virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command_int_external_position_estimate(const mavlink_command_int_t &packet);
virtual bool set_home_to_current_location(bool lock) = 0;
virtual bool set_home(const Location& loc, bool lock) = 0;
#if AP_ARMING_ENABLED
virtual MAV_RESULT handle_command_component_arm_disarm(const mavlink_command_int_t &packet);
#endif
MAV_RESULT handle_command_do_aux_function(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_storage_format(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
void handle_mission_request_list(const mavlink_message_t &msg);
#if AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED
void handle_mission_request(const mavlink_message_t &msg);
#endif
void handle_mission_request_int(const mavlink_message_t &msg);
void handle_mission_clear_all(const mavlink_message_t &msg);
#if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
// Note that there exists a relatively new mavlink DO command,
// MAV_CMD_DO_SET_MISSION_CURRENT which provides an acknowledgement
// that the command has been received, rather than the GCS having to
// rely on getting back an identical sequence number as some currently
// do.
virtual void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg);
#endif
void handle_mission_count(const mavlink_message_t &msg);
void handle_mission_write_partial_list(const mavlink_message_t &msg);
void handle_mission_item(const mavlink_message_t &msg);
void handle_distance_sensor(const mavlink_message_t &msg);
void handle_obstacle_distance(const mavlink_message_t &msg);
void handle_obstacle_distance_3d(const mavlink_message_t &msg);
void handle_adsb_message(const mavlink_message_t &msg);
void handle_osd_param_config(const mavlink_message_t &msg) const;
void handle_common_param_message(const mavlink_message_t &msg);
void handle_param_set(const mavlink_message_t &msg);
void handle_param_request_list(const mavlink_message_t &msg);
void handle_param_request_read(const mavlink_message_t &msg);
virtual bool params_ready() const { return true; }
void handle_rc_channels_override(const mavlink_message_t &msg);
void handle_system_time_message(const mavlink_message_t &msg);
void handle_common_rally_message(const mavlink_message_t &msg);
void handle_rally_fetch_point(const mavlink_message_t &msg);
void handle_rally_point(const mavlink_message_t &msg) const;
#if HAL_MOUNT_ENABLED
virtual void handle_mount_message(const mavlink_message_t &msg);
#endif
void handle_fence_message(const mavlink_message_t &msg);
void handle_param_value(const mavlink_message_t &msg);
#if HAL_LOGGING_ENABLED
virtual uint32_t log_radio_bit() const { return 0; }
#endif
void handle_radio_status(const mavlink_message_t &msg);
void handle_serial_control(const mavlink_message_t &msg);
void handle_vision_position_delta(const mavlink_message_t &msg);
virtual void handle_message(const mavlink_message_t &msg);
void handle_set_gps_global_origin(const mavlink_message_t &msg);
void handle_setup_signing(const mavlink_message_t &msg) const;
virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
#if AP_MAVLINK_FAILURE_CREATION_ENABLED
struct {
HAL_Semaphore sem;
bool taken;
} _deadlock_sem;
void deadlock_sem(void);
#endif
MAV_RESULT handle_do_set_safety_switch_state(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_do_set_sys_cmp_id(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
// reset a message interval via mavlink:
MAV_RESULT handle_command_set_message_interval(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_get_message_interval(const mavlink_command_int_t &packet);
bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const;
MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet);
MAV_RESULT handle_START_RX_PAIR(const mavlink_command_int_t &packet);
virtual MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet);
#if AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED
void handle_send_autopilot_version(const mavlink_message_t &msg);
#endif
#if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_int_t &packet);
#endif
virtual void send_banner();
// send a (textual) message to the GCS that a received message has
// been deprecated
void send_received_message_deprecation_warning(const char *message);
void handle_device_op_read(const mavlink_message_t &msg);
void handle_device_op_write(const mavlink_message_t &msg);
void send_timesync();
// returns the time a timesync message was most likely received:
uint64_t timesync_receive_timestamp_ns() const;
// returns a timestamp suitable for packing into the ts1 field of TIMESYNC:
uint64_t timesync_timestamp_ns() const;
void handle_timesync(const mavlink_message_t &msg);
struct {
int64_t sent_ts1;
uint32_t last_sent_ms;
const uint16_t interval_ms = 10000;
} _timesync_request;
void handle_statustext(const mavlink_message_t &msg) const;
void handle_named_value(const mavlink_message_t &msg) const;
bool telemetry_delayed() const;
virtual uint32_t telem_delay() const = 0;
MAV_RESULT handle_command_run_prearm_checks(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_flash_bootloader(const mavlink_command_int_t &packet);
// generally this should not be overridden; Plane overrides it to ensure
// failsafe isn't triggered during calibration
virtual MAV_RESULT handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg);
#if AP_MISSION_ENABLED
virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_jump_tag(const mavlink_command_int_t &packet);
#endif
MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet);
void handle_command_long(const mavlink_message_t &msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet);
#if HAL_MOUNT_ENABLED
virtual MAV_RESULT handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
#endif
MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
MAV_RESULT handle_command_do_gripper(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_sprayer(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_set_mode(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_get_home_position(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_fence_enable(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_debug_trap(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_airframe_configuration(const mavlink_command_int_t &packet);
/*
handle MAV_CMD_CAN_FORWARD and CAN_FRAME messages for CAN over MAVLink
*/
void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &);
#if HAL_CANMANAGER_ENABLED
MAV_RESULT handle_can_forward(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
#endif
void handle_can_frame(const mavlink_message_t &msg) const;
void handle_optical_flow(const mavlink_message_t &msg);
void handle_manual_control(const mavlink_message_t &msg);
void handle_radio_rc_channels(const mavlink_message_t &msg);
// default empty handling of LANDING_TARGET
virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { }
// vehicle-overridable message send function
virtual bool try_send_message(enum ap_message id);
virtual void send_global_position_int();
// message sending functions:
bool try_send_mission_message(enum ap_message id);
void send_hwstatus();
void handle_data_packet(const mavlink_message_t &msg);
// these two methods are called after current_loc is updated:
virtual int32_t global_position_int_alt() const;
virtual int32_t global_position_int_relative_alt() const;
virtual float vfr_hud_climbrate() const;
virtual float vfr_hud_airspeed() const;
virtual int16_t vfr_hud_throttle() const { return 0; }
#if AP_AHRS_ENABLED
virtual float vfr_hud_alt() const;
#endif
#if HAL_HIGH_LATENCY2_ENABLED
virtual int16_t high_latency_target_altitude() const { return 0; }
virtual uint8_t high_latency_tgt_heading() const { return 0; }
virtual uint16_t high_latency_tgt_dist() const { return 0; }
virtual uint8_t high_latency_tgt_airspeed() const { return 0; }
virtual uint8_t high_latency_wind_speed() const { return 0; }
virtual uint8_t high_latency_wind_direction() const { return 0; }
int8_t high_latency_air_temperature() const;
MAV_RESULT handle_control_high_latency(const mavlink_command_int_t &packet);
#endif // HAL_HIGH_LATENCY2_ENABLED
static constexpr const float magic_force_arm_value = 2989.0f;
static constexpr const float magic_force_disarm_value = 21196.0f;
void manual_override(class RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false);
uint8_t receiver_rssi() const;
/*
correct an offboard timestamp in microseconds to a local time
since boot in milliseconds
*/
uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size);
#if AP_MAVLINK_COMMAND_LONG_ENABLED
// converts a COMMAND_LONG packet to a COMMAND_INT packet, where
// the command-long packet is assumed to be in the supplied frame.
// If location is not present in the command then just omit frame.
// this method ensures the passed-in structure is entirely
// initialised.
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);
virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const;
MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
#endif
// methods to extract a Location object from a command_int
bool location_from_command_t(const mavlink_command_int_t &in, Location &out);
private:
// define the two objects used for parsing incoming messages:
mavlink_message_t _channel_buffer;
mavlink_status_t _channel_status;
const AP_SerialManager::UARTState *uartstate;
// last time we got a non-zero RSSI from RADIO_STATUS
static struct LastRadioStatus {
uint32_t remrssi_ms;
uint8_t rssi;
uint32_t received_ms; // time RADIO_STATUS received
} last_radio_status;
enum class Flags {
USING_SIGNING = (1<<0),
ACTIVE = (1<<1),
STREAMING = (1<<2),
PRIVATE = (1<<3),
LOCKED = (1<<4),
};
void log_mavlink_stats();
MAV_RESULT _set_mode_common(const MAV_MODE base_mode, const uint32_t custom_mode);
// send a (textual) message to the GCS that a received message has
// been deprecated
uint32_t last_deprecation_warning_send_time_ms;
const char *last_deprecation_message;
void service_statustext(void);
MAV_RESULT handle_servorelay_message(const mavlink_command_int_t &packet);
bool send_relay_status() const;
static bool command_long_stores_location(const MAV_CMD command);
bool calibrate_gyros();
/// The stream we are communicating over
AP_HAL::UARTDriver *_port;
/// Perform queued sending operations
///
enum ap_var_type _queued_parameter_type; ///< type of the next
// parameter
AP_Param::ParamToken _queued_parameter_token; ///AP_Param token for
// next() call
uint16_t _queued_parameter_index; ///< next queued
// parameter's index
uint16_t _queued_parameter_count; ///< saved count of
// parameters for
// queued send
uint32_t _queued_parameter_send_time_ms;
// number of extra ms to add to slow things down for the radio
uint16_t stream_slowdown_ms;
// last reported radio buffer percent available
uint8_t last_txbuf = 100;
// outbound ("deferred message") queue.
// "special" messages such as heartbeat, next_param etc are stored
// separately to stream-rated messages like AHRS2 etc. If these
// were to be stored in buckets then they would be slowed down
// based on stream_slowdown, which we have not traditionally done.
struct deferred_message_t {
const ap_message id;
uint16_t interval_ms;
uint16_t last_sent_ms; // from AP_HAL::millis16()
} deferred_message[3] = {
{ MSG_HEARTBEAT, },
{ MSG_NEXT_PARAM, },
#if HAL_HIGH_LATENCY2_ENABLED
{ MSG_HIGH_LATENCY2, },
#endif
};
// returns index of id in deferred_message[] or -1 if not present
int8_t get_deferred_message_index(const ap_message id) const;
// returns index of a message in deferred_message[] which should
// be sent (or -1 if none to send at the moment)
int8_t deferred_message_to_send_index(uint16_t now16_ms);
// cache of which deferred message should be sent next:
int8_t next_deferred_message_to_send_cache = -1;
struct deferred_message_bucket_t {
Bitmask<MSG_LAST> ap_message_ids;
uint16_t interval_ms;
uint16_t last_sent_ms; // from AP_HAL::millis16()
};
deferred_message_bucket_t deferred_message_bucket[10];
static const uint8_t no_bucket_to_send = -1;
static const ap_message no_message_to_send = (ap_message)-1;
uint8_t sending_bucket_id = no_bucket_to_send;
Bitmask<MSG_LAST> bucket_message_ids_to_send;
ap_message next_deferred_bucket_message_to_send(uint16_t now16_ms);
void find_next_bucket_to_send(uint16_t now16_ms);
void remove_message_from_bucket(int8_t bucket, ap_message id);
// bitmask of IDs the code has spontaneously decided it wants to
// send out. Examples include HEARTBEAT (gcs_send_heartbeat)
Bitmask<MSG_LAST> pushed_ap_message_ids;
// returns true if it is OK to send a message while we are in
// delay callback. In particular, when we are doing sensor init
// we still send heartbeats.
bool should_send_message_in_delay_callback(const ap_message id) const;
// if true is returned, interval will contain the default interval for id
bool get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const;
// if true is returned, interval will contain the default interval for id
// returns an interval in milliseconds for any ap_message in stream id
uint16_t get_interval_for_stream(GCS_MAVLINK::streams id) const;
// set an inverval for a specific mavlink message. Returns false
// on failure (typically because there is no mapping from that
// mavlink ID to an ap_message)
bool set_mavlink_message_id_interval(const uint32_t mavlink_id,
const uint16_t interval_ms);
// map a mavlink ID to an ap_message which, if passed to
// try_send_message, will cause a mavlink message with that id to
// be emitted. Returns MSG_LAST if no such mapping exists.
ap_message mavlink_id_to_ap_message_id(const uint32_t mavlink_id) const;
// set the interval at which an ap_message should be emitted (in ms)
bool set_ap_message_interval(enum ap_message id, uint16_t interval_ms);
// call set_ap_message_interval for each entry in a stream,
// the interval being based on the stream's rate
void initialise_message_intervals_for_stream(GCS_MAVLINK::streams id);
// call initialise_message_intervals_for_stream on every stream:
void initialise_message_intervals_from_streamrates();
// boolean that indicated that message intervals have been set
// from streamrates:
bool deferred_messages_initialised;
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
// read configuration files from (e.g.) SD and ROMFS, set
// intervals from same
void initialise_message_intervals_from_config_files();
// read file, set message intervals from it:
void get_intervals_from_filepath(const char *path, DefaultIntervalsFromFiles &);
#endif
// return interval deferred message bucket should be sent after.
// When sending parameters and waypoints this may be longer than
// the interval specified in "deferred"
uint16_t get_reschedule_interval_ms(const deferred_message_bucket_t &deferred) const;
bool do_try_send_message(const ap_message id);
// time when we missed sending a parameter for GCS
static uint32_t reserve_param_space_start_ms;
// bitmask of what mavlink channels are active
static uint8_t mavlink_active;
// bitmask of what mavlink channels are private
static uint8_t mavlink_private;
// bitmask of what mavlink channels are streaming
static uint8_t chan_is_streaming;
// mavlink routing object
static MAVLink_routing routing;
struct pending_param_request {
mavlink_channel_t chan;
int16_t param_index;
char param_name[AP_MAX_NAME_SIZE+1];
};
struct pending_param_reply {
mavlink_channel_t chan;
float value;
enum ap_var_type p_type;
int16_t param_index;
uint16_t count;
char param_name[AP_MAX_NAME_SIZE+1];
};
// queue of pending parameter requests and replies
static ObjectBuffer<pending_param_request> param_requests;
static ObjectBuffer<pending_param_reply> param_replies;
// have we registered the IO timer callback?
static bool param_timer_registered;
// IO timer callback for parameters
void param_io_timer(void);
uint8_t send_parameter_async_replies();
#if AP_MAVLINK_FTP_ENABLED
enum class FTP_OP : uint8_t {
None = 0,
TerminateSession = 1,
ResetSessions = 2,
ListDirectory = 3,
OpenFileRO = 4,
ReadFile = 5,
CreateFile = 6,
WriteFile = 7,
RemoveFile = 8,
CreateDirectory = 9,
RemoveDirectory = 10,
OpenFileWO = 11,
TruncateFile = 12,
Rename = 13,
CalcFileCRC32 = 14,
BurstReadFile = 15,
Ack = 128,
Nack = 129,
};
enum class FTP_ERROR : uint8_t {
None = 0,
Fail = 1,
FailErrno = 2,
InvalidDataSize = 3,
InvalidSession = 4,
NoSessionsAvailable = 5,
EndOfFile = 6,
UnknownCommand = 7,
FileExists = 8,
FileProtected = 9,
FileNotFound = 10,
};
struct pending_ftp {
uint32_t offset;
mavlink_channel_t chan;
uint16_t seq_number;
FTP_OP opcode;
FTP_OP req_opcode;
bool burst_complete;
uint8_t size;
uint8_t session;
uint8_t sysid;
uint8_t compid;
uint8_t data[239];
};
enum class FTP_FILE_MODE {
Read,