MockLinkGimbal.h

Go to the documentation of this file.

#pragma once
 
#include "MAVLinkLib.h"
 
#include <QtCore/QList>
#include <QtCore/QMutex>
 
#include <cmath>
 
class MockLink;
 
class MockLinkGimbal
{
public:
    enum class DeltaYawMode {
        Legacy,     
        Unknown,    
        Explicit,   
    };
 
    struct PitchYawCommand {
        float pitchDeg = NAN;
        float yawDeg = NAN;
        float pitchRateDegS = NAN;
        float yawRateDegS = NAN;
        uint32_t flags = 0;
        uint8_t deviceId = 0;
        int count = 0;
    };
 
    struct SetAttitudeCommand {
        float pitchRateDegS = NAN;
        float yawRateDegS = NAN;
        uint32_t flags = 0;
        int count = 0;
    };
 
    enum class InformationResponse {
        Accept,     
        Nack,       
        Silent,     
    };
 
    explicit MockLinkGimbal(
        MockLink *mockLink,
        bool hasRollAxis = true,
        bool hasPitchAxis = true,
        bool hasYawAxis = true,
        bool hasYawFollow = true,
        bool hasYawLock = true,
        bool hasRetract = true,
        bool hasNeutral = true,
        uint8_t gimbalDeviceId = MAV_COMP_ID_GIMBAL);
    ~MockLinkGimbal() = default;
 
    void run1HzTasks();
 
    bool handleMavlinkMessage(const mavlink_message_t &msg);
 
    // Test APIs. All are safe to call from the test thread while the worker runs 1Hz tasks.
 
    void setAttitudeDeg(float rollDeg, float pitchDeg, float yawDeg);
    void setDeltaYawMode(DeltaYawMode mode);
    void setDeltaYawDeg(float deltaYawDeg);
    void setYawInEarthFrame(bool earthFrame);
    void setPrimaryControl(uint8_t sysid, uint8_t compid);
    void sendGimbalDeviceAttitudeStatusNow();
    void sendGimbalManagerStatusNow();
 
    void sendGimbalManagerInformationWithDeviceId(uint8_t deviceIdField);
    void sendGimbalManagerStatusWithDeviceId(uint8_t deviceIdField);
    void sendGimbalDeviceAttitudeStatusFrom(uint8_t sourceCompid, uint8_t deviceIdField);
 
    void setInformationResponse(InformationResponse response);
    void setIgnoreStatusIntervalRequests(int count);
    QList<int> statusIntervalRequests() const;
 
    PitchYawCommand lastPitchYawCommand() const;
    SetAttitudeCommand lastSetAttitudeCommand() const;
    bool yawLock() const;
    uint8_t gimbalDeviceId() const { return _gimbalDeviceId; }
 
private:
    bool _handleSetMessageInterval(const mavlink_command_long_t &request);
 
    bool _handleRequestMessage(const mavlink_command_long_t &request);
 
    bool _handleGimbalManagerPitchYaw(const mavlink_command_long_t &request);
 
    bool _handleGimbalManagerConfigure(const mavlink_command_long_t &request, uint8_t senderSysid, uint8_t senderCompid);
 
    void _handleGimbalManagerSetAttitude(const mavlink_message_t &msg);
 
    void _sendGimbalManagerStatus();
    void _sendGimbalManagerStatus(uint8_t deviceIdField);
    void _sendGimbalDeviceAttitudeStatus();
    void _sendGimbalDeviceAttitudeStatus(uint8_t sourceCompid, uint8_t deviceIdField);
    void _sendGimbalManagerInformation();
    void _sendGimbalManagerInformation(uint8_t deviceIdField);
    void _sendGimbalDeviceInformation();
    void _sendCommandAck(uint16_t command, uint8_t result, uint8_t sourceCompId);
 
    static void _applyControlPair(float sysidValue, float compidValue, uint8_t senderSysid, uint8_t senderCompid,
                                  uint8_t &targetSysid, uint8_t &targetCompid);
    static void _applyControlField(float value, uint8_t senderValue, uint8_t &target);
 
    uint8_t _attitudeSourceCompid() const { return (_gimbalDeviceId <= 6) ? static_cast<uint8_t>(MAV_COMP_ID_AUTOPILOT1) : _gimbalDeviceId; }
    uint8_t _attitudeDeviceIdField() const { return (_gimbalDeviceId <= 6) ? _gimbalDeviceId : 0; }
 
    static constexpr int kDefaultIntervalUs = 1000000; // 1 Hz default
    static constexpr const char* kVendorName = "MockVendor";
    static constexpr const char* kModelName = "MockGimbal";
 
    MockLink *_mockLink = nullptr;
    const uint8_t _gimbalDeviceId = MAV_COMP_ID_GIMBAL;
    InformationResponse _informationResponse = InformationResponse::Accept;
    int       _ignoreStatusIntervalRequests = 0;
    QList<int> _statusIntervalRequests;
    int       _managerStatusIntervalUs = 0;          // 0 = disabled
    int       _deviceAttitudeStatusIntervalUs = 0;  // 0 = disabled by default
    qint64    _managerStatusLastSentMs = 0;
    qint64    _deviceAttitudeStatusLastSentMs = 0;
 
    // Simulated gimbal attitude (degrees)
    float     _roll = 0.0f;
    float     _pitch = 0.0f;
    float     _yaw = 0.0f;
    bool      _manualControl = false;  // true when under external control command
    bool      _yawLock = false;
    bool      _yawInEarthFrame = false;
    DeltaYawMode _deltaYawMode = DeltaYawMode::Unknown;
    float     _deltaYawDeg = 0.0f;
    PitchYawCommand _lastPitchYawCommand;
    SetAttitudeCommand _lastSetAttitudeCommand;
    mutable QMutex _stateMutex;
 
    // Gimbal manager configuration
    uint8_t   _gimbalManagerSysidPrimary = 0;    // System ID for primary control
    uint8_t   _gimbalManagerCompidPrimary = 0;   // Component ID for primary control
    uint8_t   _gimbalManagerSysidSecondary = 0;  // System ID for secondary control
    uint8_t   _gimbalManagerCompidSecondary = 0; // Component ID for secondary control
 
    // Gimbal capability flags
    bool _hasRollAxis = true;
    bool _hasPitchAxis = true;
    bool _hasYawAxis = true;
    bool _hasYawFollow = true;
    bool _hasYawLock = true;
    bool _hasRetract = true;
    bool _hasNeutral = true;
};