void BasicDeviceDriver::consume(const IMC::LoggingControl* msg) { if ((msg->getDestination() != getSystemId()) || (msg->getDestinationEntity() != getEntityId())) return; switch (msg->op) { case IMC::LoggingControl::COP_CURRENT_NAME: if (m_log_name_pending) { m_log_name_pending = false; openLog(m_ctx.dir_log / msg->name); } break; case IMC::LoggingControl::COP_STARTED: openLog(m_ctx.dir_log / msg->name); break; case IMC::LoggingControl::COP_STOPPED: closeLog(); break; } }
BinInputStreamType* XSLTInputSource::makeStream() const { BinInputStreamType* theResult = 0; MemoryManager* theManager = getMemoryManager(); assert(theManager != 0 ); if (m_stream != 0) { theResult = new (theManager) StdBinInputStream(*m_stream); } else if (m_node == 0) { const XalanDOMChar* const theSystemId = getSystemId(); if (theSystemId != 0) { XALAN_USING_XERCES(XMLURL) XMLURL theURL(theManager); URISupport::getURLFromString(theSystemId, theURL, *theManager); theResult = theURL.makeNewStream(); } } return theResult; }
void consume(const IMC::LoggingControl* msg) { if (msg->getSource() != getSystemId()) return; if (!m_args.save_in_837) return; m_log_filename = msg->name; switch (msg->op) { case IMC::LoggingControl::COP_STARTED: if (m_log_file.is_open()) m_log_file.close(); if (m_active) m_log_file.open((m_ctx.dir_log / m_log_filename / "Data.837").c_str(), std::ios::binary); break; case IMC::LoggingControl::COP_REQUEST_STOP: m_log_file.close(); break; } }
void ViewController::goToGameList(SystemData* system) { if(mState.viewing == SYSTEM_SELECT) { // move system list auto sysList = getSystemListView(); float offX = sysList->getPosition().x(); int sysId = getSystemId(system); sysList->setPosition(sysId * (float)Renderer::getScreenWidth(), sysList->getPosition().y()); offX = sysList->getPosition().x() - offX; mCamera.translation().x() -= offX; } mState.viewing = GAME_LIST; mState.system = system; if (mCurrentView) { mCurrentView->onHide(); } mCurrentView = getGameListView(system); if (mCurrentView) { mCurrentView->onShow(); } playViewTransition(); }
void consume(const IMC::VehicleCommand* vc) { if (vc->type == IMC::VehicleCommand::VC_REQUEST) return; if ((vc->getDestination() != getSystemId()) || (vc->getDestinationEntity() != getEntityId()) || (m_vreq_ctr != vc->request_id)) return; if (!pendingReply()) return; m_vc_reply_deadline = -1; bool error = vc->type == IMC::VehicleCommand::VC_FAILURE; // Ignore failure if it failed to stop calibration if (error && (vc->command == IMC::VehicleCommand::VC_STOP_CALIBRATION)) { debug("%s", vc->info.c_str()); error = false; } if (initMode() || execMode()) { if (error) changeMode(IMC::PlanControlState::PCS_READY, vc->info, false); return; } }
void Daemon::consume(const DUNE::IMC::EntityParameters* msg) { if (msg->getDestination() != getSystemId()) return; if (msg->getDestinationEntity() != getEntityId()) return; std::string task_name; try { task_name = m_ctx.entities.resolveTaskName(msg->name); } catch (...) { return; } IMC::MessageList<IMC::EntityParameter>::const_iterator itr = msg->params.begin(); for ( ; itr != msg->params.end(); ++itr) { if (m_ctx.config.get(task_name, (*itr)->name) != (*itr)->value) m_scfg.set(task_name, (*itr)->name, (*itr)->value); } m_scfg.writeToFile(m_scfg_file.c_str()); }
/** * \return Unique combination of system ID (linux, osx, win) and host name. * E.g. "linux_vertigo". Used for test data that may differ on different machines. */ QString RS::getHostId() { return QString("%1_%2") .arg(getSystemId()) #if defined(Q_OS_LINUX) .arg(getenv("HOSTNAME")); #elif defined(Q_OS_MAC) // environment variable HOSTNAME not exported on OS X by default: .arg(QHostInfo::localHostName());
void consume(const IMC::EstimatedState* msg) { if (msg->getSource() != getSystemId()) return; m_state = *msg; }
// --------------------------------------------------------------------------- // LocalFileInputSource: InputSource interface implementation // --------------------------------------------------------------------------- BinInputStream* LocalFileInputSource::makeStream() const { BinFileInputStream* retStrm = new (getMemoryManager()) BinFileInputStream(getSystemId(), getMemoryManager()); if (!retStrm->getIsOpen()) { delete retStrm; return 0; } return retStrm; }
void BasicDeviceDriver::consume(const IMC::EstimatedState* msg) { if (msg->getSource() != getSystemId()) return; if (!isActive()) return; onEstimatedState(*msg); }
void ViewController::goToSystemView(SystemData* system) { mState.viewing = SYSTEM_SELECT; mState.system = system; auto systemList = getSystemListView(); systemList->setPosition(getSystemId(system) * (float)Renderer::getScreenWidth(), systemList->getPosition().y()); systemList->goToSystem(system, false); mCurrentView = systemList; playViewTransition(); }
bool send_device_updates() { if (!m_update_pool_empty) { spew("won't send device updates message because pool is not empty"); return false; } debug("sending device updates"); DUNE::IMC::DeviceUpdate msg; uint8_t buffer[65535]; std::map<std::string, IMC::Announce>::iterator it; for (it = m_last_announces.begin(); it != m_last_announces.end(); it++) { DevicePosition pos; pos.id = it->second.getSource(); pos.lat = it->second.lat; pos.lon = it->second.lon; pos.time = it->second.getTimeStamp(); msg.positions.push_back(pos); spew("position to be sent: id:%d, lat: %f, lon: %f, time: %f", pos.id, pos.lat, pos.lon, pos.time); } m_last_announces.clear(); msg.source = getSystemId(); msg.destination = 0xFFFF; DUNE::IMC::IridiumMsgTx * m = new DUNE::IMC::IridiumMsgTx(); int len = msg.serialize(buffer); m->data.assign(buffer, buffer + len); m->req_id = m_rnd->random() % 65535; m->ttl = m_args.delay_between_device_updates; m->setTimeStamp(); m_dev_update_req_id = m->req_id; dispatch(m); std::stringstream ss; m->toText(ss); spew("sent the following message: %s", ss.str().c_str()); m_update_pool_empty = false; return true; }
void handleCAMUA(std::auto_ptr<NMEAReader>& stn) { unsigned src = 0; *stn >> src; unsigned dst = 0; *stn >> dst; // Get value. std::string val; *stn >> val; unsigned value = 0; std::sscanf(val.c_str(), "%04X", &value); if (value == c_code_abort_ack) { m_acop_out.op = IMC::AcousticOperation::AOP_ABORT_ACKED; m_acop_out.system = m_acop.system; dispatch(m_acop_out); resetOp(); } if (value == c_code_plan_ack) { inf(DTR("plan started")); m_pc->setDestination(m_pc->getSource()); m_pc->setDestinationEntity(m_pc->getSourceEntity()); m_pc->setSource(getSystemId()); m_pc->setSourceEntity(getEntityId()); m_pc->type = IMC::PlanControl::PC_SUCCESS; m_pc->flags = 0; dispatch(*m_pc); } else if (value & c_mask_qtrack) { unsigned beacon = (value & c_mask_qtrack_beacon) >> 10; unsigned range = (value & c_mask_qtrack_range); IMC::LblRangeAcceptance msg; msg.setSource(m_mimap[src]); msg.id = beacon; msg.range = range; msg.acceptance = IMC::LblRangeAcceptance::RR_ACCEPTED; dispatch(msg); inf("%s %u: %u", DTR("range to"), beacon, range); }
void ViewController::goToSystemView(SystemData* system) { // Tell any current view it's about to be hidden if (mCurrentView) { mCurrentView->onHide(); } mState.viewing = SYSTEM_SELECT; mState.system = system; auto systemList = getSystemListView(); systemList->setPosition(getSystemId(system) * (float)Renderer::getScreenWidth(), systemList->getPosition().y()); systemList->goToSystem(system, false); mCurrentView = systemList; playViewTransition(); }
void consume(const IMC::PowerOperation* po) { if (po->getDestination() != getSystemId()) return; switch (po->op) { case IMC::PowerOperation::POP_PWR_DOWN_IP: closeDB(); setEntityState(IMC::EntityState::ESTA_ERROR, Status::CODE_POWER_DOWN); break; case IMC::PowerOperation::POP_PWR_DOWN_ABORTED: openDB(); break; default: break; } }
void consume(const IMC::LoggingControl* msg) { if (msg->getSource() != getSystemId()) return; if (!m_args.save_to_file) return; switch (msg->op) { case IMC::LoggingControl::COP_STARTED: closeLog(); openLog(m_ctx.dir_log / msg->name / m_args.file_name_837); break; case IMC::LoggingControl::COP_STOPPED: closeLog(); break; } }
BinInputStream * CompressedInputSource::makeStream() const { if (head_[0] == 'B' && head_[1] == 'Z') { Bzip2InputStream * retStrm = new Bzip2InputStream(Internal::StringManager().convert(getSystemId())); if (!retStrm->getIsOpen()) { delete retStrm; return 0; } return retStrm; } else /* (bz[0] == g1 && bz[1] == g2), where char g1 = 0x1f and char g2 = 0x8b */ { GzipInputStream * retStrm = new GzipInputStream(Internal::StringManager().convert(getSystemId())); if (!retStrm->getIsOpen()) { delete retStrm; return 0; } return retStrm; } }
void consume(const IMC::Message* msg) { if (msg->getSource() == getSystemId()) m_msg_mon.updateMessage(msg); }
/** * 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); } } } }
void BasicAutopilot::consume(const IMC::EstimatedState* msg) { if (!isActive()) return; if (msg->getSource() != getSystemId()) return; m_estate = *msg; // check if vertical control mode is valid if (m_vertical_mode >= VERTICAL_MODE_SIZE) { signalBadVertical(DTR("invalid vertical control mode %d")); return; } else if (m_vertical_mode == VERTICAL_MODE_NONE) { float delta = m_vmode_delta.getDelta(); if (delta > 0.0) m_vmode_wait += delta; if (m_vmode_wait > c_mode_timeout) { m_vmode_wait = 0.0; signalBadVertical(DTR("timed out while waiting for vertical control mode")); } return; } else if (m_vertical_mode == VERTICAL_MODE_ALTITUDE) { // Required depth for bottom following = current depth + required altitude correction // in case the follow depth is lower than 0.0 it will be capped since the btrack // is not possible // check if we have altitude measurements if (msg->alt < 0.0) { setEntityState(IMC::EntityState::ESTA_ERROR, DTR(c_no_alt)); err("%s", DTR(c_no_alt)); return; } m_bottom_follow_depth = m_estate.depth + (msg->alt - m_vertical_ref); if (m_bottom_follow_depth < 0.0) { if (m_btrack_wrn.overflow()) { std::string desc = String::str(DTR("water column not deep enough for altitude control ( %0.2f < %0.2f )"), msg->alt + m_estate.depth, m_vertical_ref); setEntityState(IMC::EntityState::ESTA_NORMAL, desc); war("%s", desc.c_str()); m_btrack_wrn.reset(); } } else { m_btrack_wrn.reset(); } // Will not let the bottom follow depth be lower than zero // to avoid causing excessive controller integration m_bottom_follow_depth = std::max(m_bottom_follow_depth, (float)0.0); } else if (m_vertical_mode == VERTICAL_MODE_PITCH) { if (m_estate.depth >= m_max_depth - c_depth_margin) { const std::string desc = DTR("getting too close to maximum admissible depth"); setEntityState(IMC::EntityState::ESTA_ERROR, desc); err("%s", desc.c_str()); requestDeactivation(); return; } } // check if yaw control mode is valid if (m_yaw_mode >= YAW_MODE_SIZE) { signalBadYaw(DTR("invalid yaw control mode %d")); return; } else if (m_yaw_mode == YAW_MODE_NONE) { float delta = m_ymode_delta.getDelta(); if (delta > 0.0) m_ymode_wait += delta; if (m_ymode_wait > c_mode_timeout) { m_ymode_wait = 0.0; signalBadYaw(DTR("timed out waiting for yaw control mode")); } return; } // Compute time delta. float timestep = m_last_estate.getDelta(); // Check if we have a valid time delta. if (timestep < 0.0) return; onEstimatedState(timestep, msg); }
bool DOMDocumentTypeImpl::isEqualNode(const DOMNode* arg) const { if (isSameNode(arg)) { return true; } if (!fNode.isEqualNode(arg)) { return false; } DOMDocumentType* argDT = (DOMDocumentType*) arg; // check the string values if (!getPublicId()) { if (argDT->getPublicId()) { return false; } } else if (!XMLString::equals(getPublicId(), argDT->getPublicId())) { return false; } if (!getSystemId()) { if (argDT->getSystemId()) { return false; } } else if (!XMLString::equals(getSystemId(), argDT->getSystemId())) { return false; } if (!getInternalSubset()) { if (argDT->getInternalSubset()) { return false; } } else if (!XMLString::equals(getInternalSubset(), argDT->getInternalSubset())) { return false; } // check the notations if (getNotations()) { if (!argDT->getNotations()) return false; DOMNamedNodeMap* map1 = getNotations(); DOMNamedNodeMap* map2 = argDT->getNotations(); XMLSize_t len = map1->getLength(); if (len != map2->getLength()) { return false; } for (XMLSize_t i = 0; i < len; i++) { DOMNode* n1 = map1->item(i); DOMNode* n2 = map2->getNamedItem(n1->getNodeName()); if (!n2 || !n1->isEqualNode(n2)) { return false; } } } else { if (argDT->getNotations()) return false; } // check the entities if (getEntities()) { if (!argDT->getEntities()) return false; DOMNamedNodeMap* map1 = getEntities(); DOMNamedNodeMap* map2 = argDT->getEntities(); XMLSize_t len = map1->getLength(); if (len != map2->getLength()) { return false; } for (XMLSize_t i = 0; i < len; i++) { DOMNode* n1 = map1->item(i); DOMNode* n2 = map2->getNamedItem(n1->getNodeName()); if (!n2 || !n1->isEqualNode(n2)) { return false; } } } else { if (argDT->getEntities()) return false; } return fParent.isEqualNode(arg); }