Updated sensor read pool

src/uas/UAS.cc

@@ -62,7 +62,9 @@ UAS::UAS(int id=0) :
         manualRollAngle(0),
         manualPitchAngle(0),
         manualYawAngle(0),
-        manualThrust(0)
+        manualThrust(0),
+        receiveDropRate(0),
+        sendDropRate(0)
 {
     uasId = id;
     setBattery(LIPOLY, 3);
@@ -173,7 +175,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit batteryChanged(this, filterVoltage(), getChargeLevel(), timeRemaining);
                 emit voltageChanged(message.sysid, state.vbat/1000.0f);
 
-                // Output audio
+                // COMMUNICATIONS DROP RATE
+                emit dropRateChanged(this->getUASID(), this->receiveDropRate, this->sendDropRate);
+
+                // AUDIO
                 if (modechanged && statechanged)
                 {
                     // Output both messages
@@ -195,6 +200,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 }
             }
             break;
+        case MAVLINK_MSG_ID_AUX_STATUS:
+        {
+            aux_status_t status;
+            message_aux_status_decode(&message, &status);
+            emit loadChanged(this, status.load/100.0f);
+            emit valueChanged(this, "Load", status.load/1000.0f, MG::TIME::getGroundTimeNow());
+        }
+        break;
         case MAVLINK_MSG_ID_RAW_IMU:
             {
                 raw_imu_t raw;
@@ -224,10 +237,43 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
             }
             break;
+        case MAVLINK_MSG_ID_RAW_SENSOR:
+            {
+                raw_sensor_t raw;
+                message_raw_imu_decode(&message, &raw);
+                quint64 time = raw.msec;
+                if (time == 0)
+                {
+                    time = MG::TIME::getGroundTimeNow();
+                }
+                else
+                {
+                    if (onboardTimeOffset == 0)
+                    {
+                        onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
+                    }
+                    time += onboardTimeOffset;
+                }
+
+                emit valueChanged(uasId, "Accel. X", raw.xacc, time);
+                emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
+                emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
+                emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
+                emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
+                emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
+                emit valueChanged(uasId, "Mag. X", raw.xmag, time);
+                emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
+                emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
+                emit valueChanged(uasId, "Pressure", raw.baro, time);
+                emit valueChanged(uasId, "Temperature", raw.baro, time);
+            }
+            break;
         case MAVLINK_MSG_ID_ATTITUDE:
             //std::cerr << std::endl;
             //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << message_attitude_get_roll(message.payload) << " pitch: " << message_attitude_get_pitch(message.payload) << " yaw: " << message_attitude_get_yaw(message.payload) << std::endl;
             {
+                attitude_t attitude;
+                message_attitude_decode(&message, &attitude);
                 quint64 time = message_attitude_get_msec(&message);
                 if (time == 0)
                 {
@@ -247,6 +293,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(this, "roll IMU", message_attitude_get_roll(&message), time);
                 emit valueChanged(this, "pitch IMU", message_attitude_get_pitch(&message), time);
                 emit valueChanged(this, "yaw IMU", message_attitude_get_yaw(&message), time);
+                emit valueChanged(uasId, "rollspeed IMU", attitude.rollspeed, time);
+                emit valueChanged(uasId, "pitchspeed IMU", attitude.pitchspeed, time);
+                emit valueChanged(uasId, "yawspeed IMU", attitude.yawspeed, time);
                 emit attitudeChanged(this, message_attitude_get_roll(&message), message_attitude_get_pitch(&message), message_attitude_get_yaw(&message), time);
             }
             break;

src/uas/UAS.h

@@ -118,6 +118,8 @@ protected:
     double manualPitchAngle;    ///< Pitch angle set by human pilot (radians)
     double manualYawAngle;      ///< Yaw angle set by human pilot (radians)
     double manualThrust;        ///< Thrust set by human pilot (radians)
+    float receiveDropRate;      ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
+    float sendDropRate;         ///< Percentage of packets that were not received from the MAV by the GCS
 
     /** @brief Set the current battery type */
     void setBattery(BatteryType type, int cells);

src/uas/UASInterface.h

@@ -47,7 +47,8 @@ This file is part of the PIXHAWK project
  * This interface is abstract and thus cannot be instantiated. It serves only as type definition.
  * It represents an unmanned aerial vehicle, e.g. a micro air vehicle.
  **/
-class UASInterface : public QObject {
+class UASInterface : public QObject
+{
     Q_OBJECT
 public:
     UASInterface() :
@@ -207,6 +208,14 @@ signals:
      * @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
      */
     void statusChanged(UASInterface* uas, QString status, QString description);
+    /**
+     * @brief Drop rate of communication link updated
+     *
+     * @param systemId id of the air system
+     * @param receiveDrop drop rate of packets this MAV receives (send from GCS or other MAVs)
+     * @param sendDrop drop rate of packets this MAV sends (received on GCS)
+     */
+    void dropRateChanged(int systemId,  float receiveDrop, float sendDrop);
     /** @brief Robot mode has changed */
     void modeChanged(int sysId, QString status, QString description);
     /** @brief A command has been issued **/

src/uas/UASManager.cc

@@ -91,7 +91,7 @@ UASInterface* UASManager::getActiveUAS()
     if(!activeUAS)
     {
         QMessageBox msgBox;
-        msgBox.setText(tr("No Unmanned System loaded. Please add one first."));
+        msgBox.setText(tr("No Micro Air Vehicle connected. Please connect one first."));
         msgBox.exec();
     }
     return activeUAS; ///< Return zero pointer if no UAS has been loaded

src/ui/MainWindow.cc

@@ -313,7 +313,7 @@ void MainWindow::addLink()
 void MainWindow::UASCreated(UASInterface* uas)
 {
     // Connect the UAS to the full user interface
-    ui.menuConnected_Systems->addAction(QIcon(":/images/mavs/generic.svg"), tr("View ") + uas->getUASName(), uas, SLOT(setSelected()));
+    //ui.menuConnected_Systems->addAction(QIcon(":/images/mavs/generic.svg"), tr("View ") + uas->getUASName(), uas, SLOT(setSelected()));
 
     // Line chart
     // FIXME DO THIS ONLY FOR THE FIRST CONNECTED SYSTEM

src/ui/MainWindow.ui

@@ -35,7 +35,7 @@
      <x>0</x>
      <y>0</y>
      <width>1000</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuMGround">
@@ -43,7 +43,6 @@
      <string>File</string>
     </property>
     <addaction name="actionJoystickSettings"/>
-    <addaction name="actionSettings"/>
     <addaction name="separator"/>
     <addaction name="actionExit"/>
    </widget>
@@ -65,11 +64,6 @@
     <addaction name="actionAdd_Link"/>
     <addaction name="separator"/>
    </widget>
-   <widget class="QMenu" name="menuConnected_Systems">
-    <property name="title">
-     <string>Systems</string>
-    </property>
-   </widget>
    <widget class="QMenu" name="menuWindow">
     <property name="title">
      <string>Window</string>
@@ -84,7 +78,6 @@
    <addaction name="menuMGround"/>
    <addaction name="menuNetwork"/>
    <addaction name="menuUnmanned_System"/>
-   <addaction name="menuConnected_Systems"/>
    <addaction name="menuWindow"/>
   </widget>
   <widget class="QToolBar" name="mainToolBar">

src/ui/uas/UASInfoWidget.cc

 /*=====================================================================
- 
+
 PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
- 
+
 (c) 2009 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
- 
+
 This file is part of the PIXHAWK project
- 
+
     PIXHAWK 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.
- 
+
     PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
- 
+
 ======================================================================*/
- 
+
 /**
  * @file
  *   @brief Brief Description
@@ -83,14 +83,14 @@ void UASInfoWidget::addUAS(UASInterface* uas)
 {
     if (uas != NULL)
     {
-    //    connect(uas, SIGNAL(voltageChanged(int, double)), this, SLOT(setVoltage(int, double)));
-    connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int)));
-    connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(valueChanged(int,QString,double,quint64)));
-    connect(uas, SIGNAL(actuatorChanged(UASInterface*,int,double)), this, SLOT(actuatorChanged(UASInterface*,int,double)));
-    connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double)));
-
-    // Set this UAS as active if it is the first one
-    if (activeUAS == 0) activeUAS = uas;
+        //    connect(uas, SIGNAL(voltageChanged(int, double)), this, SLOT(setVoltage(int, double)));
+        connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int)));
+        connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(valueChanged(int,QString,double,quint64)));
+        connect(uas, SIGNAL(actuatorChanged(UASInterface*,int,double)), this, SLOT(actuatorChanged(UASInterface*,int,double)));
+        connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double)));
+
+        // Set this UAS as active if it is the first one
+        if (activeUAS == 0) activeUAS = uas;
     }
 }
 
@@ -99,16 +99,7 @@ void UASInfoWidget::setActiveUAS(UASInterface* uas)
     activeUAS = uas;
 }
 
-void UASInfoWidget::addInstrument(QString key, double min, double max, double initial, QString unit)
-{
-
-}
-
-void UASInfoWidget::valueChanged(int uasid, QString key, double value,quint64 time)
-{
-
-}
-
+/*
 void UASInfoWidget::actuatorChanged(UASInterface* uas, int actId, double value)
 {
     if (activeUAS == uas)
@@ -133,7 +124,7 @@ void UASInfoWidget::actuatorChanged(UASInterface* uas, int actId, double value)
             break;
         }
     }
-}
+}*/
 
 void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
 {
@@ -142,14 +133,23 @@ void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double perc
     setTimeRemaining(uas, seconds);
 }
 
+/**
+ *
+ */
 void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
 {
     if (activeUAS == uas)
     {
-        this->load = load;
+        this->load = load*100.0f;
     }
 }
 
+void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop)
+{
+    ui.receiveLossBar->setValue(receiveDrop * 100.0f);
+    ui.sendLossBar->setValue(sendDrop * 100.0f);
+}
+
 //void UASInfoWidget::setBattery(int uasid, BatteryType type, int cells)
 //{
 //    this->batteryType = type;
@@ -230,19 +230,19 @@ void UASInfoWidget::refresh()
     ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals));
     ui.loadBar->setValue(static_cast<int>(this->load));
 
-//    if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME)
-//    {
-//        // Filter output to get a higher stability
-//        static int filterTime = static_cast<int>(this->timeRemaining);
-//        //filterTime = 0.8 * filterTime + 0.2 * static_cast<int>(this->timeRemaining);
-//
-//        int hours = filterTime % (60 * 60);
-//        int min = (filterTime - hours * 60) % 60;
-//        int sec = (filterTime - hours * 60 - min * 60);
-//        QString timeText;
-//        timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
-//        ui.voltageTimeEstimateLabel->setText(timeText);
-//    } else {
-//        ui.voltageTimeEstimateLabel->setText(tr("Calculating"));
-//    }
+    //    if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME)
+    //    {
+    //        // Filter output to get a higher stability
+    //        static int filterTime = static_cast<int>(this->timeRemaining);
+    //        //filterTime = 0.8 * filterTime + 0.2 * static_cast<int>(this->timeRemaining);
+    //
+    //        int hours = filterTime % (60 * 60);
+    //        int min = (filterTime - hours * 60) % 60;
+    //        int sec = (filterTime - hours * 60 - min * 60);
+    //        QString timeText;
+    //        timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
+    //        ui.voltageTimeEstimateLabel->setText(timeText);
+    //    } else {
+    //        ui.voltageTimeEstimateLabel->setText(tr("Calculating"));
+    //    }
 }

src/ui/uas/UASInfoWidget.h

@@ -49,15 +49,6 @@ public:
     UASInfoWidget(QWidget *parent = 0, QString name = "");
     ~UASInfoWidget();
 
-//    enum BatteryType {
-//        NICD = 0,
-//        NIMH = 1,
-//        LIION = 2,
-//        LIPOLY = 3,
-//        LIFE = 4,
-//        AGZN = 5
-//           };
-
 public slots:
     void addUAS(UASInterface* uas);
 
@@ -65,16 +56,15 @@ public slots:
 
     void updateBattery(UASInterface* uas, double voltage, double percent, int seconds);
     void updateCPULoad(UASInterface* uas, double load);
+    void updateDropRate(int sysId, float receiveDrop, float sendDrop);
 
     void setVoltage(UASInterface* uas, double voltage);
     void setChargeLevel(UASInterface* uas, double chargeLevel);
     void setTimeRemaining(UASInterface* uas, double seconds);
 //    void setBattery(int uasid, BatteryType type, int cells);
 
-    void addInstrument(QString key, double min, double max, double initial, QString unit="");
-    void valueChanged(int uasid, QString key, double value,quint64 time);
-    void actuatorChanged(UASInterface* uas, int actId, double value);
-//    double calculateTimeRemaining();
+//    void valueChanged(int uasid, QString key, double value,quint64 time);
+//    void actuatorChanged(UASInterface* uas, int actId, double value);
     void refresh();
 
 protected: