Commit bf9c2150 authored by Don Gagne's avatar Don Gagne

Pre-flight check now done in AutoPilot plugin

So this special casing is no longer needed
parent 8f50527b
#include "QGCMAVLinkUASFactory.h" #include "QGCMAVLinkUASFactory.h"
#include "UASManager.h" #include "UASManager.h"
#include "QGXPX4UAS.h"
QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) : QGCMAVLinkUASFactory::QGCMAVLinkUASFactory(QObject *parent) :
QObject(parent) QObject(parent)
...@@ -20,62 +19,31 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte ...@@ -20,62 +19,31 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
p = mavlink; p = mavlink;
} }
UASInterface* uas; UASInterface* uasInterface;
switch (heartbeat->autopilot) UAS* uasObject = new UAS(mavlink, sysid);
{ Q_CHECK_PTR(uasObject);
case MAV_AUTOPILOT_GENERIC: uasInterface = uasObject;
{
UAS* mav = new UAS(mavlink, sysid); uasObject->setSystemType((int)heartbeat->type);
// Set the system type
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), mav->getFileManager(), SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = mav;
}
break;
case MAV_AUTOPILOT_PX4:
{
QGXPX4UAS* px4 = new QGXPX4UAS(mavlink, sysid);
// Set the system type
px4->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), px4, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
uas = px4;
}
break;
default:
{
UAS* mav = new UAS(mavlink, sysid);
mav->setSystemType((int)heartbeat->type);
// Connect this robot to the UAS object // Connect this robot to the UAS object
// it is IMPORTANT here to use the right object type, // It is IMPORTANT here to use the right object type,
// else the slot of the parent object is called (and thus the special // else the slot of the parent object is called (and thus the special
// packets never reach their goal) // packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); connect(mavlink, &MAVLinkProtocol::messageReceived, uasObject, &UAS::receiveMessage);
uas = mav;
}
break;
}
// Set the autopilot type // Set the autopilot type
uas->setAutopilotType((int)heartbeat->autopilot); uasInterface->setAutopilotType((int)heartbeat->autopilot);
// Make UAS aware that this link can be used to communicate with the actual robot // Make UAS aware that this link can be used to communicate with the actual robot
uas->addLink(link); uasInterface->addLink(link);
// First thing we do with a new UAS is get the parameters // First thing we do with a new UAS is get the parameters
uas->getParamManager()->requestParameterList(); uasInterface->getParamManager()->requestParameterList();
// Now add UAS to "official" list, which makes the whole application aware of it // Now add UAS to "official" list, which makes the whole application aware of it
UASManager::instance()->addUAS(uas); UASManager::instance()->addUAS(uasInterface);
return uas; return uasInterface;
} }
#include "QGXPX4UAS.h"
QGXPX4UAS::QGXPX4UAS(MAVLinkProtocol* mavlink, int id) :
UAS(mavlink, id)
{
}
/**
* This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
* mavlink packet is received.
*
* @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
* messages can be sent back to the system via this link
* @param message MAVLink message, as received from the MAVLink protocol stack
*/
void QGXPX4UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
UAS::receiveMessage(link, message);
}
void QGXPX4UAS::processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue)
{
Q_UNUSED(rawValue);
Q_UNUSED(paramValue);
int compId = msg.compid;
if (paramName == "SYS_AUTOSTART") {
bool ok;
int val = parameters.value(compId)->value(paramName).toInt(&ok);
if (ok && val == 0) {
emit misconfigurationDetected(this);
qDebug() << "HINTING MISCONFIGURATION";
}
qDebug() << "SYS_AUTOSTART FOUND WITH VAL: " << val;
}
}
#ifndef QGXPX4UAS_H
#define QGXPX4UAS_H
#include "UAS.h"
class QGXPX4UAS : public UAS
{
Q_OBJECT
Q_INTERFACES(UASInterface)
public:
QGXPX4UAS(MAVLinkProtocol* mavlink, int id);
public slots:
/** @brief Receive a MAVLink message from this MAV */
void receiveMessage(LinkInterface* link, mavlink_message_t message);
virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
};
#endif // QGXPX4UAS_H
...@@ -1029,8 +1029,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -1029,8 +1029,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
paramVal.type = rawValue.param_type; paramVal.type = rawValue.param_type;
processParamValueMsg(message, parameterName,rawValue,paramVal); processParamValueMsg(message, parameterName,rawValue,paramVal);
processParamValueMsgHook(message, parameterName,rawValue,paramVal);
} }
break; break;
case MAVLINK_MSG_ID_COMMAND_ACK: case MAVLINK_MSG_ID_COMMAND_ACK:
......
...@@ -930,7 +930,6 @@ protected: ...@@ -930,7 +930,6 @@ protected:
quint64 getUnixReferenceTime(quint64 time); quint64 getUnixReferenceTime(quint64 time);
virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
virtual void processParamValueMsgHook(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue) { Q_UNUSED(msg); Q_UNUSED(paramName); Q_UNUSED(rawValue); Q_UNUSED(paramValue); };
int componentID[256]; int componentID[256];
bool componentMulti[256]; bool componentMulti[256];
......
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