Skip to content
Vehicle.h 75.8 KiB
Newer Older
    void setOfflineEditingDefaultComponentId(int defaultComponentId);
Don Gagne's avatar
Don Gagne committed
    /// @return -1 = Unknown, Number of motors on vehicle
    int motorCount(void);

    /// @return true: Motors are coaxial like an X8 config, false: Quadcopter for example
    bool coaxialMotors(void);

    /// @return true: X confiuration, false: Plus configuration
    bool xConfigMotors(void);

    /// @return Firmware plugin instance data associated with this Vehicle
    QObject* firmwarePluginInstanceData(void) { return _firmwarePluginInstanceData; }

    /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
    /// and destroyed when the vehicle goes away.
    void setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData);

    QString vehicleImageOpaque  () const;
    QString vehicleImageOutline () const;
    QString vehicleImageCompass () const;

    const QVariantList&         toolBarIndicators   ();
Gus Grubba's avatar
Gus Grubba committed
    const QVariantList&         staticCameraList    (void) const;
    bool capabilitiesKnown      (void) const { return _vehicleCapabilitiesKnown; }
    uint64_t capabilityBits     (void) const { return _capabilityBits; }    // Change signalled by capabilityBitsChanged
    QGCCameraManager*           dynamicCameras      () { return _cameras; }
    QString                     hobbsMeter          ();
    /// @true: When flying a mission the vehicle is always facing towards the next waypoint
    bool vehicleYawsToNextWaypointInMission(void) const;

DonLakeFlyer's avatar
DonLakeFlyer committed
    /// The vehicle is responsible for making the initial request for the Plan.
    /// @return: true: initial request is complete, false: initial request is still in progress;
    bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; }

Don Gagne's avatar
Don Gagne committed
    void forceInitialPlanRequestComplete(void);
    void _setFlying(bool flying);
    void _setLanding(bool landing);
    void setVtolInFwdFlight(bool vtolInFwdFlight);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _setHomePosition(QGeoCoordinate& homeCoord);
    void _setMaxProtoVersion (unsigned version);
    /// Vehicle is about to be deleted
    void prepareDelete();

Don Gagne's avatar
Don Gagne committed
    void allLinksInactive(Vehicle* vehicle);
    void coordinateChanged(QGeoCoordinate coordinate);
    void joystickModeChanged(int mode);
    void joystickEnabledChanged(bool enabled);
    void activeChanged(bool active);
Don Gagne's avatar
Don Gagne committed
    void mavlinkMessageReceived(const mavlink_message_t& message);
    void homePositionChanged(const QGeoCoordinate& homePosition);
Don Gagne's avatar
Don Gagne committed
    void armedChanged(bool armed);
    void flightModeChanged(const QString& flightMode);
    void hilModeChanged(bool hilMode);
    /** @brief HIL actuator controls (replaces HIL controls) */
    void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode);
    void connectionLostChanged(bool connectionLost);
    void connectionLostEnabledChanged(bool connectionLostEnabled);
Don Gagne's avatar
Don Gagne committed
    void autoDisconnectChanged(bool autoDisconnectChanged);
Don Gagne's avatar
Don Gagne committed
    void flyingChanged(bool flying);
    void landingChanged(bool landing);
Don Gagne's avatar
Don Gagne committed
    void guidedModeChanged(bool guidedMode);
    void vtolInFwdFlightChanged(bool vtolInFwdFlight);
Don Gagne's avatar
Don Gagne committed
    void prearmErrorChanged(const QString& prearmError);
    void soloFirmwareChanged(bool soloFirmware);
    void unhealthySensorsChanged(void);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void defaultCruiseSpeedChanged(double cruiseSpeed);
    void defaultHoverSpeedChanged(double hoverSpeed);
    void firmwareTypeChanged(void);
    void vehicleTypeChanged(void);
    void dynamicCamerasChanged();
    void hobbsMeterChanged();
    void capabilitiesKnownChanged(bool capabilitiesKnown);
Don Gagne's avatar
Don Gagne committed
    void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete);
    void capabilityBitsChanged(uint64_t capabilityBits);
    void toolBarIndicatorsChanged(void);
Don Gagne's avatar
Don Gagne committed
    void highLatencyLinkChanged(bool highLatencyLink);
    void priorityLinkNameChanged(const QString& priorityLinkName);
    void linksChanged(void);
    void linksPropertiesChanged(void);
    void messagesReceivedChanged    ();
    void messagesSentChanged        ();
    void messagesLostChanged        ();

    /// Used internally to move sendMessage call to main thread
    void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message);
Gus Grubba's avatar
Gus Grubba committed
    void messageTypeChanged         ();
    void newMessageCountChanged     ();
    void messageCountChanged        ();
    void formatedMessagesChanged    ();
    void formatedMessageChanged     ();
    void latestErrorChanged         ();
    void longitudeChanged           ();
    void currentConfigChanged       ();
    void flowImageIndexChanged      ();
    void rcRSSIChanged              (int rcRSSI);
    void telemetryRRSSIChanged      (int value);
    void telemetryLRSSIChanged      (int value);
    void telemetryRXErrorsChanged   (unsigned int value);
    void telemetryFixedChanged      (unsigned int value);
    void telemetryTXBufferChanged   (unsigned int value);
    void telemetryLNoiseChanged     (int value);
    void telemetryRNoiseChanged     (int value);
    void autoDisarmChanged          (void);
    void flightModesChanged         (void);
    void sensorsPresentBitsChanged  (int sensorsPresentBits);
    void sensorsEnabledBitsChanged  (int sensorsEnabledBits);
    void sensorsHealthBitsChanged   (int sensorsHealthBits);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void sensorsUnhealthyBitsChanged(int sensorsUnhealthyBits);
    void firmwareVersionChanged(void);
    void firmwareCustomVersionChanged(void);
    void gitHashChanged(QString hash);
Gus Grubba's avatar
Gus Grubba committed
    void vehicleUIDChanged();
Don Gagne's avatar
Don Gagne committed
    /// New RC channel values
    ///     @param channelCount Number of available channels, cMaxRcChannels max
    ///     @param pwmValues -1 signals channel not available
    void rcChannelsChanged(int channelCount, int pwmValues[cMaxRcChannels]);

    /// Remote control RSSI changed  (0% - 100%)
    void remoteControlRSSIChanged(uint8_t rssi);

Don Gagne's avatar
Don Gagne committed
    void mavlinkRawImu(mavlink_message_t message);
    void mavlinkScaledImu1(mavlink_message_t message);
    void mavlinkScaledImu2(mavlink_message_t message);
    void mavlinkScaledImu3(mavlink_message_t message);

Gus Grubba's avatar
Gus Grubba committed
    void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
    /// Signalled in response to usage of sendMavCommand
    ///     @param vehicleId Vehicle which command was sent to
    ///     @param component Component which command was sent to
    ///     @param command MAV_CMD Command which was sent
    ///     @param result MAV_RESULT returned in ack
    ///     @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result
    void mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
    // MAVlink Serial Data
    void mavlinkSerialControl(uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);

    // MAVLink protocol version
    void requestProtocolVersion(unsigned version);

private slots:
    void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
Don Gagne's avatar
Don Gagne committed
    void _linkInactiveOrDeleted(LinkInterface* link);
    void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message);
    void _sendMessageMultipleNext(void);
    void _addNewMapTrajectoryPoint(void);
    void _parametersReady(bool parametersReady);
Don Gagne's avatar
Don Gagne committed
    void _remoteControlRSSIChanged(uint8_t rssi);
Don Gagne's avatar
Don Gagne committed
    void _handleFlightModeChanged(const QString& flightMode);
    void _announceArmedChanged(bool armed);
    void _offlineFirmwareTypeSettingChanged(QVariant value);
    void _offlineVehicleTypeSettingChanged(QVariant value);
    void _offlineCruiseSpeedSettingChanged(QVariant value);
    void _offlineHoverSpeedSettingChanged(QVariant value);
    void _updateHighLatencyLink(bool sendCommand = true);
    void _handleTextMessage                 (int newCount);
dogmaphobic's avatar
dogmaphobic committed
    void _handletextMessageReceived         (UASMessage* message);
dogmaphobic's avatar
dogmaphobic committed
    /** @brief A new camera image has arrived */
    void _imageReady                        (UASInterface* uas);
Don Gagne's avatar
Don Gagne committed
    void _prearmErrorTimeout(void);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _missionLoadComplete(void);
    void _geoFenceLoadComplete(void);
    void _rallyPointLoadComplete(void);
    void _sendMavCommandAgain(void);
    void _clearTrajectoryPoints(void);
    void _clearCameraTriggerPoints(void);
    void _updateDistanceToHome(void);
    void _updateHobbsMeter(void);
    void _vehicleParamLoaded(bool ready);
    void _sendQGCTimeToVehicle(void);
Gus Grubba's avatar
Gus Grubba committed
    void _trafficUpdate         (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
    void _adsbTimerTimeout      ();
private:
    bool _containsLink(LinkInterface* link);
    void _addLink(LinkInterface* link);
    void _loadSettings(void);
    void _saveSettings(void);
    void _startJoystick(bool start);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _handlePing(LinkInterface* link, mavlink_message_t& message);
Don Gagne's avatar
Don Gagne committed
    void _handleHomePosition(mavlink_message_t& message);
    void _handleHeartbeat(mavlink_message_t& message);
    void _handleRadioStatus(mavlink_message_t& message);
Don Gagne's avatar
Don Gagne committed
    void _handleRCChannels(mavlink_message_t& message);
    void _handleRCChannelsRaw(mavlink_message_t& message);
Don Gagne's avatar
Don Gagne committed
    void _handleBatteryStatus(mavlink_message_t& message);
    void _handleSysStatus(mavlink_message_t& message);
    void _handleWindCov(mavlink_message_t& message);
    void _handleVibration(mavlink_message_t& message);
Don Gagne's avatar
Don Gagne committed
    void _handleExtendedSysState(mavlink_message_t& message);
    void _handleCommandAck(mavlink_message_t& message);
    void _handleCommandLong(mavlink_message_t& message);
Gus Grubba's avatar
Gus Grubba committed
    void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message);
    void _handleProtocolVersion(LinkInterface* link, mavlink_message_t& message);
    void _handleHilActuatorControls(mavlink_message_t& message);
    void _handleGpsRawInt(mavlink_message_t& message);
    void _handleGlobalPositionInt(mavlink_message_t& message);
    void _handleAltitude(mavlink_message_t& message);
    void _handleVfrHud(mavlink_message_t& message);
    void _handleScaledPressure(mavlink_message_t& message);
    void _handleScaledPressure2(mavlink_message_t& message);
    void _handleScaledPressure3(mavlink_message_t& message);
    void _handleHighLatency2(mavlink_message_t& message);
    void _handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians);
    void _handleAttitude(mavlink_message_t& message);
    void _handleAttitudeQuaternion(mavlink_message_t& message);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _handleAttitudeTarget(mavlink_message_t& message);
    void _handleDistanceSensor(mavlink_message_t& message);
    void _handleEstimatorStatus(mavlink_message_t& message);
    // ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT)
    void _handleCameraFeedback(const mavlink_message_t& message);
    void _handleCameraImageCaptured(const mavlink_message_t& message);
    void _handleADSBVehicle(const mavlink_message_t& message);
    void _missionManagerError(int errorCode, const QString& errorMsg);
    void _geoFenceManagerError(int errorCode, const QString& errorMsg);
    void _rallyPointManagerError(int errorCode, const QString& errorMsg);
    void _mapTrajectoryStart(void);
    void _mapTrajectoryStop(void);
    void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID);
    void _say(const QString& text);
    QString _vehicleIdSpeech(void);
    void _handleMavlinkLoggingData(mavlink_message_t& message);
    void _handleMavlinkLoggingDataAcked(mavlink_message_t& message);
    void _ackMavlinkLogData(uint16_t sequence);
    void _sendNextQueuedMavCommand(void);
    void _updatePriorityLink(bool updateActive, bool sendCommand);
    void _commonInit(void);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _startPlanRequest(void);
    void _setupAutoDisarmSignalling(void);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _setCapabilities(uint64_t capabilityBits);
DonLakeFlyer's avatar
DonLakeFlyer committed
    void _updateArmed(bool armed);
    bool _apmArmingNotRequired(void);
    int     _id;                    ///< Mavlink system id
    int     _defaultComponentId;
    bool    _active;
    bool    _offlineEditingVehicle; ///< This Vehicle is a "disconnected" vehicle for ui use while offline editing
    MAV_AUTOPILOT       _firmwareType;
    MAV_TYPE            _vehicleType;
    FirmwarePlugin*     _firmwarePlugin;
    QObject*            _firmwarePluginInstanceData;
    AutoPilotPlugin*    _autopilotPlugin;
Don Gagne's avatar
Don Gagne committed
    MAVLinkProtocol*    _mavlink;
    bool                _soloFirmware;
    QGCToolbox*         _toolbox;
    SettingsManager*    _settingsManager;
Don Gagne's avatar
Don Gagne committed
    QList<LinkInterface*> _links;
    JoystickMode_t  _joystickMode;
    bool            _joystickEnabled;
    QGeoCoordinate  _coordinate;
    QGeoCoordinate  _homePosition;
    UASInterface*   _mav;
    int             _currentMessageCount;
    int             _messageCount;
    int             _currentErrorCount;
    int             _currentWarningCount;
    int             _currentNormalCount;
    MessageType_t   _currentMessageType;
    QString         _latestError;
    int             _updateCount;
dogmaphobic's avatar
dogmaphobic committed
    QString         _formatedMessage;
Don Gagne's avatar
Don Gagne committed
    int             _rcRSSI;
    double          _rcRSSIstore;
Don Gagne's avatar
Don Gagne committed
    bool            _autoDisconnect;    ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat
Don Gagne's avatar
Don Gagne committed
    bool            _flying;
    bool            _vtolInFwdFlight;
    uint32_t        _onboardControlSensorsPresent;
    uint32_t        _onboardControlSensorsEnabled;
    uint32_t        _onboardControlSensorsHealth;
    uint32_t        _onboardControlSensorsUnhealthy;
    bool            _gpsRawIntMessageAvailable;
    bool            _globalPositionIntMessageAvailable;
DonLakeFlyer's avatar
DonLakeFlyer committed
    double          _defaultCruiseSpeed;
    double          _defaultHoverSpeed;
Gus Grubba's avatar
Gus Grubba committed
    int             _telemetryRRSSI;
    int             _telemetryLRSSI;
    uint32_t        _telemetryRXErrors;
    uint32_t        _telemetryFixed;
    uint32_t        _telemetryTXBuffer;
    int             _telemetryLNoise;
    int             _telemetryRNoise;
    unsigned        _maxProtoVersion;
    bool            _vehicleCapabilitiesKnown;
    uint64_t        _capabilityBits;
Don Gagne's avatar
Don Gagne committed
    bool            _highLatencyLink;
    bool            _receivingAttitudeQuaternion;
    QGCCameraManager* _cameras;

        int         component;
        bool        commandInt; // true: use COMMAND_INT, false: use COMMAND_LONG
        MAV_CMD     command;
        MAV_FRAME   frame;
        double      rgParam[7];
        bool        showError;
    } MavCommandQueueEntry_t;

    QList<MavCommandQueueEntry_t>   _mavCommandQueue;
    QTimer                          _mavCommandAckTimer;
    int                             _mavCommandRetryCount;
    static const int                _mavCommandMaxRetryCount = 3;
    static const int                _mavCommandAckTimeoutMSecs = 3000;
DonLakeFlyer's avatar
DonLakeFlyer committed
    static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;
Don Gagne's avatar
Don Gagne committed
    QString             _prearmError;
    QTimer              _prearmErrorTimer;
    static const int    _prearmErrorTimeoutMSecs = 35 * 1000;   ///< Take away prearm error after 35 seconds

    // Lost connection handling
    bool                _connectionLost;
    bool                _connectionLostEnabled;

DonLakeFlyer's avatar
DonLakeFlyer committed
    bool                _initialPlanRequestComplete;

Don Gagne's avatar
Don Gagne committed
    MissionManager*     _missionManager;
    bool                _missionManagerInitialRequestSent;
    GeoFenceManager*    _geoFenceManager;
    bool                _geoFenceManagerInitialRequestSent;

    RallyPointManager*  _rallyPointManager;
    bool                _rallyPointManagerInitialRequestSent;
#if defined(QGC_AIRMAP_ENABLED)
    AirspaceVehicleManager* _airspaceVehicleManager;
Don Gagne's avatar
Don Gagne committed
    bool    _armed;         ///< true: vehicle is armed
    uint8_t _base_mode;     ///< base_mode from HEARTBEAT
    uint32_t _custom_mode;  ///< custom_mode from HEARTBEAT

    /// Used to store a message being sent by sendMessageMultiple
    typedef struct {
        mavlink_message_t   message;    ///< Message to send multiple times
        int                 retryCount; ///< Number of retries left
    } SendMessageMultipleInfo_t;
    QList<SendMessageMultipleInfo_t> _sendMessageMultipleList;    ///< List of messages being sent multiple times
    static const int _sendMessageMultipleRetries = 5;
    static const int _sendMessageMultipleIntraMessageDelay = 500;
    QTimer  _sendMultipleTimer;
    int     _nextSendMessageMultipleIndex;
    QTime               _flightTimer;
    QTimer              _mapTrajectoryTimer;
    QmlObjectListModel  _mapTrajectoryList;
    QGeoCoordinate      _mapTrajectoryLastCoordinate;
    bool                _mapTrajectoryHaveFirstCoordinate;
    static const int    _mapTrajectoryMsecsBetweenPoints = 1000;
    QmlObjectListModel  _cameraTriggerPoints;

    QmlObjectListModel              _adsbVehicles;
    QMap<uint32_t, ADSBVehicle*>    _adsbICAOMap;
    QMap<QString, ADSBVehicle*>     _trafficVehicleMap;
    // Toolbox references
dogmaphobic's avatar
dogmaphobic committed
    FirmwarePluginManager*      _firmwarePluginManager;
    JoystickManager*            _joystickManager;

    int                         _flowImageIndex;

Don Gagne's avatar
Don Gagne committed
    bool _allLinksInactiveSent; ///< true: allLinkInactive signal already sent one time

    uint                _messagesReceived;
    uint                _messagesSent;
    uint                _messagesLost;
    uint8_t             _messageSeq;
    uint8_t             _compID;
    bool                _heardFrom;

Don Gagne's avatar
Don Gagne committed
    int _firmwareMajorVersion;
    int _firmwareMinorVersion;
    int _firmwarePatchVersion;
    int _firmwareCustomMajorVersion;
    int _firmwareCustomMinorVersion;
    int _firmwareCustomPatchVersion;
    FIRMWARE_VERSION_TYPE _firmwareVersionType;
    QString _gitHash;
Gus Grubba's avatar
Gus Grubba committed
    quint64 _uid;
    int _lastAnnouncedLowBatteryPercent;
    SharedLinkInterfacePointer _priorityLink;  // We always keep a reference to the priority link to manage shutdown ordering
Don Gagne's avatar
Don Gagne committed
    // FactGroup facts

    Fact _rollFact;
    Fact _pitchFact;
    Fact _headingFact;
DonLakeFlyer's avatar
DonLakeFlyer committed
    Fact _rollRateFact;
    Fact _pitchRateFact;
    Fact _yawRateFact;
Don Gagne's avatar
Don Gagne committed
    Fact _groundSpeedFact;
    Fact _airSpeedFact;
    Fact _climbRateFact;
    Fact _altitudeRelativeFact;
    Fact _altitudeAMSLFact;
    Fact _flightDistanceFact;
    Fact _flightTimeFact;
    Fact _distanceToHomeFact;
    Fact _hobbsFact;
    VehicleGPSFactGroup             _gpsFactGroup;
DonLakeFlyer's avatar
DonLakeFlyer committed
    VehicleBatteryFactGroup         _battery1FactGroup;
    VehicleBatteryFactGroup         _battery2FactGroup;
    VehicleWindFactGroup            _windFactGroup;
    VehicleVibrationFactGroup       _vibrationFactGroup;
    VehicleTemperatureFactGroup     _temperatureFactGroup;
    VehicleClockFactGroup           _clockFactGroup;
    VehicleSetpointFactGroup        _setpointFactGroup;
    VehicleDistanceSensorFactGroup  _distanceSensorFactGroup;
    VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup;
Don Gagne's avatar
Don Gagne committed

    static const char* _rollFactName;
    static const char* _pitchFactName;
    static const char* _headingFactName;
DonLakeFlyer's avatar
DonLakeFlyer committed
    static const char* _rollRateFactName;
    static const char* _pitchRateFactName;
    static const char* _yawRateFactName;
Don Gagne's avatar
Don Gagne committed
    static const char* _groundSpeedFactName;
    static const char* _airSpeedFactName;
    static const char* _climbRateFactName;
    static const char* _altitudeRelativeFactName;
    static const char* _altitudeAMSLFactName;
    static const char* _flightDistanceFactName;
    static const char* _flightTimeFactName;
    static const char* _distanceToHomeFactName;
    static const char* _hobbsFactName;
Don Gagne's avatar
Don Gagne committed
    static const char* _gpsFactGroupName;
DonLakeFlyer's avatar
DonLakeFlyer committed
    static const char* _battery1FactGroupName;
    static const char* _battery2FactGroupName;
Don Gagne's avatar
Don Gagne committed
    static const char* _windFactGroupName;
    static const char* _vibrationFactGroupName;
    static const char* _temperatureFactGroupName;
dheideman's avatar
dheideman committed
    static const char* _clockFactGroupName;
    static const char* _distanceSensorFactGroupName;
    static const char* _estimatorStatusFactGroupName;
Don Gagne's avatar
Don Gagne committed

    static const int _vehicleUIUpdateRateMSecs = 100;

    // Settings keys
    static const char* _settingsGroup;
    static const char* _joystickModeSettingsKey;
    static const char* _joystickEnabledSettingsKey;