radio calibration data is now being sent to the uas

qgroundcontrol.pro

@@ -18,9 +18,10 @@ TARGET = qgroundcontrol
 BASEDIR = .
 BUILDDIR = build
 LANGUAGE = C++
-CONFIG += debug_and_release \
-    #console
-OBJECTS_DIR = $$BUILDDIR/obj
+CONFIG += debug_and_release \ # console
+    OBJECTS_DIR \
+    = \
+    $$BUILDDIR/obj
 MOC_DIR = $$BUILDDIR/moc
 UI_HEADERS_DIR = src/ui/generated
 
@@ -161,6 +162,7 @@ HEADERS += src/MG.h \
     src/ui/QGCRemoteControlView.h \
     src/WaypointGlobal.h \
     src/ui/WaypointGlobalView.h \
+    src/ui/RadioCalibration/RadioCalibrationData.h \
     src/ui/RadioCalibration/RadioCalibrationWindow.h \
     src/ui/RadioCalibration/AirfoilServoCalibrator.h \
     src/ui/RadioCalibration/SwitchCalibrator.h \
@@ -236,7 +238,8 @@ SOURCES += src/main.cc \
     src/ui/RadioCalibration/AirfoilServoCalibrator.cc \
     src/ui/RadioCalibration/SwitchCalibrator.cc \
     src/ui/RadioCalibration/CurveCalibrator.cc \
-    src/ui/RadioCalibration/AbstractCalibrator.cc
+    src/ui/RadioCalibration/AbstractCalibrator.cc \
+    src/ui/RadioCalibration/RadioCalibrationData.cc
 RESOURCES = mavground.qrc
 
 # Include RT-LAB Library

src/comm/OpalLink.cc

@@ -142,6 +142,16 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
         	this->sendRCValues = static_cast<bool>(rc.enabled);
         }
         break;
+#ifdef MAVLINK_ENABLE_UALBERTA_MESSAGES
+        case MAVLINK_MSG_ID_RADIO_CALIBRATION:
+        {
+            mavlink_radio_calibration_t radio;
+            mavlink_msg_radio_calibration_decode(&msg, &radio);
+            qDebug() << "RADIO CALIBRATION RECEIVED";
+            qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
+        }
+        break;
+#endif
         default:
             {
                 qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";

src/uas/UAS.cc

@@ -552,6 +552,41 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time);
             }
             break;
+       case MAVLINK_MSG_ID_RADIO_CALIBRATION:
+            {
+                mavlink_radio_calibration_t radioMsg;
+                mavlink_msg_radio_calibration_decode(&message, &radioMsg);
+                QVector<float> aileron;
+                QVector<float> elevator;
+                QVector<float> rudder;
+                QVector<float> gyro;
+                QVector<float> pitch;
+                QVector<float> throttle;
+
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN; ++i)
+                    aileron << radioMsg.aileron[i];
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN; ++i)
+                    elevator << radioMsg.elevator[i];
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN; ++i)
+                    rudder << radioMsg.rudder[i];
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_GYRO_LEN; ++i)
+                    gyro << radioMsg.gyro[i];
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN; ++i)
+                    pitch << radioMsg.pitch[i];
+                for (int i=0; i<MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN; ++i)
+                    throttle << radioMsg.throttle[i];
+
+                QPointer<RadioCalibrationData> radioData = new RadioCalibrationData(aileron,
+                                                                                    elevator,
+                                                                                    rudder,
+                                                                                    gyro,
+                                                                                    pitch,
+                                                                                    throttle);
+                emit radioCalibrationReceived(radioData);
+                delete radioData;
+            }
+            break;
+
 #endif
         default:
             {

src/uas/UASInterface.h

@@ -36,10 +36,12 @@ This file is part of the QGROUNDCONTROL project
 #include <QList>
 #include <QAction>
 #include <QColor>
+#include <QPointer>
 
 #include "LinkInterface.h"
 #include "ProtocolInterface.h"
 #include "UASWaypointManager.h"
+#include "RadioCalibration/RadioCalibrationData.h"
 
 /**
  * @brief Interface for all robots.
@@ -339,6 +341,8 @@ signals:
     void remoteControlChannelChanged(int channelId, float raw, float normalized);
     /** @brief Remote control RSSI changed */
     void remoteControlRSSIChanged(float rssi);
+    /** @brief Radio Calibration Data has been received from the MAV*/
+    void radioCalibrationReceived(const QPointer<RadioCalibrationData>&);
 
     /**
      * @brief Localization quality changed

src/ui/QGCRemoteControlView.cc

@@ -98,6 +98,8 @@ void QGCRemoteControlView::setUASId(int id)
             // The UAS exists, disconnect any existing connections
             disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
             disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
+            disconnect(uas, SIGNAL(radioCalibrationReceived(const QPointer<RadioCalibrationData>)), calibrationWindow, SLOT(receive(const QPointer<RadioCalibrationData>&)));
+            disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float)));
         }
     }
 
@@ -108,6 +110,7 @@ void QGCRemoteControlView::setUASId(int id)
         // New UAS exists, connect
         nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName()));
         calibrationWindow->setUASId(id);
+        connect(newUAS, SIGNAL(radioCalibrationReceived(const QPointer<RadioCalibrationData>)), calibrationWindow, SLOT(receive(const QPointer<RadioCalibrationData>&)));
         connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
         connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float)));
         connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));

src/ui/RadioCalibration/AbstractCalibrator.h

@@ -4,6 +4,7 @@
 #include <QWidget>
 #include <QString>
 #include <QLabel>
+#include <QVector>
 
 #include <math.h>
 
@@ -14,6 +15,8 @@ public:
     explicit AbstractCalibrator(QWidget *parent = 0);
     ~AbstractCalibrator();
 
+    virtual void set(const QVector<float>& data)=0;
+
 public slots:
     void channelChanged(float raw);
 

src/ui/RadioCalibration/AirfoilServoCalibrator.cc

@@ -105,3 +105,13 @@ void AirfoilServoCalibrator::setLow()
     lowPulseWidth->setText(QString::number(static_cast<double>(logExtrema())));
     emit lowSetpointChanged(logExtrema());
 }
+
+void AirfoilServoCalibrator::set(const QVector<float> &data)
+{
+    if (data.size() == 3)
+    {
+        lowPulseWidth->setText(QString::number(data[0]));
+        centerPulseWidth->setText(QString::number(data[1]));
+        highPulseWidth->setText(QString::number(data[2]));
+    }
+}

src/ui/RadioCalibration/AirfoilServoCalibrator.h

@@ -23,6 +23,9 @@ public:
 
     explicit AirfoilServoCalibrator(AirfoilType type = AILERON, QWidget *parent = 0);
 
+    /** @param data must have exaclty 3 elemets.  they are assumed to be low center high */
+    void set(const QVector<float>& data);
+
 signals:
     void highSetpointChanged(float);
     void centerSetpointChanged(float);
@@ -38,9 +41,9 @@ protected:
     QLabel *centerPulseWidth;
     QLabel *lowPulseWidth;
 
-    float high;
-    float center;
-    float low;
+//    float high;
+//    float center;
+//    float low;
 };
 
 #endif // AIRFOILSERVOCALIBRATOR_H

src/ui/RadioCalibration/CurveCalibrator.cc

@@ -87,3 +87,17 @@ void CurveCalibrator::setSetpoint(int setpoint)
 
     emit setpointChanged(setpoint, static_cast<float>(setpoints->value(setpoint)));
 }
+
+void CurveCalibrator::set(const QVector<float> &data)
+{
+    if (data.size() == 5)
+    {
+        delete setpoints;
+        QVector<double> dataDouble;
+        for (int i=0; i<data.size(); ++i)
+            dataDouble << static_cast<double>(data[i]);
+        setpoints = new QVector<double>(dataDouble);
+        curve->setData(*positions, *setpoints);
+        plot->replot();
+    }
+}

src/ui/RadioCalibration/CurveCalibrator.h

@@ -23,6 +23,8 @@ Q_OBJECT
 public:
     explicit CurveCalibrator(QString title = QString(), QWidget *parent = 0);
     ~CurveCalibrator();
+
+    void set(const QVector<float> &data);
 signals:
     void setpointChanged(int setpoint, float raw);    
 

src/ui/RadioCalibration/RadioCalibrationData.cc

@@ -0,0 +1,49 @@
+#include "RadioCalibrationData.h"
+
+RadioCalibrationData::RadioCalibrationData()
+{
+    data = new QVector<QVector<float> >();
+}
+
+RadioCalibrationData::RadioCalibrationData(const QVector<float> &aileron,
+                                                                   const QVector<float> &elevator,
+                                                                   const QVector<float> &rudder,
+                                                                   const QVector<float> &gyro,
+                                                                   const QVector<float> &pitch,
+                                                                   const QVector<float> &throttle)
+{
+    data = new QVector<QVector<float> >();
+    (*data) << aileron
+            << elevator
+            << rudder
+            << gyro
+            << pitch
+            << throttle;
+}
+
+RadioCalibrationData::RadioCalibrationData(const RadioCalibrationData &other)
+    :QObject()
+{
+    data = new QVector<QVector<float> >(*other.data);
+}
+
+const float* RadioCalibrationData::operator [](int i) const
+{
+    if (i < data->size())
+    {
+        return (*data)[i].constData();
+    }
+
+    return NULL;
+}
+
+const QVector<float>& RadioCalibrationData::operator ()(int i) const
+{
+    if (i < data->size())
+    {
+        return (*data)[i];
+    }
+
+    // This is not good.  If it is ever used after being returned it will cause a crash
+    return QVector<float>();
+}

src/ui/RadioCalibration/RadioCalibrationData.h

@@ -0,0 +1,53 @@
+#ifndef RADIOCALIBRATIONDATA_H
+#define RADIOCALIBRATIONDATA_H
+
+#include <QObject>
+#include <QDebug>
+#include <QVector>
+
+
+class RadioCalibrationData : public QObject
+{
+Q_OBJECT
+
+public:
+    explicit RadioCalibrationData();
+    RadioCalibrationData(const RadioCalibrationData&);
+    RadioCalibrationData(const QVector<float>& aileron,
+                         const QVector<float>& elevator,
+                         const QVector<float>& rudder,
+                         const QVector<float>& gyro,
+                         const QVector<float>& pitch,
+                         const QVector<float>& throttle);
+
+    enum RadioElement
+    {
+        AILERON=0,
+        ELEVATOR,
+        RUDDER,
+        GYRO,
+        PITCH,
+        THROTTLE
+    };
+
+    void loadFile();
+    void saveFile();
+    void send();
+    void receive();
+    const float* operator[](int i) const;
+    const QVector<float>& operator()(int i) const;
+
+protected:
+    QVector<QVector<float> > *data;
+
+
+    void init(const QVector<float>& aileron,
+              const QVector<float>& elevator,
+              const QVector<float>& rudder,
+              const QVector<float>& gyro,
+              const QVector<float>& pitch,
+              const QVector<float>& throttle);
+
+};
+
+#endif // RADIOCALIBRATIONDATA_H

src/ui/RadioCalibration/RadioCalibrationWindow.cc

@@ -41,7 +41,8 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
     connect(load, SIGNAL(clicked()), this, SLOT(loadFile()));
     connect(save, SIGNAL(clicked()), this, SLOT(saveFile()));
     connect(transmit, SIGNAL(clicked()), this, SLOT(send()));
-    connect(get, SIGNAL(clicked()), this, SLOT(receive()));
+    connect(get, SIGNAL(clicked()), this, SLOT(request()));
+
 
     setUASId(0);
 }
@@ -76,53 +77,18 @@ void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized)
     }
 }
 
-
-/*
-  ** RadioCalibrationData Function Definitions **
-*/
-
-RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(RadioCalibrationWindow *parent)
-    :parent(parent)
-{
-    data = new QVector<QVector<float> >();
-}
-
-RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(const QVector<float> &aileron,
-                                                                   const QVector<float> &elevator,
-                                                                   const QVector<float> &rudder,
-                                                                   const QVector<float> &gyro,
-                                                                   const QVector<float> &pitch,
-                                                                   const QVector<float> &throttle,
-                                                                   RadioCalibrationWindow *parent)
-                                                                       :parent(parent)
-{
-    data = new QVector<QVector<float> >();
-    (*data) << aileron
-            << elevator
-            << rudder
-            << gyro
-            << pitch
-            << throttle;
-}
-
-RadioCalibrationWindow::RadioCalibrationData::RadioCalibrationData(RadioCalibrationData &other)
-    :parent(other.parent)
-{
-    data = new QVector<QVector<float> >(*other.data);
-}
-
-void RadioCalibrationWindow::RadioCalibrationData::saveFile()
+void RadioCalibrationWindow::saveFile()
 {
     qDebug() << __FILE__ << __LINE__ << "SAVE TO FILE";
 }
 
-void RadioCalibrationWindow::RadioCalibrationData::loadFile()
+void RadioCalibrationWindow::loadFile()
 {
     qDebug() << __FILE__ << __LINE__ << "LOAD FROM FILE";
 }
 
-void RadioCalibrationWindow::RadioCalibrationData::send()
-{    
+void RadioCalibrationWindow::send()
+{
     qDebug() << __FILE__ << __LINE__ << "uasId = " << uasId;
 #ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
     UAS *uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(uasId));
@@ -131,24 +97,42 @@ void RadioCalibrationWindow::RadioCalibrationData::send()
         qDebug()<< "we have a uas";
         mavlink_message_t msg;
         mavlink_msg_radio_calibration_pack(uasId, 0, &msg,
-                                           (*data)[AILERON].constData(),
-                                           (*data)[ELEVATOR].constData(),
-                                           (*data)[RUDDER].constData(),
-                                           (*data)[GYRO].constData(),
-                                           (*data)[PITCH].constData(),
-                                           (*data)[THROTTLE].constData());
+                                           (*radio)[RadioCalibrationData::AILERON],
+                                           (*radio)[RadioCalibrationData::ELEVATOR],
+                                           (*radio)[RadioCalibrationData::RUDDER],
+                                           (*radio)[RadioCalibrationData::GYRO],
+                                           (*radio)[RadioCalibrationData::PITCH],
+                                           (*radio)[RadioCalibrationData::THROTTLE]);
         uas->sendMessage(msg);
-
     }
 #endif
 }
 
-void RadioCalibrationWindow::RadioCalibrationData::receive()
+void RadioCalibrationWindow::request()
 {
     qDebug() << __FILE__ << __LINE__ << "READ FROM UAV";
+    UAS *uas = dynamic_cast<UAS*>(UASManager::instance()->getUASForId(uasId));
+    if (uas)
+    {
+        mavlink_message_t msg;
+        mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC);
+        uas->sendMessage(msg);
+    }
 }
 
-void RadioCalibrationWindow::RadioCalibrationData::setUASId(int id)
+void RadioCalibrationWindow::receive(const QPointer<RadioCalibrationData>& radio)
 {
-    this->uasId = id;
+    if (radio)
+    {
+        if (this->radio)
+            delete this->radio;
+        this->radio = new RadioCalibrationData(*radio);
+
+        aileron->set((*radio)(RadioCalibrationData::AILERON));
+        elevator->set((*radio)(RadioCalibrationData::ELEVATOR));
+        rudder->set((*radio)(RadioCalibrationData::RUDDER));
+        gyro->set((*radio)(RadioCalibrationData::GYRO));
+        pitch->set((*radio)(RadioCalibrationData::PITCH));
+        throttle->set((*radio)(RadioCalibrationData::THROTTLE));
+    }        
 }

src/ui/RadioCalibration/RadioCalibrationWindow.h

@@ -9,15 +9,18 @@
 #include <QGridLayout>
 #include <QHBoxLayout>
 #include <QDebug>
+#include <QPointer>
 
 #include "AirfoilServoCalibrator.h"
 #include "SwitchCalibrator.h"
 #include "CurveCalibrator.h"
 
-#include "UASManager.h"
-#include "UASInterface.h"
-#include "UAS.h"
 #include "mavlink.h"
+#include "mavlink_types.h"
+#include "UAS.h"
+#include "UASManager.h"
+#include "RadioCalibrationData.h"
+
 
 class RadioCalibrationWindow : public QWidget
 {
@@ -30,11 +33,12 @@ signals:
 
 public slots:
     void setChannel(int ch, float raw, float normalized);
-    void loadFile() {this->radio->loadFile();}
-    void saveFile() {this->radio->saveFile();}
-    void send() {this->radio->send();}
-    void receive() {this->radio->receive();}
-    void setUASId(int id) {this->radio->setUASId(id);}
+    void loadFile();
+    void saveFile();
+    void send();
+    void request();
+    void receive(const QPointer<RadioCalibrationData>& radio);
+    void setUASId(int id) {this->uasId = id;}
 
 
 protected:
@@ -44,54 +48,8 @@ protected:
         SwitchCalibrator *gyro;        
         CurveCalibrator *pitch;
         CurveCalibrator *throttle;
-
-        class RadioCalibrationData
-        {
-        public:
-            explicit RadioCalibrationData(RadioCalibrationWindow *parent=0);
-            RadioCalibrationData(RadioCalibrationData&);            
-            RadioCalibrationData(const QVector<float>& aileron,
-                                 const QVector<float>& elevator,
-                                 const QVector<float>& rudder,
-                                 const QVector<float>& gyro,
-                                 const QVector<float>& pitch,
-                                 const QVector<float>& throttle,
-                                 RadioCalibrationWindow *parent=0);
-
-            enum RadioElement
-            {
-                AILERON=0,
-                ELEVATOR,
-                RUDDER,
-                GYRO,
-                PITCH,
-                THROTTLE
-            };
-
-            void loadFile();
-            void saveFile();
-            void send();
-            void receive();
-            void setUASId(int id);
-
-        protected:
-            QVector<QVector<float> > *data;
-            int uasId;
-
-            void init(const QVector<float>& aileron,
-                      const QVector<float>& elevator,
-                      const QVector<float>& rudder,
-                      const QVector<float>& gyro,
-                      const QVector<float>& pitch,
-                      const QVector<float>& throttle);
-
-        private:
-            RadioCalibrationWindow *parent;
-
-
-        };
-
-        RadioCalibrationData *radio;
+        int uasId;
+        QPointer<RadioCalibrationData> radio;
 };
 
 #endif // RADIOCALIBRATIONWINDOW_H

src/ui/RadioCalibration/SwitchCalibrator.cc

@@ -47,3 +47,12 @@ void SwitchCalibrator::setToggled()
     toggledPulseWidth->setText(QString::number(static_cast<double>(logExtrema())));
     emit toggledSetpointChanged(logExtrema());
 }
+
+void SwitchCalibrator::set(const QVector<float> &data)
+{
+    if (data.size() == 2)
+    {
+        defaultPulseWidth->setText(QString::number(data[0]));
+        toggledPulseWidth->setText(QString::number(data[1]));
+    }
+}

src/ui/RadioCalibration/SwitchCalibrator.h

@@ -16,6 +16,7 @@ Q_OBJECT
 public:
     explicit SwitchCalibrator(QString title=QString(), QWidget *parent = 0);
 
+    void set(const QVector<float> &data);
 signals:
     void defaultSetpointChanged(float);
     void toggledSetpointChanged(float);