diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 3341c6b9c2ceb78de1e1fa6a08d743eda32c5b86..898e2c54488e096d3e1e593e1f9365941a1cc853 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -169,3 +169,15 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // FIXME: Need to implement mavlink message severity adjustment } + +void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + // Streams are not started automatically on APM stack + vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTENDED_STATUS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_RC_CHANNELS, 2); + vehicle->requestDataStream(MAV_DATA_STREAM_POSITION, 3); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA1, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA2, 10); + vehicle->requestDataStream(MAV_DATA_STREAM_EXTRA3, 3); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 524a36f3850bee872d70fc5fbca5fd7aec62406d..6e798089bbf2fc32ea648a18a9d39456eb4104bf 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -37,7 +37,6 @@ class APMFirmwarePlugin : public FirmwarePlugin public: // Overrides from FirmwarePlugin - virtual bool isCapable(FirmwareCapabilities capabilities); virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); virtual QStringList flightModes(void); @@ -45,7 +44,8 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); - + virtual void initializeVehicle(Vehicle* vehicle); + private: /// All access to singleton is through AutoPilotPluginManager::instance APMFirmwarePlugin(QObject* parent = NULL); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index c84a8ece94e9a9b6dd71726f495faeeb571bbfc5..3783c5a8db6b2576b7630836e2195a99273e7f5b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -35,6 +35,8 @@ #include #include +class Vehicle; + /// This is the base class for Firmware specific plugins /// /// The FirmwarePlugin class is the abstract base class which represents the methods and objects @@ -92,6 +94,9 @@ public: /// @param message[in,out] Mavlink message to adjust if needed. virtual void adjustMavlinkMessage(mavlink_message_t* message) = 0; + /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. + virtual void initializeVehicle(Vehicle* vehicle) = 0; + protected: FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { } }; diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc index 5b11fdf1c3702b3974ec820b74d2ee1cae33fd40..910eba2f8ee99abbfb1794098be820df84b91e88 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc @@ -103,3 +103,10 @@ void GenericFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // Generic plugin does no message adjustment } + +void GenericFirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + + // Generic Flight Stack is by definition "generic", so no extra work +} diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h index 505a3de3df34b2f3ba9cddbc31cf3ddcfd29d736..7b2634e4f6a313ba5e722fcf30f4ff03e39e6df5 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h @@ -45,6 +45,7 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void initializeVehicle(Vehicle* vehicle); private: /// All access to singleton is through AutoPilotPluginManager::instance diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 731f7505675865b6dbf8b50a47d2e066630189d6..2a2de268b7f826e0f28e58c43886271c0be2be6f 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -193,3 +193,10 @@ bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) { return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability)) == capabilities; } + +void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + + // PX4 Flight Stack doesn't need to do any extra work +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 953842e595c2db0f7696f5b50443df6a7f4c2efe..f1400622b12812b1d9f197d9a58657433c8d2447 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -45,6 +45,7 @@ public: virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode); virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); + virtual void initializeVehicle(Vehicle* vehicle); private: /// All access to singleton is through AutoPilotPluginManager::instance diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index e31895b986328fce9ef9148ecf40048f996067d9..17c308c4fe0c4ce9854d89ac16cace497d2f834a 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -89,6 +89,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _armed(false) , _base_mode(0) , _custom_mode(0) + , _nextSendMessageMultipleIndex(0) { _addLink(link); @@ -161,6 +162,11 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) if (qgcApp()->useNewMissionEditor()) { _missionManager = new MissionManager(this); } + + _firmwarePlugin->initializeVehicle(this); + + _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay); + connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext); } Vehicle::~Vehicle() @@ -1069,3 +1075,49 @@ bool Vehicle::missingParameters(void) { return _autopilotPlugin->missingParameters(); } + +void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate) +{ + mavlink_message_t msg; + mavlink_request_data_stream_t dataStream; + + dataStream.req_stream_id = stream; + dataStream.req_message_rate = rate; + dataStream.start_stop = 1; // start + dataStream.target_system = id(); + dataStream.target_component = 0; + + mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream); + + // We use sendMessageMultiple since we really want these to make it to the vehicle + sendMessageMultiple(msg); +} + +void Vehicle::_sendMessageMultipleNext(void) +{ + if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) { + qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; + + sendMessage(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message); + + if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) { + _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex); + } else { + _nextSendMessageMultipleIndex++; + } + } + + if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) { + _nextSendMessageMultipleIndex = 0; + } +} + +void Vehicle::sendMessageMultiple(mavlink_message_t message) +{ + SendMessageMultipleInfo_t info; + + info.message = message; + info.retryCount = _sendMessageMultipleRetries; + + _sendMessageMultipleList.append(info); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 97e4649f8ee4785a6c15d1cae58852a0dad9ab9a..84145e2500d566ae7bfea11893829ee47b7ae591 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -151,6 +151,10 @@ public: /// Sends this specified message to all links accociated with this vehicle void sendMessage(mavlink_message_t message); + /// Sends the specified messages multiple times to the vehicle in order to attempt to + /// guarantee that it makes it to the vehicle. + void sendMessageMultiple(mavlink_message_t message); + /// Provides access to uas from vehicle. Temporary workaround until UAS is fully phased out. UAS* uas(void) { return _uas; } @@ -180,6 +184,11 @@ public: bool hilMode(void); void setHilMode(bool hilMode); + /// Requests the specified data stream from the vehicle + /// @param stream Stream which is being requested + /// @param rate Rate at which to send stream in Hz + void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate); + bool missingParameters(void); typedef enum { @@ -289,7 +298,8 @@ private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkDisconnected(LinkInterface* link); void _sendMessage(mavlink_message_t message); - + void _sendMessageMultipleNext(void); + void _handleTextMessage (int newCount); /** @brief Attitude from main autopilot / system state */ void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); @@ -397,7 +407,22 @@ private: 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 _sendMessageMultipleList; ///< List of messages being sent multiple times + + static const int _sendMessageMultipleRetries = 5; + static const int _sendMessageMultipleIntraMessageDelay = 500; + + QTimer _sendMultipleTimer; + int _nextSendMessageMultipleIndex; + // Settings keys static const char* _settingsGroup; static const char* _joystickModeSettingsKey; static const char* _joystickEnabledSettingsKey;