PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_ulog.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 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_ulog.h
36  * ULog data streaming via MAVLink
37  *
38  * @author Beat Küng <beat-kueng@gmx.net>
39  */
40 
41 #pragma once
42 
43 #include <stddef.h>
44 #include <stdint.h>
45 #include <px4_platform_common/tasks.h>
46 #include <px4_platform_common/sem.h>
47 #include <drivers/drv_hrt.h>
48 
49 #include <uORB/Publication.hpp>
50 #include <uORB/Subscription.hpp>
53 
54 #include "mavlink_bridge_header.h"
55 
56 /**
57  * @class MavlinkULog
58  * ULog streaming class. At most one instance (stream) can exist, assigned to a specific mavlink channel.
59  */
61 {
62 public:
63  /**
64  * initialize: call this once on startup (this function is not thread-safe!)
65  */
66  static void initialize();
67 
68  /**
69  * try to start a new stream. This fails if a stream is already running.
70  * thread-safe
71  * @param datarate maximum link data rate in B/s
72  * @param max_rate_factor let ulog streaming use a maximum of max_rate_factor * datarate
73  * @param target_system ID for mavlink message
74  * @param target_component ID for mavlink message
75  * @return instance, or nullptr
76  */
77  static MavlinkULog *try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component);
78 
79  /**
80  * stop the stream. It also deletes the singleton object, so make sure cleanup
81  * all pointers to it.
82  * thread-safe
83  */
84  void stop();
85 
86  /**
87  * periodic update method: check for ulog stream messages and handle retransmission.
88  * @return 0 on success, <0 otherwise
89  */
90  int handle_update(mavlink_channel_t channel);
91 
92  /** ack from mavlink for a data message */
93  void handle_ack(mavlink_logging_ack_t ack);
94 
95  /** this is called when we got an vehicle_command_ack from the logger */
96  void start_ack_received();
97 
98  float current_data_rate() const { return _current_rate_factor; }
99  float maximum_data_rate() const { return _max_rate_factor; }
100 
101 private:
102 
103  MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component);
104 
105  ~MavlinkULog() = default;
106 
107  static void lock()
108  {
109  do {} while (px4_sem_wait(&_lock) != 0);
110  }
111 
112  static void unlock()
113  {
114  px4_sem_post(&_lock);
115  }
116 
117  void publish_ack(uint16_t sequence);
118 
119  static px4_sem_t _lock;
120  static bool _init;
122  static const float _rate_calculation_delta_t; ///< rate update interval
123 
127  uint8_t _sent_tries = 0;
128  volatile bool _ack_received = false; ///< set to true if a matching ack received
129  hrt_abstime _last_sent_time = 0; ///< last time we sent a message that requires an ack
131  const uint8_t _target_system;
132  const uint8_t _target_component;
133 
134  const float _max_rate_factor; ///< maximum rate percentage at which we're allowed to push data
135  const int _max_num_messages; ///< maximum number of messages we can send within _rate_calculation_delta_t
136  float _current_rate_factor; ///< currently used rate percentage
137  int _current_num_msgs = 0; ///< number of messages sent within the current time interval
138  hrt_abstime _next_rate_check; ///< next timestamp at which to update the rate
139 
140  /* do not allow copying this class */
141  MavlinkULog(const MavlinkULog &) = delete;
142  MavlinkULog operator=(const MavlinkULog &) = delete;
143 };
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58