<commit_before>#include "global_include.h"
#include "offboard_impl.h"
#include "dronecore_impl.h"
#include "px4_custom_mode.h"

namespace dronecore {

OffboardImpl::OffboardImpl()
{
}

OffboardImpl::~OffboardImpl()
{
}

void OffboardImpl::init()
{
    // We need the system state.
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&OffboardImpl::process_heartbeat, this, std::placeholders::_1),
        this);
}

void OffboardImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

Offboard::Result OffboardImpl::start() const
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode == Mode::NOT_ACTIVE) {
            return Offboard::Result::NO_SETPOINT_SET;
        }
    }

    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
    uint8_t custom_sub_mode = 0;

    return offboard_result_from_command_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

Offboard::Result OffboardImpl::stop() const
{
    {
        //std::lock_guard<std::mutex> lock(_mutex);
        if (_mode != Mode::NOT_ACTIVE && _call_every_cookie != nullptr) {
            _parent->remove_call_every(_call_every_cookie);
        }
    }

    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

    return offboard_result_from_command_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

void OffboardImpl::start_async(Offboard::result_callback_t callback)
{
    {
        //std::lock_guard<std::mutex> lock(_mutex);

        if (_mode == Mode::NOT_ACTIVE) {
            if (callback) {
                callback(Offboard::Result::NO_SETPOINT_SET);
            }<commit_msg>offboard: locks were commented out by accident<commit_after>#include "global_include.h"
#include "offboard_impl.h"
#include "dronecore_impl.h"
#include "px4_custom_mode.h"

namespace dronecore {

OffboardImpl::OffboardImpl()
{
}

OffboardImpl::~OffboardImpl()
{
}

void OffboardImpl::init()
{
    // We need the system state.
    _parent->register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        std::bind(&OffboardImpl::process_heartbeat, this, std::placeholders::_1),
        this);
}

void OffboardImpl::deinit()
{
    _parent->unregister_all_mavlink_message_handlers(this);
}

Offboard::Result OffboardImpl::start() const
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode == Mode::NOT_ACTIVE) {
            return Offboard::Result::NO_SETPOINT_SET;
        }
    }

    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
    uint8_t custom_sub_mode = 0;

    return offboard_result_from_command_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

Offboard::Result OffboardImpl::stop() const
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode != Mode::NOT_ACTIVE && _call_every_cookie != nullptr) {
            _parent->remove_call_every(_call_every_cookie);
        }
    }

    // Note: the safety flag is not needed in future versions of the PX4 Firmware
    //       but want to be rather safe than sorry.
    uint8_t flag_safety_armed = _parent->is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;

    uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed;
    uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
    uint8_t custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;

    return offboard_result_from_command_result(
               _parent->send_command_with_ack(
                   MAV_CMD_DO_SET_MODE,
                   MavlinkCommands::Params {float(mode),
                                            float(custom_mode),
                                            float(custom_sub_mode),
                                            NAN, NAN, NAN, NAN},
                   MavlinkCommands::DEFAULT_COMPONENT_ID_AUTOPILOT));
}

void OffboardImpl::start_async(Offboard::result_callback_t callback)
{
    {
        std::lock_guard<std::mutex> lock(_mutex);

        if (_mode == Mode::NOT_ACTIVE) {
            if (callback) {
                callback(Offboard::Result::NO_SETPOINT_SET);
            }
{'fraction_non_alphanumeric': 0.09162175404213621, 'fraction_numerical': 0.006859382655560999, 'mean_word_length': 2.351943076081007, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 23, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 24, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1061208', 'n_tokens_mistral': 1945, 'n_tokens_neox': 1929, 'n_words': 326}