dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: create stream if needed and trigger once 

Instead of creating the stream and deleting it again, we now create it
with a rate of 0 and trigger it once.

This should avoid heap fragmentation by continous allocations.
This commit is contained in:
Julian Oes 2020-05-28 12:31:43 +02:00
parent acdc15e2cd
commit 3815e608f4
1 changed files with 11 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -517,8 +517,6 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo
{ {
bool stream_found = false; bool stream_found = false;
uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
for (const auto stream : _mavlink->get_streams()) { for (const auto stream : _mavlink->get_streams()) {
if (stream->get_id() == message_id) { if (stream->get_id() == message_id) {
stream_found = stream->request_message(param2, param3, param4, param5, param6, param7); stream_found = stream->request_message(param2, param3, param4, param5, param6, param7);
@ -527,23 +525,23 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo
} }
if (!stream_found) { if (!stream_found) {
MavlinkStream *stream = create_mavlink_stream(message_id, _mavlink); // If we don't find the stream, we can configure it with rate 0 and then trigger it once.
const char *stream_name = get_stream_name(message_id);
if (stream == nullptr) { if (stream_name != nullptr) {
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; _mavlink->configure_stream_threadsafe(stream_name, 0.0f);
} else { // Now we try again to send it.
bool message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); for (const auto stream : _mavlink->get_streams()) {
if (stream->get_id() == message_id) {
if (!message_sent) { stream_found = stream->request_message(param2, param3, param4, param5, param6, param7);
result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; break;
}
} }
delete stream;
} }
} }
return result; return (stream_found ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
} }