XmlElement* ComponentTypeHandler::createXmlFor (Component* comp, const ComponentLayout* layout)
{
XmlElement* e = new XmlElement (getXmlTagName());
e->setAttribute ("name", comp->getName());
e->setAttribute ("id", String::toHexString (getComponentId (comp)));
e->setAttribute ("memberName", comp->getProperties() ["memberName"].toString());
e->setAttribute ("virtualName", comp->getProperties() ["virtualName"].toString());
e->setAttribute ("explicitFocusOrder", comp->getExplicitFocusOrder());
RelativePositionedRectangle pos (getComponentPosition (comp));
pos.updateFromComponent (*comp, layout);
pos.applyToXml (*e);
SettableTooltipClient* const ttc = dynamic_cast <SettableTooltipClient*> (comp);
if (ttc != 0 && ttc->getTooltip().isNotEmpty())
e->setAttribute ("tooltip", ttc->getTooltip());
for (int i = 0; i < colours.size(); ++i)
{
if (comp->isColourSpecified (colours[i]->colourId))
{
e->setAttribute (colours[i]->xmlTagName,
colourToHex (comp->findColour (colours[i]->colourId)));
}
}
return e;
}
int CompositeChannelBuffer::getBytes(int index, const ChannelBufferPtr& dst, int dstIndex, int length) const {
int componentId = getComponentId(index);
if (index > (capacity() - length) || dstIndex > (dst->capacity() - length)) {
CETTY_NDC_SCOPE();
throw RangeException("CompositeChannelBuffer getBytes out of range.");
}
int i = componentId;
int transferredBytes = length;
while (length > 0) {
const ChannelBufferPtr& s = components[i];
int adjustment = indices[i];
int localLength = std::min(length, s->capacity() - (index - adjustment));
s->getBytes(index - adjustment, dst, dstIndex, localLength);
index += localLength;
dstIndex += localLength;
length -= localLength;
++i;
}
return transferredBytes;
}
int CompositeChannelBuffer::setShort(int index, int value) {
int componentId = getComponentId(index);
if (index + 2 <= indices[componentId + 1]) {
return components[componentId]->setShort(index - indices[componentId], value);
}
else if (order() == ByteOrder::BIG) {
setByte(index, (value >> 8));
setByte(index + 1, value);
}
void Packet_v1::Print_v1(std::string const & componentName, Manager const * manager) const
{
Print_base();
std::cout << "[COMPONENTID: " << componentName << " {" << PROTOV1_COMPONENTID_SIZE << "}]"
<< "[REQUESTID: ";
COLOR_REVERSE_START;
std::cout << manager->getRegisteredRequestName(getComponentId(), getRequestId());
std::cout << " {" << PROTOV1_REQUESTID_SIZE << "}]"
<< "[SESSIONID: " << getSessionId() << " {" << PROTOV1_SESSIONID_SIZE << "}]"
<< std::endl;
}
int32_t CompositeChannelBuffer::getInt(int index) const {
int componentId = getComponentId(index);
if (index + 4 <= indices[componentId + 1]) {
return components[componentId]->getInt(index - indices[componentId]);
}
else if (order() == ByteOrder::BIG) {
return ((getShort(index) & 0xffff) << 16) | (getShort(index + 2) & 0xffff);
}
else {
return (getShort(index) & 0xFFFF) | ((getShort(index + 2) & 0xFFFF) << 16);
}
}
int64_t CompositeChannelBuffer::getLong(int index) const {
int componentId = getComponentId(index);
if (index + 8 <= indices[componentId + 1]) {
return components[componentId]->getLong(index - indices[componentId]);
}
else if (order() == ByteOrder::BIG) {
return ((static_cast<int64_t>(getInt(index)) & 0xffffffffL) << 32) | (getInt(index + 4) & 0xffffffffL);
}
else {
return (getInt(index) & 0xFFFFFFFFL) |
((static_cast<int64_t>(getInt(index + 4)) & 0xFFFFFFFFL) << 32);
}
}
int16_t CompositeChannelBuffer::getShort(int index) const {
int componentId = getComponentId(index);
if (index + 2 <= indices[componentId + 1]) {
return components[componentId]->getShort(index - indices[componentId]);
}
else if (order() == ByteOrder::BIG) {
return static_cast<int16_t>(((getByte(index) & 0xff) << 8) |
(getByte(index + 1) & 0xff));
}
else {
return static_cast<int16_t>((getByte(index) & 0xff) |
((getByte(index + 1) & 0xff) << 8));
}
}
int CompositeChannelBuffer::setByte(int index, int value) {
int componentId = getComponentId(index);
return components[componentId]->setByte(index - indices[componentId], value);
}
int8_t CompositeChannelBuffer::getByte(int index) const {
int componentId = getComponentId(index);
return components[componentId]->getByte(index - indices[componentId]);
}
ComponentNode::~ComponentNode()
{
COCA_DEBUG_INFO( "~ComponentNode " << getName() << ":" << getComponentId() );
resetComponent();
COCA_DEBUG_INFO( "~ComponentNode end " << getName() << ":" << getComponentId() );
}
/**
* 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/
* parsing state machine.
* @param link The interface to read from
* @see LinkInterface
**/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
// receiveMutex.lock();
mavlink_message_t message;
mavlink_status_t status;
// Cache the link ID for common use.
int linkId = link->getId();
static int mavlink09Count = 0;
static int nonmavlinkCount = 0;
static bool decodedFirstPacket = false;
static bool warnedUser = false;
static bool checkedUserNonMavlink = false;
static bool warnedUserNonMavlink = false;
// FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
if ((uint8_t)b[position] == 0x55) mavlink09Count++;
if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
{
warnedUser = true;
// Obviously the user tries to use a 0.9 autopilot
// with QGroundControl built for version 1.0
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. "
"Please upgrade the MAVLink version of your autopilot. "
"If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."));
}
if (decodeState == 0 && !decodedFirstPacket)
{
nonmavlinkCount++;
if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
{
//2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink)
{
link->requestReset();
checkedUserNonMavlink = true;
}
else
{
warnedUserNonMavlink = true;
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Please check if the baud rates of QGroundControl and your autopilot are the same."));
}
}
}
if (decodeState == 1)
{
decodedFirstPacket = true;
if (message.msgid == MAVLINK_MSG_ID_PING)
{
// process ping requests (tgt_system and tgt_comp must be zero)
mavlink_ping_t ping;
mavlink_msg_ping_decode(&message, &ping);
if (!ping.target_system && !ping.target_component)
{
mavlink_message_t msg;
mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid);
sendMessage(msg);
}
}
if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
{
// process telemetry status message
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(&message, &rstatus);
emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rstatus.rssi, rstatus.remrssi,
rstatus.txbuf, rstatus.noise, rstatus.remnoise);
}
// Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN + sizeof(quint64)];
// Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000;
qToBigEndian(time, buf);
// Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message);
// Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64);
// Now write this timestamp/message pair to the log.
QByteArray b((const char*)buf, len);
if (_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
}
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// ORDER MATTERS HERE!
// The UAS object has first to be created and connected,
// only then the rest of the application can be made aware
// of its existence, as it only then can send and receive
// it's first messages.
// Check if the UAS has the same id like this system
if (message.sysid == getSystemId())
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
}
// Create a new UAS based on the heartbeat received
// Todo dynamically load plugin at run-time for MAV
// WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION
// First create new UAS object
// Decode heartbeat message
mavlink_heartbeat_t heartbeat;
// Reset version field to 0
heartbeat.mavlink_version = 0;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
// Check if the UAS has a different protocol version
if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
{
// Bring up dialog to inform user
if (!versionMismatchIgnore)
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("The MAVLink protocol version on the MAV and QGroundControl mismatch! "
"It is unsafe to use different MAVLink versions. "
"QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
versionMismatchIgnore = true;
}
// Ignore this message and continue gracefully
continue;
}
// Create a new UAS object
uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
}
// Increase receive counter
totalReceiveCounter[linkId]++;
currReceiveCounter[linkId]++;
// Determine what the next expected sequence number is, accounting for
// never having seen a message for this system/component pair.
int lastSeq = lastIndex[message.sysid][message.compid];
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// Determine how many messages were skipped
int lostMessages = message.seq - expectedSeq;
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
if (lostMessages < 0)
{
lostMessages = 0;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[linkId] += lostMessages;
currLossCounter[linkId] += lostMessages;
}
// And update the last sequence number for this system/component pair
lastIndex[message.sysid][message.compid] = expectedSeq;
// Update on every 32th packet
if ((totalReceiveCounter[linkId] & 0x1F) == 0)
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)currLossCounter[linkId] / (double)(currReceiveCounter[linkId] + currLossCounter[linkId]);
receiveLoss *= 100.0f;
currLossCounter[linkId] = 0;
currReceiveCounter[linkId] = 0;
emit receiveLossChanged(message.sysid, receiveLoss);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short
// kind of inefficient, but no issue for a groundstation pc.
// It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message);
// Multiplex message if enabled
if (m_multiplexingEnabled)
{
// Get all links connected to this unit
QList<LinkInterface*> links = _linkMgr->getLinks();
// Emit message on all links that are currently connected
foreach(LinkInterface* currLink, links)
{
// Only forward this message to the other links,
// not the link the message was received on
if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
}
}
}
}
