<commit_before>#include "system.h"
#include "global_include.h"
#include "dronecore_impl.h"
#include "mavlink_include.h"
#include "mavlink_system.h"
#include "plugin_impl_base.h"
#include <functional>
#include <algorithm>
#include "px4_custom_mode.h"

// Set to 1 to log incoming/outgoing mavlink messages.
#define MESSAGE_DEBUGGING 0

namespace dronecore {

using namespace std::placeholders; // for `_1`

MAVLinkSystem::MAVLinkSystem(DroneCoreImpl &parent,
                             uint8_t system_id, uint8_t comp_id) :
    _system_id(system_id),
    _parent(parent),
    _params(*this),
    _commands(*this),
    _timeout_handler(_time),
    _call_every_handler(_time)
{
    _system_thread = new std::thread(system_thread, this);

    register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&MAVLinkSystem::process_heartbeat, this, _1), this);

    // We're registering for Autopilot version because it is a good time do so,
    // regardless whether we deal with Autopilot.
    register_mavlink_message_handler(
        MAVLINK_MSG_ID_AUTOPILOT_VERSION,
        std::bind(&MAVLinkSystem::process_autopilot_version, this, _1), this);

    register_mavlink_message_handler(
        MAVLINK_MSG_ID_STATUSTEXT,
        std::bind(&MAVLinkSystem::process_statustext, this, _1), this);

    add_new_component(comp_id);
}

MAVLinkSystem::~MAVLinkSystem()
{
    _should_exit = true;
    unregister_all_mavlink_message_handlers(this);

    unregister_timeout_handler(_autopilot_version_timed_out_cookie);
    unregister_timeout_handler(_heartbeat_timeout_cookie);

    if (_system_thread != nullptr) {
        _system_thread->join();
        delete _system_thread;
        _system_thread = nullptr;
    }
}

bool MAVLinkSystem::is_connected() const
{
    return _connected;
}

void MAVLinkSystem::register_mavlink_message_handler(uint16_t msg_id,
                                                     mavlink_message_handler_t callback,
                                                     const void *cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);

    MAVLinkHandlerTableEntry entry = {msg_id, callback, cookie};
    _mavlink_handler_table.push_back(entry);
}

void MAVLinkSystem::unregister_all_mavlink_message_handlers(const void *cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);

    for (auto it = _mavlink_handler_table.begin();
         it != _mavlink_handler_table.end();
         /* no ++it */) {

        if (it->cookie == cookie) {
            it = _mavlink_handler_table.erase(it);
        } else {
            ++it;
        }
    }
    _mavlink_handler_table.clear();
}

void MAVLinkSystem::register_timeout_handler(std::function<void()> callback,
                                             double duration_s,
                                             void **cookie)
{
    _timeout_handler.add(callback, duration_s, cookie);
}

void MAVLinkSystem::refresh_timeout_handler(const void *cookie)
{
    _timeout_handler.refresh(cookie);
}

void MAVLinkSystem::unregister_timeout_handler(const void *cookie)
{
    _timeout_handler.remove(cookie);
}

void MAVLinkSystem::process_mavlink_message(const mavlink_message_t &message)
{
    if (_communication_locked) {
        return;
    }

    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);

#if MESSAGE_DEBUGGING==1
    bool forwarded = false;
#endif
    for (auto it = _mavlink_handler_table.begin(); it != _mavlink_handler_table.end(); ++it) {
        if (it->msg_id == message.msgid) {
#if MESSAGE_DEBUGGING==1
            LogDebug() << "Forwarding msg " << int(message.msgid) << " to " << size_t(it->cookie);
            forwarded = true;
#endif
            it->callback(message);
        }
    }
#if MESSAGE_DEBUGGING==1
    if (!forwarded) {
        LogDebug() << "Ignoring msg " << int(message.msgid);
    }
#endif
}

void MAVLinkSystem::add_call_every(std::function<void()> callback, float interval_s, void **cookie)
{
    _call_every_handler.add(callback, interval_s, cookie);
}

void MAVLinkSystem::change_call_every(float interval_s, const void *cookie)
{
    _call_every_handler.change(interval_s, cookie);
}

void MAVLinkSystem::reset_call_every(const void *cookie)
{
    _call_every_handler.reset(cookie);
}

void MAVLinkSystem::remove_call_every(const void *cookie)
{
    _call_every_handler.remove(cookie);
}

void MAVLinkSystem::process_heartbeat(const mavlink_message_t &message)<commit_msg>core: release locks before calling stuff<commit_after>#include "system.h"
#include "global_include.h"
#include "dronecore_impl.h"
#include "mavlink_include.h"
#include "mavlink_system.h"
#include "plugin_impl_base.h"
#include <functional>
#include <algorithm>
#include "px4_custom_mode.h"

// Set to 1 to log incoming/outgoing mavlink messages.
#define MESSAGE_DEBUGGING 0

namespace dronecore {

using namespace std::placeholders; // for `_1`

MAVLinkSystem::MAVLinkSystem(DroneCoreImpl &parent,
                             uint8_t system_id, uint8_t comp_id) :
    _system_id(system_id),
    _parent(parent),
    _params(*this),
    _commands(*this),
    _timeout_handler(_time),
    _call_every_handler(_time)
{
    _system_thread = new std::thread(system_thread, this);

    register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&MAVLinkSystem::process_heartbeat, this, _1), this);

    // We're registering for Autopilot version because it is a good time do so,
    // regardless whether we deal with Autopilot.
    register_mavlink_message_handler(
        MAVLINK_MSG_ID_AUTOPILOT_VERSION,
        std::bind(&MAVLinkSystem::process_autopilot_version, this, _1), this);

    register_mavlink_message_handler(
        MAVLINK_MSG_ID_STATUSTEXT,
        std::bind(&MAVLinkSystem::process_statustext, this, _1), this);

    add_new_component(comp_id);
}

MAVLinkSystem::~MAVLinkSystem()
{
    _should_exit = true;
    unregister_all_mavlink_message_handlers(this);

    unregister_timeout_handler(_autopilot_version_timed_out_cookie);
    unregister_timeout_handler(_heartbeat_timeout_cookie);

    if (_system_thread != nullptr) {
        _system_thread->join();
        delete _system_thread;
        _system_thread = nullptr;
    }
}

bool MAVLinkSystem::is_connected() const
{
    return _connected;
}

void MAVLinkSystem::register_mavlink_message_handler(uint16_t msg_id,
                                                     mavlink_message_handler_t callback,
                                                     const void *cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);

    MAVLinkHandlerTableEntry entry = {msg_id, callback, cookie};
    _mavlink_handler_table.push_back(entry);
}

void MAVLinkSystem::unregister_all_mavlink_message_handlers(const void *cookie)
{
    std::lock_guard<std::mutex> lock(_mavlink_handler_table_mutex);

    for (auto it = _mavlink_handler_table.begin();
         it != _mavlink_handler_table.end();
         /* no ++it */) {

        if (it->cookie == cookie) {
            it = _mavlink_handler_table.erase(it);
        } else {
            ++it;
        }
    }
    _mavlink_handler_table.clear();
}

void MAVLinkSystem::register_timeout_handler(std::function<void()> callback,
                                             double duration_s,
                                             void **cookie)
{
    _timeout_handler.add(callback, duration_s, cookie);
}

void MAVLinkSystem::refresh_timeout_handler(const void *cookie)
{
    _timeout_handler.refresh(cookie);
}

void MAVLinkSystem::unregister_timeout_handler(const void *cookie)
{
    _timeout_handler.remove(cookie);
}

void MAVLinkSystem::process_mavlink_message(const mavlink_message_t &message)
{
    if (_communication_locked) {
        return;
    }

    _mavlink_handler_table_mutex.lock();

#if MESSAGE_DEBUGGING==1
    bool forwarded = false;
#endif
    for (auto it = _mavlink_handler_table.begin(); it != _mavlink_handler_table.end(); ++it) {
        if (it->msg_id == message.msgid) {
#if MESSAGE_DEBUGGING==1
            LogDebug() << "Forwarding msg " << int(message.msgid) << " to " << size_t(it->cookie);
            forwarded = true;
#endif
            _mavlink_handler_table_mutex.unlock();
            it->callback(message);
            _mavlink_handler_table_mutex.lock();
        }
    }
    _mavlink_handler_table_mutex.unlock();

#if MESSAGE_DEBUGGING==1
    if (!forwarded) {
        LogDebug() << "Ignoring msg " << int(message.msgid);
    }
#endif
}

void MAVLinkSystem::add_call_every(std::function<void()> callback, float interval_s, void **cookie)
{
    _call_every_handler.add(callback, interval_s, cookie);
}

void MAVLinkSystem::change_call_every(float interval_s, const void *cookie)
{
    _call_every_handler.change(interval_s, cookie);
}

void MAVLinkSystem::reset_call_every(const void *cookie)
{
    _call_every_handler.reset(cookie);
}

void MAVLinkSystem::remove_call_every(const void *cookie)
{
    _call_every_handler.remove(cookie);
}

void MAVLinkSystem::process_heartbeat(const mavlink_message_t &message)
{'fraction_non_alphanumeric': 0.10585387323943662, 'fraction_numerical': 0.0030809859154929575, 'mean_word_length': 3.5106699751861044, 'pattern_counts': {'":': 0, '<': 40, '<?xml version=': 0, '>': 26, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 16, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2190465', 'n_tokens_mistral': 2972, 'n_tokens_neox': 2934, 'n_words': 496}