SlugsDataSensorView.cc 5.49 KB
Newer Older
1 2 3 4
#include "SlugsDataSensorView.h"
#include "ui_SlugsDataSensorView.h"

#include <UASManager.h>
tecnosapiens's avatar
tecnosapiens committed
5
#include "SlugsMAV.h"
6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54

SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SlugsDataSensorView)
{
    ui->setupUi(this);


    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));

    activeUAS = NULL;

    loadParameters();


    this->setVisible(false);

    // timer for refresh UI
    updateTimer = new QTimer(this);
    connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh()));
    updateTimer->start(200);
}

void SlugsDataSensorView::loadParameters()
{
    Xpos = 0;
    Ypos = 0;
    Zpos = 0;
    TimeActualPosition = 0;

    VXpos = 0;
    VYpos = 0;
    VZpos = 0;
    TimeActualSpeed =0;

    roll = 0;
    pitch = 0;
    yaw = 0;
    TimeActualAttitude = 0;

    //Sensor Biases
    //Acelerometer
    Axb = 0;
    Ayb = 0;
    Azb = 0;
    //Gyro
    Gxb = 0;
    Gyb = 0;
    Gzb = 0;
55
    TimeActualBias = 0;
56 57 58 59 60 61 62 63 64 65

}

SlugsDataSensorView::~SlugsDataSensorView()
{
    delete ui;
}

void SlugsDataSensorView::addUAS(UASInterface* uas)
{
tecnosapiens's avatar
tecnosapiens committed
66 67 68
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(uas);

    if (slugsMav != NULL)
69
    {
tecnosapiens's avatar
tecnosapiens committed
70 71 72 73
        connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChange(UASInterface*,double,double,double,quint64)));
        connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64)));
        connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64)));
        connect(slugsMav, SIGNAL(slugsSensorBias(UASInterface*,double,double,double,double,double,double,quint64)), this, SLOT(slugsSensorBiasAcelerometerChanged(UASInterface*,double,double,double,quint64)));
tecnosapiens's avatar
tecnosapiens committed
74

75 76 77 78 79 80 81 82 83 84 85 86

        // Set this UAS as active if it is the first one
        if(activeUAS == 0)
        {
            activeUAS = uas;
        }
    }
}

void SlugsDataSensorView::setActiveUAS(UASInterface* uas)
{
    activeUAS = uas;
87 88


89 90 91 92 93
}


void SlugsDataSensorView::refresh()
{
94 95
    if(activeUAS)
    {
96

97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
            //refresh UI position data
            ui->ed_x->setPlainText(QString::number(Xpos, 'f', 4));
            ui->ed_y->setPlainText(QString::number(Ypos, 'f', 4));
            ui->ed_z->setPlainText(QString::number(Zpos, 'f', 4));

            //refresh UI speed position data
            ui->ed_vx->setPlainText(QString::number(VXpos,'f',4));
            ui->ed_vy->setPlainText(QString::number(VYpos,'f',4));
            ui->ed_vz->setPlainText(QString::number(VZpos,'f',4));

            //refresh UI attitude data
            ui->m_SlugAttitudeRoll_plainTextEdit->setPlainText(QString::number(roll,'f',4));
            ui->m_SlugAttitudePitch_plainTextEdit->setPlainText(QString::number(pitch,'f',4));
            ui->m_SlugAttitudeYaw_plainTextEdit->setPlainText(QString::number(yaw,'f',4));

            //refresh UI sensor bias acelerometer data
            ui->m_SlugsAxBiases_textEdit->setText(QString::number(Axb, 'f', 4));
            ui->m_SlugsAyBiases_textEdit->setText(QString::number(Ayb, 'f', 4));
            ui->m_SlugsAzBiases_textEdit->setText(QString::number(Azb, 'f', 4));
            ui->m_SlugsGxBiases_textEdit->setText(QString::number(Gxb, 'f', 4));
            ui->m_SlugsGyBiases_textEdit->setText(QString::number(Gyb, 'f', 4));
            ui->m_SlugsGzBiases_textEdit->setText(QString::number(Gzb, 'f', 4));
119

120 121


tecnosapiens's avatar
tecnosapiens committed
122 123


124
    }
125 126 127

}

128 129 130 131 132
void SlugsDataSensorView::slugLocalPositionChanged(UASInterface * uasTemp,
                                                   double x,
                                                   double y,
                                                   double z,
                                                   quint64 time)
133 134 135 136 137 138 139 140 141 142
{
    Q_UNUSED( uasTemp);

    Xpos = x;
    Ypos = y;
    Zpos = z;
    TimeActualPosition = time;

}

143 144 145 146 147
void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface *uasTemp,
                                                        double vx,
                                                        double vy,
                                                        double vz,
                                                        quint64 time)
148 149 150 151 152 153 154 155 156
{
    Q_UNUSED( uasTemp);

    VXpos = vx;
    VYpos = vy;
    VZpos = vz;
    TimeActualSpeed = time;
}

157 158 159 160 161
void SlugsDataSensorView::slugAttitudeChanged(UASInterface *uasTemp,
                                              double slugroll,
                                              double slugpitch,
                                              double slugyaw,
                                              quint64 time)
162 163 164 165 166 167 168 169 170
{
    Q_UNUSED( uasTemp);

    roll = slugroll;
    pitch = slugpitch;
    yaw = slugyaw;
    TimeActualAttitude = time;
}

171 172 173 174 175 176 177 178
void SlugsDataSensorView::slugsSensorBiasChanged(UASInterface *uasTemp,
                                                 double axb,
                                                 double ayb,
                                                 double azb,
                                                 double gxb,
                                                 double gyb,
                                                 double gzb,
                                                 quint64 time)
179 180 181 182 183 184
{
     Q_UNUSED( uasTemp);

     Axb = axb;
     Ayb = ayb;
     Azb = azb;
185 186 187 188
     Gxb = gxb;
     Gyb = gyb;
     Gzb = gzb;
     TimeActualBias = time;
189 190 191 192 193


}