42 #include <px4_platform_common/log.h> 44 #include <mathlib/mathlib.h> 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)
74 PX4_DEBUG(
"got logger ack");
81 "Invalid uorb ulog_stream.data length");
83 "Invalid uorb ulog_stream.data length");
87 PX4_WARN(
"no ack from logger (is it running?)");
96 bool check_for_updates =
false;
100 check_for_updates =
true;
105 if (++
_sent_tries > ulog_stream_ack_s::ACK_MAX_TRIES) {
109 PX4_DEBUG(
"re-sending ulog mavlink message (try=%i)",
_sent_tries);
114 mavlink_logging_data_acked_t
msg;
116 msg.length = ulog_data.
length;
120 memcpy(msg.data, ulog_data.
data,
sizeof(msg.data));
121 mavlink_msg_logging_data_acked_send_struct(channel, &msg);
126 if (!check_for_updates) {
135 if (ulog_data.
flags & ulog_stream_s::FLAGS_NEED_ACK) {
143 mavlink_logging_data_acked_t
msg;
145 msg.length = ulog_data.
length;
149 memcpy(msg.data, ulog_data.
data,
sizeof(msg.data));
150 mavlink_msg_logging_data_acked_send_struct(channel, &msg);
153 mavlink_logging_data_t
msg;
155 msg.length = ulog_data.
length;
159 memcpy(msg.data, ulog_data.
data,
sizeof(msg.data));
160 mavlink_msg_logging_data_send_struct(channel, &msg);
193 px4_sem_init(&
_lock, 1, 1);
198 uint8_t target_component)
215 PX4_ERR(
"alloc failed");
void stop()
stop the stream.
static const float _rate_calculation_delta_t
rate update interval
const uint8_t _target_system
int _current_num_msgs
number of messages sent within the current time interval
void handle_ack(mavlink_logging_ack_t ack)
ack from mavlink for a data message
void publish_ack(uint16_t sequence)
void start_ack_received()
this is called when we got an vehicle_command_ack from the logger
uORB::Publication< ulog_stream_ack_s > _ulog_stream_ack_pub
float _current_rate_factor
currently used rate percentage
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
int handle_update(mavlink_channel_t channel)
periodic update method: check for ulog stream messages and handle retransmission. ...
static MavlinkULog * try_start(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
try to start a new stream.
bool publish(const T &data)
Publish the struct.
volatile bool _ack_received
set to true if a matching ack received
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
static MavlinkULog * _instance
hrt_abstime _last_sent_time
last time we sent a message that requires an ack
bool _waiting_for_initial_ack
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
const uint8_t _target_component
const int _max_num_messages
maximum number of messages we can send within _rate_calculation_delta_t
MavlinkULog(int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component)
constexpr _Tp max(_Tp a, _Tp b)
ULog data streaming via MAVLink.
uint8_t first_message_offset
static void initialize()
initialize: call this once on startup (this function is not thread-safe!)
const float _max_rate_factor
maximum rate percentage at which we're allowed to push data
hrt_abstime _next_rate_check
next timestamp at which to update the rate
uORB::SubscriptionData< ulog_stream_s > _ulog_stream_sub
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint16_t _wait_for_ack_sequence