Commit 3440bc7c authored by Don Gagne's avatar Don Gagne

PX4 Firmware no longer requires mavlink USB start

parent 05092fe2
......@@ -131,10 +131,6 @@ public:
/// set into the link when it is added to LinkManager
uint8_t getMavlinkChannel(void) const { Q_ASSERT(_mavlinkChannelSet); return _mavlinkChannel; }
/// @return true: "sh /etc/init.d/rc.usb" must be sent on link to start mavlink
virtual bool requiresUSBMavlinkStart(void) const { return false; }
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
bool connect(void);
......
......@@ -189,8 +189,6 @@ void LinkManager::_addLink(LinkInterface* link)
connect(link, &LinkInterface::communicationError, _app, &QGCApplication::criticalMessageBoxOnMainThread);
connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes);
connect(link, &LinkInterface::connected, _mavlinkProtocol, &MAVLinkProtocol::linkConnected);
connect(link, &LinkInterface::disconnected, _mavlinkProtocol, &MAVLinkProtocol::linkDisconnected);
_mavlinkProtocol->resetMetadataForLink(link);
......
......@@ -173,41 +173,6 @@ void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link)
currLossCounter[channel] = 0;
}
void MAVLinkProtocol::linkConnected(void)
{
LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());
Q_ASSERT(link);
_linkStatusChanged(link, true);
}
void MAVLinkProtocol::linkDisconnected(void)
{
LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());
Q_ASSERT(link);
_linkStatusChanged(link, false);
}
void MAVLinkProtocol::_linkStatusChanged(LinkInterface* link, bool connected)
{
qCDebug(MAVLinkProtocolLog) << "_linkStatusChanged" << QString("%1").arg((long)link, 0, 16) << connected;
Q_ASSERT(link);
if (connected) {
if (link->requiresUSBMavlinkStart()) {
// Send command to start MAVLink
// XXX hacky but safe
// Start NSH
const char init[] = {0x0d, 0x0d, 0x0d, 0x0d};
link->writeBytes(init, sizeof(init));
const char* cmd = "sh /etc/init.d/rc.usb\n";
link->writeBytes(cmd, strlen(cmd));
link->writeBytes(init, 4);
}
}
}
/**
* This method parses all incoming bytes and constructs a MAVLink packet.
* It can handle multiple links in parallel, as each link has it's own buffer/
......
......@@ -155,9 +155,6 @@ public slots:
/** @brief Receive bytes from a communication interface */
void receiveBytes(LinkInterface* link, QByteArray b);
void linkConnected(void);
void linkDisconnected(void);
/** @brief Set the rate at which heartbeats are emitted */
void setHeartbeatRate(int rate);
/** @brief Set the system id of this application */
......@@ -285,7 +282,6 @@ private slots:
void _vehicleCountChanged(int count);
private:
void _linkStatusChanged(LinkInterface* link, bool connected);
void _sendMessage(mavlink_message_t message);
void _sendMessage(LinkInterface* link, mavlink_message_t message);
void _sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid);
......
......@@ -377,15 +377,6 @@ LinkConfiguration* SerialLink::getLinkConfiguration()
return _config;
}
bool SerialLink::requiresUSBMavlinkStart(void) const
{
if (_port) {
return QGCSerialPortInfo(*_port).boardTypePixhawk();
} else {
return false;
}
}
//--------------------------------------------------------------------------
//-- SerialConfiguration
......
......@@ -146,7 +146,6 @@ public:
void requestReset();
bool isConnected() const;
qint64 getConnectionSpeed() const;
bool requiresUSBMavlinkStart(void) const;
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment