Commit d7ef3dc0 authored by pixhawk's avatar pixhawk

Made generator --pedantic GCC flag compatible/C89 compatible, fixed a bug...

Made generator --pedantic GCC flag compatible/C89 compatible, fixed a bug where heartbeats where only sent to first link
parent f6266e94
...@@ -33,6 +33,8 @@ This file is part of the PIXHAWK project ...@@ -33,6 +33,8 @@ This file is part of the PIXHAWK project
#include <QApplication> #include <QApplication>
#include "LinkManager.h" #include "LinkManager.h"
#include <QDebug>
LinkManager* LinkManager::instance() { LinkManager* LinkManager::instance() {
static LinkManager* _instance = 0; static LinkManager* _instance = 0;
if(_instance == 0) { if(_instance == 0) {
...@@ -79,7 +81,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol) ...@@ -79,7 +81,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
// the protocol will receive new bytes from the link // the protocol will receive new bytes from the link
connect(link, SIGNAL(bytesReady(LinkInterface*)), protocol, SLOT(receiveBytes(LinkInterface*))); connect(link, SIGNAL(bytesReady(LinkInterface*)), protocol, SLOT(receiveBytes(LinkInterface*)));
// Store the connection information in the protocol links map // Store the connection information in the protocol links map
protocolLinks.insert(protocol, link); protocolLinks.insertMulti(protocol, link);
//qDebug() << __FILE__ << __LINE__ << "ADDED LINK TO PROTOCOL" << link->getName() << protocol->getName() << "NEW SIZE OF LINK LIST:" << protocolLinks.size();
} }
QList<LinkInterface*> LinkManager::getLinksForProtocol(ProtocolInterface* protocol) QList<LinkInterface*> LinkManager::getLinksForProtocol(ProtocolInterface* protocol)
......
...@@ -323,6 +323,7 @@ void MAVLinkProtocol::sendMessage(mavlink_message_t message) ...@@ -323,6 +323,7 @@ void MAVLinkProtocol::sendMessage(mavlink_message_t message)
for (i = links.begin(); i != links.end(); ++i) for (i = links.begin(); i != links.end(); ++i)
{ {
sendMessage(*i, message); sendMessage(*i, message);
qDebug() << __FILE__ << __LINE__ << "SENT HEARTBEAT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
} }
} }
......
...@@ -206,7 +206,7 @@ bool MAVLinkXMLParser::generate() ...@@ -206,7 +206,7 @@ bool MAVLinkXMLParser::generate()
QString encode = "static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"; QString encode = "static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n";
QString decode = "static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"; QString decode = "static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n";
QString pack = "static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\tuint16_t i = 0;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"; QString pack = "static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n";
QString compactSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; QString compactSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif";
//QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif";
QString unpacking = ""; QString unpacking = "";
......
...@@ -118,9 +118,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -118,9 +118,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if (!links->contains(link)) if (!links->contains(link))
{ {
addLink(link); addLink(link);
// qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName();
} }
// else
// {
// qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST";
// }
//qDebug() << "UAS RECEIVED" << message.sysid << message.compid << message.msgid; // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq;
if (message.sysid == uasId) if (message.sysid == uasId)
{ {
......
...@@ -99,7 +99,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn ...@@ -99,7 +99,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
} }
else if (dynamic_cast<UDPLink*>(link) != 0) else if (dynamic_cast<UDPLink*>(link) != 0)
{ {
ui.linkGroupBox->setTitle(tr("serial link")); ui.linkGroupBox->setTitle(tr("udp link"));
} }
else else
{ {
......
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