Merged mainline

autobuild.sh

@@ -28,7 +28,7 @@ do
 	elif [ $OPT = "grab_debian_dependencies" ] &> /dev/null
 	then
 		echo you chose to install debian dependencies
-		sudo apt-get install cmake libqt4-dev flite1-dev libphonon-dev libopenscenegraph-dev
+		sudo apt-get install cmake libqt4-dev flite1-dev libphonon-dev libopenscenegraph-dev libsdl1.2-dev
 		exit 0
 
 	elif [ $OPT = "remake" ] &> /dev/null

src/comm/MAVLinkProtocol.cc

@@ -182,6 +182,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
 
         if (decodeState == 1) {
+#ifdef MAVLINK_MESSAGE_LENGTHS
+	    const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
+	    if (message.msgid >= sizeof(message_lengths) ||
+		message.len != message_lengths[message.msgid]) {
+                    qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
+		    continue;
+	    }
+#endif
             // Log data
             if (m_loggingEnabled && m_logfile) {
                 const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);

src/comm/MAVLinkSimulationLink.cc

@@ -934,7 +934,7 @@ bool MAVLinkSimulationLink::connect()
     emit connected(true);
 
     start(LowPriority);
-    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 47.376, 8.548);
+    MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 37.480391, -122.282883);
     Q_UNUSED(mav1);
 //    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2, 47.375, 8.548, 1);
 //    Q_UNUSED(mav2);

src/comm/MAVLinkSimulationMAV.cc

@@ -23,13 +23,20 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
     yaw(0.0),
     globalNavigation(true),
     firstWP(false),
-    previousSPX(8.548056),
-    previousSPY(47.376389),
-    previousSPZ(550),
-    previousSPYaw(0.0),
-    nextSPX(8.548056),
-    nextSPY(47.376389),
-    nextSPZ(550),
+//    previousSPX(8.548056),
+//    previousSPY(47.376389),
+//    previousSPZ(550),
+//    previousSPYaw(0.0),
+//    nextSPX(8.548056),
+//    nextSPY(47.376389),
+//    nextSPZ(550),
+        previousSPX(37.480391),
+        previousSPY(122.282883),
+        previousSPZ(550),
+        previousSPYaw(0.0),
+        nextSPX(37.480391),
+        nextSPY(122.282883),
+        nextSPZ(550),
     nextSPYaw(0.0),
     sys_mode(MAV_MODE_READY),
     sys_state(MAV_STATE_STANDBY),

src/comm/MAVLinkXMLParser.cc

@@ -90,6 +90,16 @@ bool MAVLinkXMLParser::generate()
 
     int mavlinkVersion = 0;
 
+    // we need to gather the message lengths across multiple includes,
+    // which we can do via detecting recursion
+    static unsigned message_lengths[256];
+    static int highest_message_id;
+    static int recursion_level;
+
+    if (recursion_level == 0) {
+        highest_message_id = 0;
+        memset(message_lengths, 0, sizeof(message_lengths));
+    }
 
 
     // Start main header
@@ -138,7 +148,9 @@ bool MAVLinkXMLParser::generate()
                                 MAVLinkXMLParser includeParser(incFilePath, topLevelOutputDirName, this);
                                 connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString)));
                                 // Generate and write
+                                recursion_level++;
                                 includeParser.generate();
+                                recursion_level--;
                                 mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n";
 
 
@@ -340,7 +352,7 @@ bool MAVLinkXMLParser::generate()
                                         QString decodeLines;
                                         QString sendArguments;
                                         QString commentLines;
-
+                                        unsigned message_length = 0;
 
 
                                         // Get the message fields
@@ -437,6 +449,40 @@ bool MAVLinkXMLParser::generate()
                                                 }
 
 
+                                                // message length calculation
+                                                unsigned element_multiplier = 1;
+                                                unsigned element_length = 0;
+                                                const struct {
+                                                    const char *prefix;
+                                                    unsigned length;
+                                                } length_map[] = {
+                                                        { "array",  1 },
+                                                        { "char",   1 },
+                                                        { "uint8",  1 },
+                                                        { "int8",   1 },
+                                                        { "uint16", 2 },
+                                                        { "int16",  2 },
+                                                        { "uint32", 4 },
+                                                        { "int32",  4 },
+                                                        { "uint64", 8 },
+                                                        { "int64",  8 },
+                                                        { "float",  4 },
+                                                        { "double", 8 },
+                                                };
+                                                if (fieldType.contains("[")) {
+                                                    element_multiplier = fieldType.split("[").at(1).split("]").first().toInt();
+                                                }
+                                                for (unsigned i=0; i<sizeof(length_map)/sizeof(length_map[0]); i++) {
+                                                    if (fieldType.startsWith(length_map[i].prefix)) {
+                                                        element_length = length_map[i].length * element_multiplier;
+                                                        break;
+                                                    }
+                                                }
+                                                if (element_length == 0) {
+                                                    emit parseState(tr("<font color=\"red\">ERROR: Unable to calculate length for %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), fieldType));
+                                                }
+                                                message_length += element_length;
+
                                                 //
                                                 //                                                QString unpackingCode;
 
@@ -489,6 +535,11 @@ bool MAVLinkXMLParser::generate()
                                             f = f.nextSibling();
                                         }
 
+                                        if (messageId > highest_message_id) {
+                                            highest_message_id = messageId;
+                                        }
+                                        message_lengths[messageId] = message_length;
+
                                         cStruct = cStruct.arg(cStructName, cStructLines);
                                         lcmStructDefs.append("\n").append(cStruct).append("\n");
                                         pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines);
@@ -540,6 +591,15 @@ bool MAVLinkXMLParser::generate()
         mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first);
     }
 
+    mainHeader += "\n\n// MESSAGE LENGTHS\n\n";
+    mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n";
+    mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { ";
+    for (int i=0; i<highest_message_id; i++) {
+        mainHeader += QString::number(message_lengths[i]);
+        if (i < highest_message_id-1) mainHeader += ", ";
+    }
+    mainHeader += " }\n\n";
+
     mainHeader += "#ifdef __cplusplus\n}\n#endif\n";
     mainHeader += "#endif";
     // Newline to make compiler happy

src/uas/UAS.cc

@@ -535,7 +535,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         {
             mavlink_global_position_int_t pos;
             mavlink_msg_global_position_int_decode(&message, &pos);
-            quint64 time = QGC::groundTimeUsecs()/1000;
+            quint64 time = getUnixTime();
             latitude = pos.lat/(double)1E7;
             longitude = pos.lon/(double)1E7;
             altitude = pos.alt/1000.0;
@@ -562,7 +562,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_GLOBAL_POSITION: {
             mavlink_global_position_t pos;
             mavlink_msg_global_position_decode(&message, &pos);
-            quint64 time = QGC::groundTimeUsecs()/1000;
+            quint64 time = getUnixTime();
             latitude = pos.lat;
             longitude = pos.lon;
             altitude = pos.alt;
@@ -845,7 +845,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
             mavlink_servo_output_raw_t servos;
             mavlink_msg_servo_output_raw_decode(&message, &servos);
-            quint64 time = getUnixTime(0);
+            quint64 time = getUnixTime();
             emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time);
             emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time);
             emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time);
@@ -939,7 +939,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         case MAVLINK_MSG_ID_NAV_FILTER_BIAS: {
             mavlink_nav_filter_bias_t bias;
             mavlink_msg_nav_filter_bias_decode(&message, &bias);
-            quint64 time = MG::TIME::getGroundTimeNow();
+            quint64 time = getUnixTime();
             emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time);
             emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time);
             emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time);
@@ -985,7 +985,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             if (!unknownPackets.contains(message.msgid)) {
                 unknownPackets.append(message.msgid);
                 QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid);
-                GAudioOutput::instance()->say(errString+tr(", please check the communication console for details."));
+                GAudioOutput::instance()->say(errString+tr(", please check console for details."));
                 emit textMessageReceived(uasId, message.compid, 255, errString);
                 std::cout << "Unable to decode message from system " << std::dec << static_cast<int>(message.sysid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
                 //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
@@ -1119,7 +1119,8 @@ void UAS::startPressureCalibration()
 quint64 UAS::getUnixTime(quint64 time)
 {
     if (time == 0) {
-        return MG::TIME::getGroundTimeNow();
+//        qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
+        return QGC::groundTimeMilliseconds();
     }
     // Check if time is smaller than 40 years,
     // assuming no system without Unix timestamp
@@ -1143,8 +1144,9 @@ quint64 UAS::getUnixTime(quint64 time)
     else if (time < 1261440000000000)
 #endif
     {
+//        qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
         if (onboardTimeOffset == 0) {
-            onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000;
+            onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
         }
         return time/1000 + onboardTimeOffset;
     } else {

src/ui/QGCParamWidget.cc

@@ -416,6 +416,7 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
  */
 void QGCParamWidget::requestParameterList()
 {
+    if (!mav) return;
     // FIXME This call does not belong here
     // Once the comm handling is moved to a new
     // Param manager class the settings can be directly
@@ -487,6 +488,7 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)
 
 void QGCParamWidget::saveParameters()
 {
+    if (!mav) return;
     QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./parameters.txt", tr("Parameter File (*.txt)"));
     QFile file(fileName);
     if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
@@ -520,6 +522,7 @@ void QGCParamWidget::saveParameters()
 
 void QGCParamWidget::loadParameters()
 {
+    if (!mav) return;
     QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Parameter file (*.txt)"));
     QFile file(fileName);
     if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
@@ -749,11 +752,13 @@ void QGCParamWidget::setParameters()
  */
 void QGCParamWidget::writeParameters()
 {
+    if (!mav) return;
     mav->writeParametersToStorage();
 }
 
 void QGCParamWidget::readParameters()
 {
+    if (!mav) return;
     mav->readParametersFromStorage();
 }
 

src/ui/WaypointView.cc

@@ -203,17 +203,17 @@ void WaypointView::updateActionView(int action)
         m_ui->orbitSpinBox->show();
         m_ui->holdTimeSpinBox->show();
         break;
-    case MAV_CMD_NAV_ORIENTATION_TARGET:
-        m_ui->orbitSpinBox->hide();
-        m_ui->takeOffAngleSpinBox->hide();
-        m_ui->turnsSpinBox->hide();
-        m_ui->holdTimeSpinBox->show();
-        m_ui->customActionWidget->hide();
-
-        m_ui->autoContinue->show();
-        m_ui->acceptanceSpinBox->hide();
-        m_ui->yawSpinBox->hide();
-        break;
+//    case MAV_CMD_NAV_ORIENTATION_TARGET:
+//        m_ui->orbitSpinBox->hide();
+//        m_ui->takeOffAngleSpinBox->hide();
+//        m_ui->turnsSpinBox->hide();
+//        m_ui->holdTimeSpinBox->show();
+//        m_ui->customActionWidget->hide();
+
+//        m_ui->autoContinue->show();
+//        m_ui->acceptanceSpinBox->hide();
+//        m_ui->yawSpinBox->hide();
+//        break;
     default:
         break;
     }