SendMavlinkMessageState.cc

Go to the documentation of this file.

#include "SendMavlinkMessageState.h"
#include "MAVLinkProtocol.h"
#include "MultiVehicleManager.h"
#include "Vehicle.h"
#include "VehicleLinkManager.h"
#include "QGCLoggingCategory.h"
 
SendMavlinkMessageState::SendMavlinkMessageState(QState *parent, MessageEncoder encoder, int retryCount)
    : QGCState(QStringLiteral("SendMavlinkMessageState"), parent)
    , _encoder(std::move(encoder))
    , _retryCount(retryCount)
{
    connect(this, &QState::entered, this, &SendMavlinkMessageState::_sendMessage);
    // Note: _runCount is NOT reset on exit - it tracks retries across re-entries.
    // The state machine is expected to be one-shot (delete after finished).
}
 
void SendMavlinkMessageState::_sendMessage()
{
    if (++_runCount > _retryCount + 1  /* +1 for initial attempt */) {
        qCDebug(QGCStateMachineLog) << "Exceeded maximum retries" << stateName();
        emit error();
        return;
    }
 
    if (!_encoder) {
        qCDebug(QGCStateMachineLog) << "No MAVLink message encoder configured" << stateName();
        emit error();
        return;
    }
 
    SharedLinkInterfacePtr sharedLink = vehicle()->vehicleLinkManager()->primaryLink().lock();
    if (!sharedLink) {
        qCWarning(QGCStateMachineLog) << "No active link available to send MAVLink message" << stateName();
        emit error();
        return;
    }
 
    mavlink_message_t message{};
 
    const uint8_t systemId = MAVLinkProtocol::instance()->getSystemId();
    const uint8_t componentId [[maybe_unused]] = MAVLinkProtocol::getComponentId();
    const uint8_t channel = sharedLink->mavlinkChannel();
 
    _encoder(systemId, channel, &message);
    if (!vehicle()->sendMessageOnLinkThreadSafe(sharedLink.get(), message)) {
        qCWarning(QGCStateMachineLog) << "Failed to send MAVLink message" << stateName();
        emit error();
        return;
    }
 
    emit advance();
}