|
|
|
/****************************************************************************
|
|
|
|
*
|
|
|
|
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
|
|
|
*
|
|
|
|
* QGroundControl is licensed according to the terms in the file
|
|
|
|
* COPYING.md in the root of the source code directory.
|
|
|
|
*
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <QObject>
|
|
|
|
#include <QThread>
|
|
|
|
#include <QMutex>
|
|
|
|
#include <QString>
|
|
|
|
|
|
|
|
#ifdef __android__
|
|
|
|
#include "qserialport.h"
|
|
|
|
#else
|
|
|
|
#include <QSerialPort>
|
|
|
|
#endif
|
|
|
|
#include <QMetaType>
|
|
|
|
#include <QLoggingCategory>
|
|
|
|
|
|
|
|
// We use QSerialPort::SerialPortError in a signal so we must declare it as a meta type
|
|
|
|
Q_DECLARE_METATYPE(QSerialPort::SerialPortError)
|
|
|
|
|
|
|
|
#include "QGCConfig.h"
|
|
|
|
#include "LinkConfiguration.h"
|
|
|
|
#include "LinkInterface.h"
|
|
|
|
|
|
|
|
Q_DECLARE_LOGGING_CATEGORY(SerialLinkLog)
|
|
|
|
|
|
|
|
class LinkManager;
|
|
|
|
|
|
|
|
/// SerialLink configuration
|
|
|
|
class SerialConfiguration : public LinkConfiguration
|
|
|
|
{
|
|
|
|
Q_OBJECT
|
|
|
|
|
|
|
|
public:
|
|
|
|
|
|
|
|
SerialConfiguration(const QString& name);
|
|
|
|
SerialConfiguration(SerialConfiguration* copy);
|
|
|
|
|
|
|
|
Q_PROPERTY(int baud READ baud WRITE setBaud NOTIFY baudChanged)
|
|
|
|
Q_PROPERTY(int dataBits READ dataBits WRITE setDataBits NOTIFY dataBitsChanged)
|
|
|
|
Q_PROPERTY(int flowControl READ flowControl WRITE setFlowControl NOTIFY flowControlChanged)
|
|
|
|
Q_PROPERTY(int stopBits READ stopBits WRITE setStopBits NOTIFY stopBitsChanged)
|
|
|
|
Q_PROPERTY(int parity READ parity WRITE setParity NOTIFY parityChanged)
|
|
|
|
Q_PROPERTY(QString portName READ portName WRITE setPortName NOTIFY portNameChanged)
|
|
|
|
Q_PROPERTY(QString portDisplayName READ portDisplayName NOTIFY portDisplayNameChanged)
|
|
|
|
Q_PROPERTY(bool usbDirect READ usbDirect WRITE setUsbDirect NOTIFY usbDirectChanged) ///< true: direct usb connection to board
|
|
|
|
|
|
|
|
int baud() const { return _baud; }
|
|
|
|
int dataBits() const { return _dataBits; }
|
|
|
|
int flowControl() const { return _flowControl; } ///< QSerialPort Enums
|
|
|
|
int stopBits() const { return _stopBits; }
|
|
|
|
int parity() const { return _parity; } ///< QSerialPort Enums
|
|
|
|
bool usbDirect() const { return _usbDirect; }
|
|
|
|
|
|
|
|
const QString portName () { return _portName; }
|
|
|
|
const QString portDisplayName () { return _portDisplayName; }
|
|
|
|
|
|
|
|
void setBaud (int baud);
|
|
|
|
void setDataBits (int databits);
|
|
|
|
void setFlowControl (int flowControl); ///< QSerialPort Enums
|
|
|
|
void setStopBits (int stopBits);
|
|
|
|
void setParity (int parity); ///< QSerialPort Enums
|
|
|
|
void setPortName (const QString& portName);
|
|
|
|
void setUsbDirect (bool usbDirect);
|
|
|
|
|
|
|
|
static QStringList supportedBaudRates();
|
|
|
|
static QString cleanPortDisplayname(const QString name);
|
|
|
|
|
|
|
|
/// From LinkConfiguration
|
|
|
|
LinkType type () { return LinkConfiguration::TypeSerial; }
|
|
|
|
void copyFrom (LinkConfiguration* source);
|
|
|
|
void loadSettings (QSettings& settings, const QString& root);
|
|
|
|
void saveSettings (QSettings& settings, const QString& root);
|
|
|
|
void updateSettings ();
|
|
|
|
QString settingsURL () { return "SerialSettings.qml"; }
|
|
|
|
QString settingsTitle () { return tr("Serial Link Settings"); }
|
|
|
|
|
|
|
|
signals:
|
|
|
|
void baudChanged ();
|
|
|
|
void dataBitsChanged ();
|
|
|
|
void flowControlChanged ();
|
|
|
|
void stopBitsChanged ();
|
|
|
|
void parityChanged ();
|
|
|
|
void portNameChanged ();
|
|
|
|
void portDisplayNameChanged ();
|
|
|
|
void usbDirectChanged (bool usbDirect);
|
|
|
|
|
|
|
|
private:
|
|
|
|
static void _initBaudRates();
|
|
|
|
|
|
|
|
int _baud;
|
|
|
|
int _dataBits;
|
|
|
|
int _flowControl;
|
|
|
|
int _stopBits;
|
|
|
|
int _parity;
|
|
|
|
QString _portName;
|
|
|
|
QString _portDisplayName;
|
|
|
|
bool _usbDirect;
|
|
|
|
};
|
|
|
|
|
|
|
|
class SerialLink : public LinkInterface
|
|
|
|
{
|
|
|
|
Q_OBJECT
|
|
|
|
|
|
|
|
public:
|
Fix Segfault on vehicle reboot (#9267)
* LinkManager::createConnectedLink: use make_shared to avoid separate allocation
Also avoids some memory leaks when returning early.
* fix LinkManager: set setAutoConnect()
* fix MAVLinkProtocol: use a weak_ptr to check if the link stays valid
It is not enough to check if the link is valid only on entering the method.
If message handling called VehicleLinkManager::closeVehicle(), the link
was removed, and possibly deleted. If more mavlink messages were in the
same buffer, the link was accessed again, triggering a segfault.
This happened for example when rebooting the vehicle from parameter editor.
* Vehicle: fix uninitialized variable _capabilityBits
From valgrind:
==690288== Conditional jump or move depends on uninitialised value(s)
==690288== at 0x5B5F18: GeoFenceController::supported() const (GeoFenceController.cc:495)
==690288== by 0x5B309E: GeoFenceController::_managerVehicleChanged(Vehicle*) (GeoFenceController.cc:125)
==690288== by 0x5B79AF: QtPrivate::FunctorCall<QtPrivate::IndexesList<0>, QtPrivate::List<Vehicle*>, void, void (GeoFenceController::*)(Vehicle*)>::call(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:152)
==690288== by 0x5B7797: void QtPrivate::FunctionPointer<void (GeoFenceController::*)(Vehicle*)>::call<QtPrivate::List<Vehicle*>, void>(void (GeoFenceController::*)(Vehicle*), GeoFenceController*, void**) (qobjectdefs_impl.h:185)
==690288== by 0x5B7325: QtPrivate::QSlotObject<void (GeoFenceController::*)(Vehicle*), QtPrivate::List<Vehicle*>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288== by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)
==690288== by 0x913C9C: PlanMasterController::managerVehicleChanged(Vehicle*) (moc_PlanMasterController.cpp:544)
==690288== by 0x5FF00B: PlanMasterController::_activeVehicleChanged(Vehicle*) (PlanMasterController.cc:160)
==690288== by 0x5FE9A4: PlanMasterController::startStaticActiveVehicle(Vehicle*, bool) (PlanMasterController.cc:116)
==690288== by 0x912EB9: PlanMasterController::qt_static_metacall(QObject*, QMetaObject::Call, int, void**) (moc_PlanMasterController.cpp:267)
==690288== by 0x913A2A: PlanMasterController::qt_metacall(QMetaObject::Call, int, void**) (moc_PlanMasterController.cpp:473)
==690288== by 0x880E5C8: QQmlObjectOrGadget::metacall(QMetaObject::Call, int, void**) const (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Qml.so.5.12.5)
* FTPManager: fix uninitialized variable & message filter logic
From Valgrind:
==690288== Conditional jump or move depends on uninitialised value(s)
==690288== at 0x6E74E6: FTPManager::_mavlinkMessageReceived(__mavlink_message const&) (FTPManager.cc:108)
==690288== by 0x70D164: Vehicle::_mavlinkMessageReceived(LinkInterface*, __mavlink_message) (Vehicle.cc:603)
==690288== by 0x726F53: QtPrivate::FunctorCall<QtPrivate::IndexesList<0, 1>, QtPrivate::List<LinkInterface*, __mavlink_message>, void, void (Vehicle::*)(LinkInterface*, __mavlink_message)>::call(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:152)
==690288== by 0x725990: void QtPrivate::FunctionPointer<void (Vehicle::*)(LinkInterface*, __mavlink_message)>::call<QtPrivate::List<LinkInterface*, __mavlink_message>, void>(void (Vehicle::*)(LinkInterface*, __mavlink_message), Vehicle*, void**) (qobjectdefs_impl.h:185)
==690288== by 0x723B4B: QtPrivate::QSlotObject<void (Vehicle::*)(LinkInterface*, __mavlink_message), QtPrivate::List<LinkInterface*, __mavlink_message>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288== by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)
==690288== by 0x95DE46: MAVLinkProtocol::messageReceived(LinkInterface*, __mavlink_message) (moc_MAVLinkProtocol.cpp:346)
==690288== by 0x757522: MAVLinkProtocol::receiveBytes(LinkInterface*, QByteArray) (MAVLinkProtocol.cc:362)
==690288== by 0x74C71E: QtPrivate::FunctorCall<QtPrivate::IndexesList<0, 1>, QtPrivate::List<LinkInterface*, QByteArray>, void, void (MAVLinkProtocol::*)(LinkInterface*, QByteArray)>::call(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:152)
==690288== by 0x74BAB4: void QtPrivate::FunctionPointer<void (MAVLinkProtocol::*)(LinkInterface*, QByteArray)>::call<QtPrivate::List<LinkInterface*, QByteArray>, void>(void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), MAVLinkProtocol*, void**) (qobjectdefs_impl.h:185)
==690288== by 0x74B3B7: QtPrivate::QSlotObject<void (MAVLinkProtocol::*)(LinkInterface*, QByteArray), QtPrivate::List<LinkInterface*, QByteArray>, void>::impl(int, QtPrivate::QSlotObjectBase*, QObject*, void**, bool*) (qobjectdefs_impl.h:414)
==690288== by 0x9D43DF5: QMetaObject::activate(QObject*, int, int, void**) (in /opt/install/qt/5.12.5/gcc_64/lib/libQt5Core.so.5.12.5)
* FTPManager: fix uninitialized Request
- Parts of the header were uninit
- zero init the payload allows for mavlink trimming
* Fix ~TerrainTile: use delete[] for arrays
* FactGroup: fix expensive & unnecessary object creation and map lookups
Every mavlink message reception was leading to creation of a list and
key lookup for each of the keys. This can easily be avoided and results
in noticeable reduction of CPU load (I'm seeing overall around 4% with
a pixhawk on USB).
The same pattern exists elsewhere as well
4 years ago
|
|
|
SerialLink(SharedLinkConfigurationPtr& config, bool isPX4Flow = false);
|
|
|
|
virtual ~SerialLink();
|
|
|
|
|
|
|
|
// LinkInterface overrides
|
|
|
|
bool isConnected(void) const override;
|
|
|
|
void disconnect (void) override;
|
|
|
|
|
|
|
|
/// Don't even think of calling this method!
|
|
|
|
QSerialPort* _hackAccessToPort(void) { return _port; }
|
|
|
|
|
|
|
|
private slots:
|
|
|
|
void _writeBytes(const QByteArray data) override;
|
|
|
|
|
|
|
|
public slots:
|
|
|
|
void linkError(QSerialPort::SerialPortError error);
|
|
|
|
|
|
|
|
private slots:
|
|
|
|
void _readBytes (void);
|
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
// LinkInterface overrides
|
|
|
|
bool _connect(void) override;
|
|
|
|
|
|
|
|
void _emitLinkError (const QString& errorMsg);
|
|
|
|
bool _hardwareConnect (QSerialPort::SerialPortError& error, QString& errorString);
|
|
|
|
bool _isBootloader (void);
|
|
|
|
|
|
|
|
QSerialPort* _port = nullptr;
|
|
|
|
quint64 _bytesRead = 0;
|
|
|
|
int _timeout;
|
|
|
|
QMutex _dataMutex; ///< Mutex for reading data from _port
|
|
|
|
QMutex _writeMutex; ///< Mutex for accessing the _transmitBuffer.
|
|
|
|
volatile bool _stopp = false;
|
|
|
|
QMutex _stoppMutex; ///< Mutex for accessing _stopp
|
|
|
|
QByteArray _transmitBuffer; ///< An internal buffer for receiving data from member functions and actually transmitting them via the serial port.
|
|
|
|
SerialConfiguration* _serialConfig = nullptr;
|
|
|
|
|
|
|
|
};
|
|
|
|
|