Working on HIL

src/comm/QGCFlightGearLink.cc

@@ -262,11 +262,17 @@ bool QGCFlightGearLink::disconnectSimulation()
     disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
     disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
 
-    process->close();
-    delete process;
-    process = NULL;
-    delete socket;
-    socket = NULL;
+    if (process)
+    {
+        process->close();
+        delete process;
+        process = NULL;
+    }
+    if (socket)
+    {
+        delete socket;
+        socket = NULL;
+    }
 
     connectState = false;
 
@@ -288,15 +294,15 @@ bool QGCFlightGearLink::connectSimulation()
 
     QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
 
-    process = new QProcess(this);
+//    process = new QProcess(this);
 
-    connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+//    connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
+//    connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
 
-    //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
-    // Catch process error
-    QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
-                      this, SLOT(processError(QProcess::ProcessError)));
+//    //connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
+//    // Catch process error
+//    QObject::connect( process, SIGNAL(error(QProcess::ProcessError)),
+//                      this, SLOT(processError(QProcess::ProcessError)));
     // Start Flightgear
     QStringList processCall;
     QString processFgfs;
@@ -352,16 +358,18 @@ bool QGCFlightGearLink::connectSimulation()
     // Add new argument with this: processCall << "";
     processCall << QString("--aircraft=%2").arg(aircraft);
 
-    process->start(processFgfs, processCall);
+//    process->start(processFgfs, processCall);
 
-    qDebug() << "STARTING: " << processFgfs << processCall;
 
-    emit flightGearConnected(connectState);
-    if (connectState) {
-        emit flightGearConnected();
-        connectionStartTime = QGC::groundTimeUsecs()/1000;
-    }
-    qDebug() << "STARTING SIM";
+
+//    emit flightGearConnected(connectState);
+//    if (connectState) {
+//        emit flightGearConnected();
+//        connectionStartTime = QGC::groundTimeUsecs()/1000;
+//    }
+//    qDebug() << "STARTING SIM";
+
+        qDebug() << "STARTING: " << processFgfs << processCall;
 
     start(LowPriority);
     return connectState;

src/configuration.h

@@ -12,7 +12,7 @@
 #define WITH_TEXT_TO_SPEECH 1
 
 #define QGC_APPLICATION_NAME "QGroundControl"
-#define QGC_APPLICATION_VERSION "v. 1.0.0 (Alpha RC17)"
+#define QGC_APPLICATION_VERSION "v. 1.0.0 (Alpha RC18)"
 
 namespace QGC
 

src/uas/UAS.cc

@@ -337,10 +337,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 mavlink_msg_sys_status_decode(&message, &state);
 
                 emit loadChanged(this,state.load/10.0f);
-                // FIXME REMOVE LATER emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime());
+
+
 
                 currentVoltage = state.voltage_battery/1000.0f;
                 lpVoltage = filterVoltage(currentVoltage);
+
+                qDebug() << "BATT VOLTAGE:" << currentVoltage << "RAW" << state.voltage_battery;
+
                 if (startVoltage == 0) startVoltage = currentVoltage;
                 timeRemaining = calculateTimeRemaining();
                 if (!batteryRemainingEstimateEnabled && chargeLevel != -1)
@@ -1194,18 +1198,11 @@ QList<int> UAS::getComponentIds()
 
 void UAS::setMode(int mode)
 {
-    if (mode >= (int)MAV_MODE_PREFLIGHT && mode < (int)MAV_MODE_ENUM_END)
-    {
-        //this->mode = mode; //no call assignament, update receive message from UAS
-        mavlink_message_t msg;
-        mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode);
-        sendMessage(msg);
-        qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
-    }
-    else
-    {
-        qDebug() << "uas Mode not assign: " << mode;
-    }
+    //this->mode = mode; //no call assignament, update receive message from UAS
+    mavlink_message_t msg;
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode);
+    sendMessage(msg);
+    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
 }
 
 void UAS::sendMessage(mavlink_message_t message)
@@ -1923,7 +1920,8 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
     manualYawAngle = yaw * yawScaling;
     manualThrust = thrust * thrustScaling;
 
-    if(mode == (int)MAV_MODE_MANUAL_ARMED)
+    // If system has manual inputs enabled and is armed
+    if((mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY))
     {
         mavlink_message_t message;
         mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
@@ -2083,7 +2081,7 @@ void UAS::startHil()
     // Connect Flight Gear Link
     simulation->connectSimulation();
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode, navMode | MAV_MODE_FLAG_HIL_ENABLED);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
     sendMessage(msg);
 }
 
@@ -2091,7 +2089,7 @@ void UAS::stopHil()
 {
     simulation->disconnectSimulation();
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode, navMode & !MAV_MODE_FLAG_HIL_ENABLED);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
     sendMessage(msg);
 }
 
@@ -2153,44 +2151,50 @@ const QString& UAS::getShortState() const
 QString UAS::getShortModeTextFor(int id)
 {
     QString mode;
+    uint8_t modeid = id;
+
+    qDebug() << "MODE:" << modeid;
 
-    switch (id) {
-    case (uint8_t)MAV_MODE_PREFLIGHT:
+    // BASE MODE DECODING
+    if (modeid & MAV_MODE_FLAG_DECODE_POSITION_AUTO)
+    {
+        mode = "AUTO";
+    }
+    else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_GUIDED)
+    {
+        mode = "GUIDED";
+    }
+    else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_STABILIZE)
+    {
+        mode = "STABILIZED";
+    }
+    else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_TEST)
+    {
+        mode = "TEST";
+    }
+    else if (modeid & MAV_MODE_FLAG_DECODE_POSITION_MANUAL)
+    {
+        mode = "MANUAL";
+    }
+    else
+    {
         mode = "PREFLIGHT";
-        break;
-    case (uint8_t)MAV_MODE_MANUAL_ARMED:
-        mode = "A|MANUAL";
-        break;
-    case (uint8_t)MAV_MODE_MANUAL_DISARMED:
-        mode = "D|MANUAL";
-        break;
-    case (uint8_t)MAV_MODE_AUTO_ARMED:
-        mode = "A|AUTO";
-        break;
-    case (uint8_t)MAV_MODE_AUTO_DISARMED:
-        mode = "D|AUTO";
-        break;
-    case (uint8_t)MAV_MODE_GUIDED_ARMED:
-        mode = "A|GUIDED";
-        break;
-    case (uint8_t)MAV_MODE_GUIDED_DISARMED:
-        mode = "D|GUIDED";
-        break;
-    case (uint8_t)MAV_MODE_STABILIZE_ARMED:
-        mode = "A|STABILIZED";
-        break;
-    case (uint8_t)MAV_MODE_STABILIZE_DISARMED:
-        mode = "D|STABILIZED";
-        break;
-    case (uint8_t)MAV_MODE_TEST_ARMED:
-        mode = "A|TEST";
-        break;
-    case (uint8_t)MAV_MODE_TEST_DISARMED:
-        mode = "D|TEST";
-        break;
-    default:
-        mode = "UNKNOWN";
-        break;
+    }
+
+    // ARMED STATE DECODING
+    if (modeid & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)
+    {
+        mode.prepend("A|");
+    }
+    else
+    {
+        mode.prepend("D|");
+    }
+
+    // HARDWARE IN THE LOOP DECODING
+    if (modeid & MAV_MODE_FLAG_DECODE_POSITION_HIL)
+    {
+        mode.prepend("HIL:");
     }
 
     return mode;

src/uas/UAS.h

@@ -165,7 +165,7 @@ protected: //COMMENTS FOR TEST UNIT
     bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life
     float chargeLevel;          ///< Charge level of battery, in percent
     int timeRemaining;          ///< Remaining time calculated based on previous and current
-    int mode;                   ///< The current mode of the MAV
+    uint8_t mode;                   ///< The current mode of the MAV
     int status;                 ///< The current status of the MAV
     uint32_t navMode;                ///< The current navigation mode of the MAV
     quint64 onboardTimeOffset;