PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_stream.cpp
Go to the documentation of this file.
1
/****************************************************************************
2
*
3
* Copyright (c) 2014-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_stream.cpp
36
* Mavlink messages stream implementation.
37
*
38
* @author Anton Babushkin <anton.babushkin@me.com>
39
* @author Lorenz Meier <lorenz@px4.io>
40
*/
41
42
#include <stdlib.h>
43
44
#include "
mavlink_stream.h
"
45
#include "
mavlink_main.h
"
46
47
MavlinkStream::MavlinkStream
(
Mavlink
*mavlink) :
48
_mavlink(mavlink)
49
{
50
_last_sent
=
hrt_absolute_time
();
51
}
52
53
/**
54
* Update subscriptions and send message if necessary
55
*/
56
int
57
MavlinkStream::update
(
const
hrt_abstime
&t)
58
{
59
update_data
();
60
61
// If the message has never been sent before we want
62
// to send it immediately and can return right away
63
if
(
_last_sent
== 0) {
64
// this will give different messages on the same run a different
65
// initial timestamp which will help spacing them out
66
// on the link scheduling
67
if
(
send
(t)) {
68
_last_sent
=
hrt_absolute_time
();
69
70
if
(!
_first_message_sent
) {
71
_first_message_sent
=
true
;
72
}
73
}
74
75
return
0;
76
}
77
78
// One of the previous iterations sent the update
79
// already before the deadline
80
if
(
_last_sent
> t) {
81
return
-1;
82
}
83
84
int64_t
dt
= t -
_last_sent
;
85
int
interval = (
_interval
> 0) ?
_interval
: 0;
86
87
if
(!
const_rate
()) {
88
interval /=
_mavlink
->
get_rate_mult
();
89
}
90
91
// Send the message if it is due or
92
// if it will overrun the next scheduled send interval
93
// by 30% of the interval time. This helps to avoid
94
// sending a scheduled message on average slower than
95
// scheduled. Doing this at 50% would risk sending
96
// the message too often as the loop runtime of the app
97
// needs to be accounted for as well.
98
// This method is not theoretically optimal but a suitable
99
// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
100
101
if
(interval == 0 || (dt > (interval - (
_mavlink
->
get_main_loop_delay
() / 10) * 3))) {
102
// interval expired, send message
103
104
// If the interval is non-zero and dt is smaller than 1.5 times the interval
105
// do not use the actual time but increment at a fixed rate, so that processing delays do not
106
// distort the average rate. The check of the maximum interval is done to ensure that after a
107
// long time not sending anything, sending multiple messages in a short time is avoided.
108
if
(
send
(t)) {
109
_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
110
111
if
(!
_first_message_sent
) {
112
_first_message_sent
=
true
;
113
}
114
115
return
0;
116
117
}
else
{
118
return
-1;
119
}
120
}
121
122
return
-1;
123
}
MavlinkStream::_first_message_sent
bool _first_message_sent
Definition:
mavlink_stream.h:132
mavlink_main.h
MAVLink 2.0 protocol interface definition.
MavlinkStream::_last_sent
hrt_abstime _last_sent
Definition:
mavlink_stream.h:131
MavlinkStream::_interval
int _interval
if set to negative value = unlimited rate
Definition:
mavlink_stream.h:118
mavlink_stream.h
Mavlink messages stream definition.
Mavlink::get_rate_mult
float get_rate_mult() const
Definition:
mavlink_main.h:415
MavlinkStream::const_rate
virtual bool const_rate()
Definition:
mavlink_stream.h:88
MavlinkStream::update_data
virtual void update_data()
Function to collect/update data for the streams at a high rate independant of actual stream rate...
Definition:
mavlink_stream.h:128
hrt_abstime
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition:
drv_hrt.h:58
MavlinkStream::_mavlink
Mavlink *const _mavlink
Definition:
mavlink_stream.h:117
dt
float dt
Definition:
px4io.c:73
MavlinkStream::update
int update(const hrt_abstime &t)
Update subscriptions and send message if necessary.
Definition:
mavlink_stream.cpp:57
Mavlink
Definition:
mavlink_main.h:105
MavlinkStream::MavlinkStream
MavlinkStream(Mavlink *mavlink)
Definition:
mavlink_stream.cpp:47
Mavlink::get_main_loop_delay
unsigned get_main_loop_delay() const
Definition:
mavlink_main.h:490
hrt_absolute_time
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
MavlinkStream::send
virtual bool send(const hrt_abstime t)=0
src
modules
mavlink
mavlink_stream.cpp
Generated by
1.8.13