Commit 31bf2e94 authored by Don Gagne's avatar Don Gagne

Delete UAS when last link disconnected

Also removed partially implemented an incorrect getCommunicationStatus
api
parent dee99ea5
......@@ -50,7 +50,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
uasId(id),
unknownPackets(),
mavlink(protocol),
commStatus(COMM_DISCONNECTED),
receiveDropRate(0),
sendDropRate(0),
......@@ -1911,11 +1910,6 @@ quint64 UAS::getUptime() const
}
}
int UAS::getCommunicationStatus() const
{
return commStatus;
}
void UAS::writeParametersToStorage()
{
mavlink_message_t msg;
......@@ -3247,23 +3241,26 @@ void UAS::addLink(LinkInterface* link)
{
_links.append(LinkManager::instance()->sharedPointerForLink(link));
qCDebug(UASLog) << "addLink:" << QString("%1").arg((ulong)link, 0, 16);
connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
connect(LinkManager::instance(), &LinkManager::linkDisconnected, this, &UAS::_linkDisconnected);
}
}
void UAS::removeLink(QObject* object)
void UAS::_linkDisconnected(LinkInterface* link)
{
qCDebug(UASLog) << "removeLink:" << QString("%1").arg((ulong)object, 0, 16);
qCDebug(UASLog) << "_linkDisconnected:" << link->getName();
qCDebug(UASLog) << "link count:" << _links.count();
LinkInterface* link = dynamic_cast<LinkInterface*>(object);
for (int i=0; i<_links.count(); i++) {
if (_links[i].data() == link) {
_links.removeAt(i);
break;
}
}
if (_links.count() == 0) {
// Remove the UAS when all links to it close
UASManager::instance()->removeUAS(this);
}
}
/**
......
......@@ -87,8 +87,6 @@ public:
/** @brief The time interval the robot is switched on */
quint64 getUptime() const;
/** @brief Get the status flag for the communication */
int getCommunicationStatus() const;
/** @brief Add one measurement and get low-passed voltage */
float filterVoltage(float value) const;
/** @brief Get the links associated with this robot */
......@@ -348,7 +346,6 @@ protected: //COMMENTS FOR TEST UNIT
QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received
MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance
CommStatus commStatus; ///< Communication status
float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS
quint64 lastHeartbeat; ///< Time of the last heartbeat message
......@@ -800,8 +797,6 @@ public slots:
/** @brief Add a link associated with this robot */
void addLink(LinkInterface* link);
/** @brief Remove a link associated with this robot */
void removeLink(QObject* object);
/** @brief Receive a message from one of the communication links. */
virtual void receiveMessage(LinkInterface* link, mavlink_message_t message);
......@@ -953,6 +948,9 @@ protected slots:
/** @brief Read settings from disk */
void readSettings();
private slots:
void _linkDisconnected(LinkInterface* link);
private:
bool _containsLink(LinkInterface* link);
};
......
......@@ -108,8 +108,6 @@ public:
virtual int getUASID() const = 0; ///< Get the ID of the connected UAS
/** @brief The time interval the robot is switched on **/
virtual quint64 getUptime() const = 0;
/** @brief Get the status flag for the communication **/
virtual int getCommunicationStatus() const = 0;
virtual double getLocalX() const = 0;
virtual double getLocalY() const = 0;
......@@ -146,21 +144,6 @@ public:
/** @brief Send a message over all links this UAS can be reached with (!= all links) */
virtual void sendMessage(mavlink_message_t message) = 0;
/* COMMUNICATION FLAGS */
enum CommStatus {
/** Unit is disconnected, no failure state reached so far **/
COMM_DISCONNECTED = 0,
/** The communication is established **/
COMM_CONNECTING = 1,
/** The communication link is up **/
COMM_CONNECTED = 2,
/** The connection is closed **/
COMM_DISCONNECTING = 3,
COMM_FAIL = 4,
COMM_TIMEDOUT = 5///< Communication link failed
};
enum Airframe {
QGC_AIRFRAME_GENERIC = 0,
QGC_AIRFRAME_EASYSTAR,
......@@ -471,8 +454,6 @@ signals:
void armingChanged(int sysId, QString armingState);
/** @brief A command has been issued **/
void commandSent(int command);
/** @brief The connection status has changed **/
void connectionChanged(CommStatus connectionFlag);
/** @brief The robot is connecting **/
void connecting();
/** @brief The robot is connected **/
......
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