PX4 Firmware
PX4 Autopilot Software http://px4.io
|
ULog streaming class. More...
#include <mavlink_ulog.h>
Public Member Functions | |
void | stop () |
stop the stream. More... | |
int | handle_update (mavlink_channel_t channel) |
periodic update method: check for ulog stream messages and handle retransmission. More... | |
void | handle_ack (mavlink_logging_ack_t ack) |
ack from mavlink for a data message More... | |
void | start_ack_received () |
this is called when we got an vehicle_command_ack from the logger More... | |
float | current_data_rate () const |
float | maximum_data_rate () const |
Static Public Member Functions | |
static void | initialize () |
initialize: call this once on startup (this function is not thread-safe!) More... | |
static MavlinkULog * | try_start (int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) |
try to start a new stream. More... | |
Private Member Functions | |
MavlinkULog (int datarate, float max_rate_factor, uint8_t target_system, uint8_t target_component) | |
~MavlinkULog ()=default | |
void | publish_ack (uint16_t sequence) |
MavlinkULog (const MavlinkULog &)=delete | |
MavlinkULog | operator= (const MavlinkULog &)=delete |
Static Private Member Functions | |
static void | lock () |
static void | unlock () |
Private Attributes | |
uORB::SubscriptionData< ulog_stream_s > | _ulog_stream_sub {ORB_ID(ulog_stream)} |
uORB::Publication< ulog_stream_ack_s > | _ulog_stream_ack_pub {ORB_ID(ulog_stream_ack)} |
uint16_t | _wait_for_ack_sequence |
uint8_t | _sent_tries = 0 |
volatile bool | _ack_received = false |
set to true if a matching ack received More... | |
hrt_abstime | _last_sent_time = 0 |
last time we sent a message that requires an ack More... | |
bool | _waiting_for_initial_ack = false |
const uint8_t | _target_system |
const uint8_t | _target_component |
const float | _max_rate_factor |
maximum rate percentage at which we're allowed to push data More... | |
const int | _max_num_messages |
maximum number of messages we can send within _rate_calculation_delta_t More... | |
float | _current_rate_factor |
currently used rate percentage More... | |
int | _current_num_msgs = 0 |
number of messages sent within the current time interval More... | |
hrt_abstime | _next_rate_check |
next timestamp at which to update the rate More... | |
Static Private Attributes | |
static px4_sem_t | _lock |
static bool | _init = false |
static MavlinkULog * | _instance = nullptr |
static const float | _rate_calculation_delta_t = 0.1f |
rate update interval More... | |
ULog streaming class.
At most one instance (stream) can exist, assigned to a specific mavlink channel.
Definition at line 60 of file mavlink_ulog.h.
|
private |
Definition at line 52 of file mavlink_ulog.cpp.
References _last_sent_time, _next_rate_check, _rate_calculation_delta_t, _ulog_stream_sub, _waiting_for_initial_ack, hrt_absolute_time(), and uORB::SubscriptionData< T >::update().
Referenced by maximum_data_rate(), and try_start().
|
privatedefault |
|
privatedelete |
|
inline |
Definition at line 98 of file mavlink_ulog.h.
References _current_rate_factor.
Referenced by Mavlink::display_status(), and Mavlink::update_rate_mult().
void MavlinkULog::handle_ack | ( | mavlink_logging_ack_t | ack | ) |
ack from mavlink for a data message
Definition at line 233 of file mavlink_ulog.cpp.
References _ack_received, _instance, _wait_for_ack_sequence, lock(), publish_ack(), and unlock().
Referenced by MavlinkReceiver::handle_message_logging_ack().
int MavlinkULog::handle_update | ( | mavlink_channel_t | channel | ) |
periodic update method: check for ulog stream messages and handle retransmission.
Definition at line 78 of file mavlink_ulog.cpp.
References _ack_received, _current_num_msgs, _current_rate_factor, _last_sent_time, _max_num_messages, _max_rate_factor, _next_rate_check, _rate_calculation_delta_t, _sent_tries, _target_component, _target_system, _ulog_stream_sub, _wait_for_ack_sequence, _waiting_for_initial_ack, ulog_stream_s::data, ulog_stream_s::first_message_offset, ulog_stream_s::flags, uORB::SubscriptionData< T >::get(), hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), ulog_stream_s::length, lock(), msg, ulog_stream_s::msg_sequence, ulog_stream_s::timestamp, unlock(), and uORB::SubscriptionData< T >::update().
Referenced by Mavlink::task_main().
|
static |
initialize: call this once on startup (this function is not thread-safe!)
Definition at line 187 of file mavlink_ulog.cpp.
Referenced by Mavlink::start().
|
inlinestaticprivate |
Definition at line 107 of file mavlink_ulog.h.
References _lock.
Referenced by handle_ack(), handle_update(), stop(), and try_start().
|
inline |
Definition at line 99 of file mavlink_ulog.h.
References _max_rate_factor, MavlinkULog(), and ~MavlinkULog().
Referenced by Mavlink::display_status().
|
privatedelete |
|
private |
Definition at line 247 of file mavlink_ulog.cpp.
References _ulog_stream_ack_pub, hrt_absolute_time(), ulog_stream_ack_s::msg_sequence, uORB::Publication< T >::publish(), and ulog_stream_ack_s::timestamp.
Referenced by handle_ack(), and unlock().
void MavlinkULog::start_ack_received | ( | ) |
this is called when we got an vehicle_command_ack from the logger
Definition at line 69 of file mavlink_ulog.cpp.
References _last_sent_time, and _waiting_for_initial_ack.
Referenced by Mavlink::task_main().
void MavlinkULog::stop | ( | ) |
stop the stream.
It also deletes the singleton object, so make sure cleanup all pointers to it. thread-safe
Definition at line 221 of file mavlink_ulog.cpp.
References _instance, lock(), and unlock().
Referenced by Mavlink::task_main().
|
static |
try to start a new stream.
This fails if a stream is already running. thread-safe
datarate | maximum link data rate in B/s |
max_rate_factor | let ulog streaming use a maximum of max_rate_factor * datarate |
target_system | ID for mavlink message |
target_component | ID for mavlink message |
Definition at line 197 of file mavlink_ulog.cpp.
References _instance, lock(), MavlinkULog(), and unlock().
Referenced by Mavlink::try_start_ulog_streaming().
|
inlinestaticprivate |
Definition at line 112 of file mavlink_ulog.h.
References _lock, and publish_ack().
Referenced by handle_ack(), handle_update(), stop(), and try_start().
|
private |
set to true if a matching ack received
Definition at line 128 of file mavlink_ulog.h.
Referenced by handle_ack(), and handle_update().
|
private |
number of messages sent within the current time interval
Definition at line 137 of file mavlink_ulog.h.
Referenced by handle_update().
|
private |
currently used rate percentage
Definition at line 136 of file mavlink_ulog.h.
Referenced by current_data_rate(), and handle_update().
|
staticprivate |
Definition at line 120 of file mavlink_ulog.h.
Referenced by initialize().
|
staticprivate |
Definition at line 121 of file mavlink_ulog.h.
Referenced by handle_ack(), stop(), and try_start().
|
private |
last time we sent a message that requires an ack
Definition at line 129 of file mavlink_ulog.h.
Referenced by handle_update(), MavlinkULog(), and start_ack_received().
|
staticprivate |
Definition at line 119 of file mavlink_ulog.h.
Referenced by initialize(), lock(), and unlock().
|
private |
maximum number of messages we can send within _rate_calculation_delta_t
Definition at line 135 of file mavlink_ulog.h.
Referenced by handle_update().
|
private |
maximum rate percentage at which we're allowed to push data
Definition at line 134 of file mavlink_ulog.h.
Referenced by handle_update(), and maximum_data_rate().
|
private |
next timestamp at which to update the rate
Definition at line 138 of file mavlink_ulog.h.
Referenced by handle_update(), and MavlinkULog().
|
staticprivate |
rate update interval
Definition at line 122 of file mavlink_ulog.h.
Referenced by handle_update(), and MavlinkULog().
|
private |
Definition at line 127 of file mavlink_ulog.h.
Referenced by handle_update().
|
private |
Definition at line 132 of file mavlink_ulog.h.
Referenced by handle_update().
|
private |
Definition at line 131 of file mavlink_ulog.h.
Referenced by handle_update().
|
private |
Definition at line 125 of file mavlink_ulog.h.
Referenced by publish_ack().
|
private |
Definition at line 124 of file mavlink_ulog.h.
Referenced by handle_update(), and MavlinkULog().
|
private |
Definition at line 126 of file mavlink_ulog.h.
Referenced by handle_ack(), and handle_update().
|
private |
Definition at line 130 of file mavlink_ulog.h.
Referenced by handle_update(), MavlinkULog(), and start_ack_received().