/** ****************************************************************************** * * @file uavobjectutilmanager.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @see The GNU Public License (GPL) Version 3 * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup UAVObjectUtilPlugin UAVObjectUtil Plugin * @{ * @brief The UAVUObjectUtil GCS plugin *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "uavobjectutilmanager.h" #include "utils/homelocationutil.h" #include #include #include #include #include #include "firmwareiapobj.h" #include "homelocation.h" #include "gpspositionsensor.h" // ****************************** // constructor/destructor UAVObjectUtilManager::UAVObjectUtilManager() { mutex = new QMutex(QMutex::Recursive); saveState = IDLE; failureTimer.stop(); failureTimer.setSingleShot(true); failureTimer.setInterval(1000); connect(&failureTimer, SIGNAL(timeout()), this, SLOT(objectPersistenceOperationFailed())); pm = NULL; obm = NULL; obum = NULL; pm = ExtensionSystem::PluginManager::instance(); if (pm) { obm = pm->getObject(); obum = pm->getObject(); } } UAVObjectUtilManager::~UAVObjectUtilManager() { // while (!queue.isEmpty()) {} disconnect(); if (mutex) { delete mutex; mutex = NULL; } } UAVObjectManager *UAVObjectUtilManager::getObjectManager() { Q_ASSERT(obm); return obm; } // ****************************** // SD card saving // /* Add a new object to save in the queue */ void UAVObjectUtilManager::saveObjectToSD(UAVObject *obj) { // Add to queue queue.enqueue(obj); qDebug() << "Enqueue object: " << obj->getName(); // If queue length is one, then start sending (call sendNextObject) // Otherwise, do nothing, it's sending anyway if (queue.length() == 1) { saveNextObject(); } } void UAVObjectUtilManager::saveNextObject() { if (queue.isEmpty()) { return; } Q_ASSERT(saveState == IDLE); // Get next object from the queue UAVObject *obj = queue.head(); qDebug() << "Send save object request to board " << obj->getName(); ObjectPersistence *objper = dynamic_cast(getObjectManager()->getObject(ObjectPersistence::NAME)); connect(objper, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); connect(objper, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(objectPersistenceUpdated(UAVObject *))); saveState = AWAITING_ACK; if (obj != NULL) { ObjectPersistence::DataFields data; data.Operation = ObjectPersistence::OPERATION_SAVE; data.Selection = ObjectPersistence::SELECTION_SINGLEOBJECT; data.ObjectID = obj->getObjID(); data.InstanceID = obj->getInstID(); objper->setData(data); objper->updated(); } // Now: we are going to get two "objectUpdated" messages (one coming from GCS, one coming from Flight, which // will confirm the object was properly received by both sides) and then one "transactionCompleted" indicating // that the Flight side did not only receive the object but it did receive it without error. Last we will get // a last "objectUpdated" message coming from flight side, where we'll get the results of the objectPersistence // operation we asked for (saved, other). } /** * @brief Process the transactionCompleted message from Telemetry indicating request sent successfully * @param[in] The object just transsacted. Must be ObjectPersistance * @param[in] success Indicates that the transaction did not time out * * After a failed transaction (usually timeout) resends the save request. After a succesful * transaction will then wait for a save completed update from the autopilot. */ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject *obj, bool success) { if (success) { Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0); Q_ASSERT(saveState == AWAITING_ACK); // Two things can happen: // Either the Object Save Request did actually go through, and then we should get in // "AWAITING_COMPLETED" mode, or the Object Save Request did _not_ go through, for example // because the object does not exist and then we will never get a subsequent update. // For this reason, we will arm a 1 second timer to make provision for this and not block // the queue: saveState = AWAITING_COMPLETED; disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(objectPersistenceTransactionCompleted(UAVObject *, bool))); failureTimer.start(2000); // Create a timeout } else { // Can be caused by timeout errors on sending. Forget it and send next. qDebug() << "objectPersistenceTranscationCompleted (error)"; UAVObject *obj = getObjectManager()->getObject(ObjectPersistence::NAME); obj->disconnect(this); queue.dequeue(); // We can now remove the object, it failed anyway. saveState = IDLE; emit saveCompleted(obj->getField("ObjectID")->getValue().toInt(), false); saveNextObject(); } } /** * @brief Object persistence operation failed, i.e. we never got an update * from the board saying "completed". */ void UAVObjectUtilManager::objectPersistenceOperationFailed() { if (saveState == AWAITING_COMPLETED) { // TODO: some warning that this operation failed somehow // We have to disconnect the object persistence 'updated' signal // and ask to save the next object: ObjectPersistence *objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objectPersistence); UAVObject *obj = queue.dequeue(); // We can now remove the object, it failed anyway. Q_ASSERT(obj); objectPersistence->disconnect(this); saveState = IDLE; emit saveCompleted(obj->getObjID(), false); saveNextObject(); } } /** * @brief Process the ObjectPersistence updated message to confirm the right object saved * then requests next object be saved. * @param[in] The object just received. Must be ObjectPersistance */ void UAVObjectUtilManager::objectPersistenceUpdated(UAVObject *obj) { Q_ASSERT(obj); Q_ASSERT(obj->getObjID() == ObjectPersistence::OBJID); ObjectPersistence::DataFields objectPersistence = ((ObjectPersistence *)obj)->getData(); if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_ERROR) { failureTimer.stop(); objectPersistenceOperationFailed(); } else if (saveState == AWAITING_COMPLETED && objectPersistence.Operation == ObjectPersistence::OPERATION_COMPLETED) { failureTimer.stop(); // Check right object saved UAVObject *savingObj = queue.head(); if (objectPersistence.ObjectID != savingObj->getObjID()) { objectPersistenceOperationFailed(); return; } obj->disconnect(this); queue.dequeue(); // We can now remove the object, it's done. saveState = IDLE; emit saveCompleted(objectPersistence.ObjectID, true); saveNextObject(); } } /** * Helper function that makes sure FirmwareIAP is updated and then returns the data */ FirmwareIAPObj::DataFields UAVObjectUtilManager::getFirmwareIap() { FirmwareIAPObj::DataFields dummy; FirmwareIAPObj *firmwareIap = FirmwareIAPObj::GetInstance(obm); Q_ASSERT(firmwareIap); if (!firmwareIap) { return dummy; } return firmwareIap->getData(); } /** * Get the UAV Board model, for anyone interested. Return format is: * (Board Type << 8) + BoardRevision. */ int UAVObjectUtilManager::getBoardModel() { FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); qDebug() << "Board type=" << firmwareIapData.BoardType; qDebug() << "Board revision=" << firmwareIapData.BoardRevision; int ret = firmwareIapData.BoardType << 8; ret = ret + firmwareIapData.BoardRevision; qDebug() << "Board info=" << ret; return ret; } /** * Get the UAV Board CPU Serial Number, for anyone interested. Return format is a byte array */ QByteArray UAVObjectUtilManager::getBoardCPUSerial() { QByteArray cpuSerial; FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); for (unsigned int i = 0; i < FirmwareIAPObj::CPUSERIAL_NUMELEM; i++) { cpuSerial.append(firmwareIapData.CPUSerial[i]); } return cpuSerial; } quint32 UAVObjectUtilManager::getFirmwareCRC() { FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); return firmwareIapData.crc; } /** * Get the UAV Board Description, for anyone interested. */ QByteArray UAVObjectUtilManager::getBoardDescription() { QByteArray ret; FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); for (unsigned int i = 0; i < FirmwareIAPObj::DESCRIPTION_NUMELEM; i++) { ret.append(firmwareIapData.Description[i]); } return ret; } // ****************************** // HomeLocation int UAVObjectUtilManager::setHomeLocation(double LLA[3], bool save_to_sdcard) { double Be[3]; int result = Utils::HomeLocationUtil().getDetails(LLA, Be); Q_ASSERT(result == 0); // save the new settings HomeLocation *homeLocation = HomeLocation::GetInstance(obm); Q_ASSERT(homeLocation != NULL); HomeLocation::DataFields homeLocationData = homeLocation->getData(); homeLocationData.Latitude = LLA[0] * 1e7; homeLocationData.Longitude = LLA[1] * 1e7; homeLocationData.Altitude = LLA[2]; homeLocationData.Be[0] = Be[0]; homeLocationData.Be[1] = Be[1]; homeLocationData.Be[2] = Be[2]; homeLocationData.Set = HomeLocation::SET_TRUE; homeLocation->setData(homeLocationData); if (save_to_sdcard) { saveObjectToSD(homeLocation); } return 0; } int UAVObjectUtilManager::getHomeLocation(bool &set, double LLA[3]) { HomeLocation *homeLocation = HomeLocation::GetInstance(obm); Q_ASSERT(homeLocation != NULL); HomeLocation::DataFields homeLocationData = homeLocation->getData(); set = homeLocationData.Set; LLA[0] = homeLocationData.Latitude * 1e-7; LLA[1] = homeLocationData.Longitude * 1e-7; LLA[2] = homeLocationData.Altitude; if (LLA[0] != LLA[0]) { LLA[0] = 0; // nan detection } else if (LLA[0] > 90) { LLA[0] = 90; } else if (LLA[0] < -90) { LLA[0] = -90; } if (LLA[1] != LLA[1]) { LLA[1] = 0; // nan detection } else if (LLA[1] > 180) { LLA[1] = 180; } else if (LLA[1] < -180) { LLA[1] = -180; } if (LLA[2] != LLA[2]) { LLA[2] = 0; // nan detection } return 0; // OK } // ****************************** // GPS int UAVObjectUtilManager::getGPSPositionSensor(double LLA[3]) { GPSPositionSensor *gpsPosition = GPSPositionSensor::GetInstance(obm); Q_ASSERT(gpsPosition != NULL); GPSPositionSensor::DataFields gpsPositionData = gpsPosition->getData(); LLA[0] = gpsPositionData.Latitude; LLA[1] = gpsPositionData.Longitude; LLA[2] = gpsPositionData.Altitude; if (LLA[0] != LLA[0]) { LLA[0] = 0; // nan detection } else if (LLA[0] > 90) { LLA[0] = 90; } else if (LLA[0] < -90) { LLA[0] = -90; } if (LLA[1] != LLA[1]) { LLA[1] = 0; // nan detection } else if (LLA[1] > 180) { LLA[1] = 180; } else if (LLA[1] < -180) { LLA[1] = -180; } if (LLA[2] != LLA[2]) { LLA[2] = 0; // nan detection } return 0; // OK } deviceDescriptorStruct UAVObjectUtilManager::getBoardDescriptionStruct() { deviceDescriptorStruct ret; descriptionToStructure(getBoardDescription(), ret); return ret; } bool UAVObjectUtilManager::descriptionToStructure(QByteArray desc, deviceDescriptorStruct & struc) { if (desc.startsWith("OpFw")) { /* * This looks like a binary with a description at the end: * 4 bytes: header: "OpFw". * 4 bytes: GIT commit tag (short version of SHA1). * 4 bytes: Unix timestamp of compile time. * 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. * 26 bytes: commit tag if it is there, otherwise branch name. '-dirty' may be added if needed. Zero-padded. * 20 bytes: SHA1 sum of the firmware. * 20 bytes: SHA1 sum of the uavo definitions. * 20 bytes: free for now. */ // Note: the ARM binary is big-endian: quint32 gitCommitHash = desc.at(7) & 0xFF; for (int i = 1; i < 4; i++) { gitCommitHash = gitCommitHash << 8; gitCommitHash += desc.at(7 - i) & 0xFF; } struc.gitHash = QString("%1").arg(gitCommitHash, 8, 16, QChar('0')); quint32 gitDate = desc.at(11) & 0xFF; for (int i = 1; i < 4; i++) { gitDate = gitDate << 8; gitDate += desc.at(11 - i) & 0xFF; } struc.gitDate = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyyMMdd HH:mm"); QString gitTag = QString(desc.mid(14, 26)); struc.gitTag = gitTag; // TODO: check platform compatibility QByteArray targetPlatform = desc.mid(12, 2); struc.boardType = (int)targetPlatform.at(0); struc.boardRevision = (int)targetPlatform.at(1); struc.fwHash.clear(); struc.fwHash = desc.mid(40, 20); struc.uavoHash.clear(); struc.uavoHash = desc.mid(60, 20); qDebug() << __FUNCTION__ << ":description from board:"; foreach(char x, desc) { qDebug() << QString::number(x, 16); } qDebug() << __FUNCTION__ << ":uavoHash:"; QByteArray array2 = struc.uavoHash.data(); foreach(char x, array2) { qDebug() << QString::number(x, 16); } return true; } return false; } // ******************************