SlugsDataSensorView.cc 12.2 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
#include <QDebug>

9 10 11 12 13
SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SlugsDataSensorView)
{
    ui->setupUi(this);
14

15
    activeUAS = NULL;
16

17
    this->setVisible(false);
18 19 20 21




22 23 24 25 26 27 28 29 30
}

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

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

33
  if (slugsMav != NULL) {
34

35
    connect(slugsMav, SIGNAL(slugsRawImu(int, const mavlink_raw_imu_t&)), this, SLOT(slugRawDataChanged(int, const mavlink_raw_imu_t&)));
36

37
    #ifdef MAVLINK_ENABLED_SLUGS
38 39

        //connect standar messages
40
    connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64)));
41 42 43
    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(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
44
    connect(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double)));
45 46 47



48 49

     //connect slugs especial messages
50 51 52 53 54 55 56 57
    connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&)));
    connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&)));
    connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&)));
    connect(slugsMav, SIGNAL(slugsNavegation(int,const mavlink_slugs_navigation_t&)),this,SLOT(slugsNavegationChanged(int,const mavlink_slugs_navigation_t&)));
    connect(slugsMav, SIGNAL(slugsDataLog(int,const mavlink_data_log_t&)), this, SLOT(slugsDataLogChanged(int,const mavlink_data_log_t&)));
    connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&)));
    connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&)));
    connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&)));
58 59
    connect(slugsMav,SIGNAL(slugsAirData(int, const mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int, const mavlink_air_data_t&)));

tecnosapiens's avatar
tecnosapiens committed
60

61
    #endif // MAVLINK_ENABLED_SLUGS
62
        // Set this UAS as active if it is the first one
63
    if(activeUAS == 0) {
64 65
            activeUAS = uas;
        }
66

67 68 69
    }
}

70 71
void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t &rawData){
 Q_UNUSED(uasId);
72

73 74 75
 ui->m_Axr->setText(QString::number(rawData.xacc));
 ui->m_Ayr->setText(QString::number(rawData.yacc));
 ui->m_Azr->setText(QString::number(rawData.zacc));
76 77 78 79 80 81 82 83 84

 ui->m_Mxr->setText(QString::number(rawData.xmag));
 ui->m_Myr->setText(QString::number(rawData.ymag));
 ui->m_Mzr->setText(QString::number(rawData.zmag));

 ui->m_Gxr->setText(QString::number(rawData.xgyro));
 ui->m_Gyr->setText(QString::number(rawData.ygyro));
 ui->m_Gzr->setText(QString::number(rawData.zgyro));

85
}
86

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

91 92
#ifdef MAVLINK_ENABLED_SLUGS

93 94 95 96
void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas,
                                                     double lat,
                                                     double lon,
                                                     double alt,
97 98 99
                                                     quint64 time) {
 Q_UNUSED(uas);
 Q_UNUSED(time);
100

101 102


103 104 105
 ui->m_GpsLatitude->setText(QString::number(lat));
 ui->m_GpsLongitude->setText(QString::number(lon));
 ui->m_GpsHeight->setText(QString::number(alt));
106 107

 qDebug()<<"GPS Position = "<<lat<<" - "<<lon<<" - "<<alt;
108 109 110 111
}


void SlugsDataSensorView::slugLocalPositionChanged(UASInterface* uas,
112 113 114
                                                   double x,
                                                   double y,
                                                   double z,
115 116 117
                                                   quint64 time) {
  Q_UNUSED(uas);
  Q_UNUSED(time);
118

119 120 121
  ui->ed_x->setPlainText(QString::number(x));
  ui->ed_y->setPlainText(QString::number(y));
  ui->ed_z->setPlainText(QString::number(z));
122

123 124
  //qDebug()<<"Local Position = "<<x<<" - "<<y<<" - "<<z;

125 126
}

127
void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface* uas,
128 129 130
                                                        double vx,
                                                        double vy,
                                                        double vz,
131
                                                        quint64 time) {
132
    Q_UNUSED( uas);
133 134 135 136 137
  Q_UNUSED(time);

  ui->ed_vx->setPlainText(QString::number(vx));
  ui->ed_vy->setPlainText(QString::number(vy));
  ui->ed_vz->setPlainText(QString::number(vz));
138

139 140 141
  //qDebug()<<"Speed Local Position = "<<vx<<" - "<<vy<<" - "<<vz;


142 143
}

144
void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas,
145 146 147 148
                                              double slugroll,
                                              double slugpitch,
                                              double slugyaw,
                                              quint64 time)
149
{
150
    Q_UNUSED( uas);
151
    Q_UNUSED(time);
152

153 154 155
    ui->m_Roll->setPlainText(QString::number(slugroll));
    ui->m_Pitch->setPlainText(QString::number(slugpitch));
    ui->m_Yaw->setPlainText(QString::number(slugyaw));
156

157 158
     qDebug()<<"Attitude change = "<<slugroll<<" - "<<slugpitch<<" - "<<slugyaw;

159
}
160

161

162 163
void SlugsDataSensorView::slugsSensorBiasChanged(int systemId,
                                                 const mavlink_sensor_bias_t& sensorBias){
164
     Q_UNUSED( systemId);
165

166 167 168 169 170 171
  ui->m_AxBiases->setText(QString::number(sensorBias.axBias));
  ui->m_AyBiases->setText(QString::number(sensorBias.ayBias));
  ui->m_AzBiases->setText(QString::number(sensorBias.azBias));
  ui->m_GxBiases->setText(QString::number(sensorBias.gxBias));
  ui->m_GyBiases->setText(QString::number(sensorBias.gyBias));
  ui->m_GzBiases->setText(QString::number(sensorBias.gzBias));
172 173 174

}

175

176
void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId,
177
                                                        const mavlink_diagnostic_t& diagnostic){
178 179
    Q_UNUSED(systemId);

180 181 182
  ui->m_Fl1->setText(QString::number(diagnostic.diagFl1));
  ui->m_Fl2->setText(QString::number(diagnostic.diagFl2));
  ui->m_Fl3->setText(QString::number(diagnostic.diagFl2));
183

184 185 186
  ui->m_Sh1->setText(QString::number(diagnostic.diagSh1));
  ui->m_Sh2->setText(QString::number(diagnostic.diagSh2));
  ui->m_Sh3->setText(QString::number(diagnostic.diagSh3));
187 188 189 190
}


void SlugsDataSensorView::slugsCpuLoadChanged(int systemId,
191
                                              const mavlink_cpu_load_t& cpuLoad){
192
     Q_UNUSED(systemId);
193 194 195
  ui->ed_sens->setText(QString::number(cpuLoad.sensLoad));
  ui->ed_control->setText(QString::number(cpuLoad.ctrlLoad));
  ui->ed_batvolt->setText(QString::number(cpuLoad.batVolt));
196 197 198
}

void SlugsDataSensorView::slugsNavegationChanged(int systemId,
199
                                                 const mavlink_slugs_navigation_t& slugsNavigation){
200
     Q_UNUSED(systemId);
201 202 203 204 205 206 207 208 209
  ui->m_Um->setText(QString::number(slugsNavigation.u_m));
  ui->m_PhiC->setText(QString::number(slugsNavigation.phi_c));
  ui->m_PitchC->setText(QString::number(slugsNavigation.theta_c));
  ui->m_PsidC->setText(QString::number(slugsNavigation.psiDot_c));
  ui->m_AyBody->setText(QString::number(slugsNavigation.ay_body));
  ui->m_TotRun->setText(QString::number(slugsNavigation.totalDist));
  ui->m_DistToGo->setText(QString::number(slugsNavigation.dist2Go));
  ui->m_FromWP->setText(QString::number(slugsNavigation.fromWP));
  ui->m_ToWP->setText(QString::number(slugsNavigation.toWP));
210 211 212 213 214
}



void SlugsDataSensorView::slugsDataLogChanged(int systemId,
215
                                              const mavlink_data_log_t& dataLog){
216
     Q_UNUSED(systemId);
217 218 219 220 221 222
  ui->m_logFl1->setText(QString::number(dataLog.fl_1));
  ui->m_logFl2->setText(QString::number(dataLog.fl_2));
  ui->m_logFl3->setText(QString::number(dataLog.fl_3));
  ui->m_logFl4->setText(QString::number(dataLog.fl_4));
  ui->m_logFl5->setText(QString::number(dataLog.fl_5));
  ui->m_logFl6->setText(QString::number(dataLog.fl_6));
223 224 225
}

void SlugsDataSensorView::slugsPWMChanged(int systemId,
226
                                          const mavlink_pwm_commands_t& pwmCommands){
227
       Q_UNUSED(systemId);
228 229 230 231 232 233 234 235 236
  ui->m_pwmThro->setText(QString::number(pwmCommands.dt_c));
  ui->m_pwmAile->setText(QString::number(pwmCommands.dla_c));
  ui->m_pwmElev->setText(QString::number(pwmCommands.dle_c));
  ui->m_pwmRudd->setText(QString::number(pwmCommands.dr_c));

  ui->m_pwmThroTrim->setText(QString::number(pwmCommands.dre_c));
  ui->m_pwmAileTrim->setText(QString::number(pwmCommands.dlf_c));
  ui->m_pwmElevTrim->setText(QString::number(pwmCommands.drf_c));
  ui->m_pwmRuddTrim->setText(QString::number(pwmCommands.aux1));
237 238 239 240

}

void SlugsDataSensorView::slugsFilteredDataChanged(int systemId,
241
                                                   const mavlink_filtered_data_t& filteredData){
242
    Q_UNUSED(systemId);
243 244 245 246 247 248 249 250 251
  ui->m_Axf->setText(QString::number(filteredData.aX));
  ui->m_Ayf->setText(QString::number(filteredData.aY));
  ui->m_Azf->setText(QString::number(filteredData.aZ));
  ui->m_Gxf->setText(QString::number(filteredData.gX));
  ui->m_Gyf->setText(QString::number(filteredData.gY));
  ui->m_Gzf->setText(QString::number(filteredData.gZ));
  ui->m_Mxf->setText(QString::number(filteredData.mX));
  ui->m_Myf->setText(QString::number(filteredData.mY));
  ui->m_Mzf->setText(QString::number(filteredData.mZ));
252 253 254
}

void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId,
255
                                                  const mavlink_gps_date_time_t& gpsDateTime){
256
    Q_UNUSED(systemId);
257 258 259 260 261 262 263 264 265 266 267 268

    QString month, day;

    month = QString::number(gpsDateTime.month);
    day = QString::number(gpsDateTime.day);

    if(gpsDateTime.month < 10) month = "0" + QString::number(gpsDateTime.month);
    if(gpsDateTime.day < 10) day = "0" + QString::number(gpsDateTime.day);


    ui->m_GpsDate->setText(day + "/" +
                         month + "/" +
269
                         QString::number(gpsDateTime.year));
270

271 272 273 274 275 276 277 278 279 280 281 282 283
    QString hour, min, sec;

    hour = QString::number(gpsDateTime.hour);
    min =  QString::number(gpsDateTime.min);
    sec =  QString::number(gpsDateTime.sec);

    if(gpsDateTime.hour < 10) hour = "0" + QString::number(gpsDateTime.hour);
    if(gpsDateTime.min < 10) min = "0" + QString::number(gpsDateTime.min);
    if(gpsDateTime.sec < 10) sec = "0" + QString::number(gpsDateTime.sec);

    ui->m_GpsTime->setText(hour + ":" +
                           min + ":" +
                           sec);
284

285
  ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat));
286 287


288
}
289

290 291 292 293 294 295
/**
     * @brief Updates the air data widget - 171
*/
void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData)
{
     Q_UNUSED(systemId);
296 297 298
     ui->ed_dynamic->setText(QString::number(airData.dynamicPressure));
     ui->ed_static->setText(QString::number(airData.staticPressure));
     ui->ed_temp->setText(QString::number(airData.temperature));
299

300 301 302 303
//     qDebug()<<"Air Data = "<<airData.dynamicPressure<<" - "
//                            <<airData.staticPressure<<" - "
//                            <<airData.temperature;

304 305
}

306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324
/**
     * @brief set COG and SOG values
     *
     * COG and SOG GPS display on the Widgets
*/
void SlugsDataSensorView::slugsGPSCogSog(int systemId,
                                         double cog,
                                         double sog)

{
     Q_UNUSED(systemId);

     ui->m_GpsCog->setText(QString::number(cog));
     ui->m_GpsSog->setText(QString::number(sog));

}



325 326


327

328
#endif // MAVLINK_ENABLED_SLUGS