PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_ulog.cpp
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.cpp
36  * ULog data streaming via MAVLink
37  *
38  * @author Beat Küng <beat-kueng@gmx.net>
39  */
40 
41 #include "mavlink_ulog.h"
42 #include <px4_platform_common/log.h>
43 #include <errno.h>
44 #include <mathlib/mathlib.h>
45 
46 bool MavlinkULog::_init = false;
48 px4_sem_t MavlinkULog::_lock;
50 
51 
52 MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
53  : _target_system(target_system), _target_component(target_component),
54  _max_rate_factor(max_rate_factor),
55  _max_num_messages(math::max(1, (int)ceilf(_rate_calculation_delta_t *_max_rate_factor * datarate /
56  (MAVLINK_MSG_ID_LOGGING_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)))),
57  _current_rate_factor(max_rate_factor)
58 {
59  // make sure we won't read any old messages
60  while (_ulog_stream_sub.update()) {
61 
62  }
63 
65  _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization
67 }
68 
70 {
72  _last_sent_time = 0;
74  PX4_DEBUG("got logger ack");
75  }
76 }
77 
78 int MavlinkULog::handle_update(mavlink_channel_t channel)
79 {
80  static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN,
81  "Invalid uorb ulog_stream.data length");
82  static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN,
83  "Invalid uorb ulog_stream.data length");
84 
86  if (hrt_elapsed_time(&_last_sent_time) > 3e5) {
87  PX4_WARN("no ack from logger (is it running?)");
88  return -1;
89  }
90 
91  return 0;
92  }
93 
94  // check if we're waiting for an ACK
95  if (_last_sent_time) {
96  bool check_for_updates = false;
97 
98  if (_ack_received) {
99  _last_sent_time = 0;
100  check_for_updates = true;
101 
102  } else {
103 
104  if (hrt_elapsed_time(&_last_sent_time) > ulog_stream_ack_s::ACK_TIMEOUT * 1000) {
105  if (++_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) {
106  return -ETIMEDOUT;
107 
108  } else {
109  PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries);
111 
112  const ulog_stream_s &ulog_data = _ulog_stream_sub.get();
113 
114  mavlink_logging_data_acked_t msg;
115  msg.sequence = ulog_data.msg_sequence;
116  msg.length = ulog_data.length;
117  msg.first_message_offset = ulog_data.first_message_offset;
118  msg.target_system = _target_system;
119  msg.target_component = _target_component;
120  memcpy(msg.data, ulog_data.data, sizeof(msg.data));
121  mavlink_msg_logging_data_acked_send_struct(channel, &msg);
122  }
123  }
124  }
125 
126  if (!check_for_updates) {
127  return 0;
128  }
129  }
130 
132  const ulog_stream_s &ulog_data = _ulog_stream_sub.get();
133 
134  if (ulog_data.timestamp > 0) {
135  if (ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) {
136  _sent_tries = 1;
138  lock();
140  _ack_received = false;
141  unlock();
142 
143  mavlink_logging_data_acked_t msg;
144  msg.sequence = ulog_data.msg_sequence;
145  msg.length = ulog_data.length;
146  msg.first_message_offset = ulog_data.first_message_offset;
147  msg.target_system = _target_system;
148  msg.target_component = _target_component;
149  memcpy(msg.data, ulog_data.data, sizeof(msg.data));
150  mavlink_msg_logging_data_acked_send_struct(channel, &msg);
151 
152  } else {
153  mavlink_logging_data_t msg;
154  msg.sequence = ulog_data.msg_sequence;
155  msg.length = ulog_data.length;
156  msg.first_message_offset = ulog_data.first_message_offset;
157  msg.target_system = _target_system;
158  msg.target_component = _target_component;
159  memcpy(msg.data, ulog_data.data, sizeof(msg.data));
160  mavlink_msg_logging_data_send_struct(channel, &msg);
161  }
162  }
163 
165  }
166 
167  //need to update the rate?
169 
170  if (t > _next_rate_check) {
173 
174  } else {
176  }
177 
178  _current_num_msgs = 0;
180  PX4_DEBUG("current rate=%.3f (max=%i msgs in %.3fs)", (double)_current_rate_factor, _max_num_messages,
181  (double)_rate_calculation_delta_t);
182  }
183 
184  return 0;
185 }
186 
188 {
189  if (_init) {
190  return;
191  }
192 
193  px4_sem_init(&_lock, 1, 1);
194  _init = true;
195 }
196 
197 MavlinkULog *MavlinkULog::try_start(int datarate, float max_rate_factor, uint8_t target_system,
198  uint8_t target_component)
199 {
200  MavlinkULog *ret = nullptr;
201  bool failed = false;
202  lock();
203 
204  if (!_instance) {
205  ret = _instance = new MavlinkULog(datarate, max_rate_factor, target_system, target_component);
206 
207  if (!_instance) {
208  failed = true;
209  }
210  }
211 
212  unlock();
213 
214  if (failed) {
215  PX4_ERR("alloc failed");
216  }
217 
218  return ret;
219 }
220 
222 {
223  lock();
224 
225  if (_instance) {
226  delete _instance;
227  _instance = nullptr;
228  }
229 
230  unlock();
231 }
232 
233 void MavlinkULog::handle_ack(mavlink_logging_ack_t ack)
234 {
235  lock();
236 
237  if (_instance) { // make sure stop() was not called right before
238  if (_wait_for_ack_sequence == ack.sequence) {
239  _ack_received = true;
240  publish_ack(ack.sequence);
241  }
242  }
243 
244  unlock();
245 }
246 
247 void MavlinkULog::publish_ack(uint16_t sequence)
248 {
249  ulog_stream_ack_s ack;
251  ack.msg_sequence = sequence;
252 
254 }
uint8_t flags
Definition: ulog_stream.h:59
uint8_t data[249]
Definition: ulog_stream.h:60
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
uint8_t length
Definition: ulog_stream.h:57
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
const T & get() const
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
uint64_t timestamp
Definition: ulog_stream.h:55
uint8_t first_message_offset
Definition: ulog_stream.h:58
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint16_t msg_sequence
Definition: ulog_stream.h:56