PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_command_sender.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_command_sender.h
36  * Mavlink commands sender with support for retransmission.
37  *
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #pragma once
42 
43 #include <px4_platform_common/tasks.h>
44 #include <px4_platform_common/sem.h>
45 #include <drivers/drv_hrt.h>
46 
49 
50 #include "timestamped_list.h"
51 #include "mavlink_bridge_header.h"
52 #include <v2.0/mavlink_types.h>
53 
54 /**
55  * @class MavlinkCommandSender
56  */
58 {
59 public:
60  /**
61  * initialize: call this once on startup (this function is not thread-safe!)
62  */
63  static void initialize();
64 
66 
67  /**
68  * Send a command on a channel and keep it in a queue for retransmission.
69  * thread-safe
70  * @return 0 on success, <0 otherwise
71  */
72  int handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel);
73 
74  /**
75  * Check timeouts to verify if an commands need retransmission.
76  * thread-safe
77  */
78  void check_timeout(mavlink_channel_t channel);
79 
80  /**
81  * Handle mavlink command_ack.
82  * thread-safe
83  */
84  void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid,
85  uint8_t channel);
86 
87 private:
88  MavlinkCommandSender() = default;
89 
91 
92  static void lock()
93  {
94  do {} while (px4_sem_wait(&_lock) != 0);
95  }
96 
97  static void unlock()
98  {
99  px4_sem_post(&_lock);
100  }
101 
103  static px4_sem_t _lock;
104 
105  // There are MAVLINK_COMM_0 to MAVLINK_COMM_3, so it should be 4.
106  static const unsigned MAX_MAVLINK_CHANNEL = 4;
107 
108  typedef struct {
109  mavlink_command_long_t command = {};
110  hrt_abstime timestamp_us = 0;
111  hrt_abstime last_time_sent_us = 0;
112  int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command
113  } command_item_t;
114 
116 
117  bool _debug_enabled = false;
118  static constexpr uint8_t RETRIES = 3;
119  static constexpr uint64_t TIMEOUT_US = 500000;
120 
121  /* do not allow copying or assigning this class */
122  MavlinkCommandSender(const MavlinkCommandSender &) = delete;
124 };
High-resolution timer with callouts and timekeeping.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58