QGCPX4SensorCalibration.cc 19.7 KB
Newer Older
1 2 3 4 5
#include "QGCPX4SensorCalibration.h"
#include "ui_QGCPX4SensorCalibration.h"
#include <UASManager.h>
#include <QMenu>
#include <QScrollBar>
6
#include <QDebug>
7 8 9 10 11 12 13 14 15 16

QGCPX4SensorCalibration::QGCPX4SensorCalibration(QWidget *parent) :
    QWidget(parent),
    activeUAS(NULL),
    clearAction(new QAction(tr("Clear Text"), this)),
    ui(new Ui::QGCPX4SensorCalibration)
{
    ui->setupUi(this);
    connect(clearAction, SIGNAL(triggered()), ui->textView, SLOT(clear()));

17 18 19
    connect(ui->gyroButton, SIGNAL(clicked()), this, SLOT(gyroButtonClicked()));
    connect(ui->magButton, SIGNAL(clicked()), this, SLOT(magButtonClicked()));
    connect(ui->accelButton, SIGNAL(clicked()), this, SLOT(accelButtonClicked()));
20
    connect(ui->diffPressureButton, SIGNAL(clicked()), this, SLOT(diffPressureButtonClicked()));
21

22 23 24 25
    connect(ui->logCheckBox, SIGNAL(clicked(bool)), ui->textView, SLOT(setVisible(bool)));
    ui->logCheckBox->setChecked(false);
    ui->textView->setVisible(false);

26 27 28 29
    ui->gyroButton->setEnabled(false);
    ui->magButton->setEnabled(false);
    ui->accelButton->setEnabled(false);

30 31 32
    ui->autopilotComboBox->setEnabled(false);
    ui->magComboBox->setEnabled(false);

33 34 35
    setInstructionImage(":/files/images/px4/calibration/accel_down.png");
    setAutopilotImage(":/files/images/px4/calibration/accel_down.png");
    setGpsImage(":/files/images/px4/calibration/accel_down.png");
36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63

    // Fill combo boxes
    ui->autopilotComboBox->addItem(tr("Default Orientation"), 0);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_45"), 1);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_90"), 2);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_135"), 3);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_180"), 4);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_225"), 5);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_270"), 6);
    ui->autopilotComboBox->addItem(tr("ROTATION_YAW_315"), 7);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180"), 8);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_45"), 9);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_90"), 10);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_135"), 11);
    ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_180"), 12);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_225"), 13);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_270"), 14);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_YAW_315"), 15);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90"), 16);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_45"), 17);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_90"), 18);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_135"), 19);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270"), 20);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_45"), 21);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_90"), 22);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_YAW_135"), 23);
    ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_90"), 24);
    ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_270"), 25);
64 65 66 67 68 69 70 71 72 73 74 75
    ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_180_YAW_90"), 26);
    ui->autopilotComboBox->addItem(tr("ROTATION_PITCH_180_YAW_270"), 27);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_PITCH_90"), 28);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_PITCH_90"), 29);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_PITCH_90"), 30);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_PITCH_180"), 31);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_PITCH_180"), 32);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_PITCH_270"), 33);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_180_PITCH_270"), 34);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_270_PITCH_270"), 35);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_PITCH_180_YAW_90"), 36);
    ui->autopilotComboBox->addItem(tr("ROTATION_ROLL_90_YAW_270"), 37);
76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102

    ui->magComboBox->addItem(tr("Default Orientation"), 0);
    ui->magComboBox->addItem(tr("ROTATION_YAW_45"), 1);
    ui->magComboBox->addItem(tr("ROTATION_YAW_90"), 2);
    ui->magComboBox->addItem(tr("ROTATION_YAW_135"), 3);
    ui->magComboBox->addItem(tr("ROTATION_YAW_180"), 4);
    ui->magComboBox->addItem(tr("ROTATION_YAW_225"), 5);
    ui->magComboBox->addItem(tr("ROTATION_YAW_270"), 6);
    ui->magComboBox->addItem(tr("ROTATION_YAW_315"), 7);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180"), 8);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_45"), 9);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_90"), 10);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_135"), 11);
    ui->magComboBox->addItem(tr("ROTATION_PITCH_180"), 12);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_225"), 13);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_270"), 14);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_180_YAW_315"), 15);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_90"), 16);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_45"), 17);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_90"), 18);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_90_YAW_135"), 19);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_270"), 20);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_45"), 21);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_90"), 22);
    ui->magComboBox->addItem(tr("ROTATION_ROLL_270_YAW_135"), 23);
    ui->magComboBox->addItem(tr("ROTATION_PITCH_90"), 24);
    ui->magComboBox->addItem(tr("ROTATION_PITCH_270"), 25);
103 104 105 106 107 108 109

    setObjectName("PX4_SENSOR_CALIBRATION");

    setStyleSheet("QScrollArea { border: 0px; } QPlainTextEdit { border: 0px }");

    setActiveUAS(UASManager::instance()->getActiveUAS());
    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
110
    ui->progressBar->setValue(0);
111 112 113 114

    connect(ui->autopilotComboBox, SIGNAL(activated(int)), this, SLOT(setAutopilotOrientation(int)));
    connect(ui->magComboBox, SIGNAL(activated(int)), this, SLOT(setGpsOrientation(int)));

Lorenz Meier's avatar
Lorenz Meier committed
115
    updateIcons();
116 117 118 119 120 121 122
}

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

123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144
void QGCPX4SensorCalibration::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
    Q_UNUSED(uas);
    Q_UNUSED(component);

    int index = (int)value.toFloat();

    if (parameterName.contains("SENS_BOARD_ROT"))
    {
        ui->autopilotComboBox->setCurrentIndex(index);
        setAutopilotImage(index);
        ui->autopilotComboBox->setEnabled(true);
    }

    if (parameterName.contains("SENS_EXT_MAG_ROT"))
    {
        ui->magComboBox->setCurrentIndex(index);
        setGpsImage(index);
        ui->magComboBox->setEnabled(true);
    }

    // Check mag calibration naively
145
    if (parameterName.contains("SENS_MAG_XOFF") || parameterName.contains("CAL_MAG0_ID")) {
146 147 148 149 150 151 152 153 154 155
        float offset = value.toFloat();
        if (offset < 0.000001f && offset > -0.000001f) {
            // Must be zero, not good
            setMagCalibrated(false);
        } else {
            setMagCalibrated(true);
        }
    }

    // Check gyro calibration naively
156
    if (parameterName.contains("SENS_GYRO_XOFF") || parameterName.contains("CAL_GYRO0_ID")) {
157 158 159 160 161 162 163 164 165 166
        float offset = value.toFloat();
        if (offset < 0.000001f && offset > -0.000001f) {
            // Must be zero, not good
            setGyroCalibrated(false);
        } else {
            setGyroCalibrated(true);
        }
    }

    // Check accel calibration naively
167
    if (parameterName.contains("SENS_ACC_XOFF") || parameterName.contains("CAL_ACC0_ID")) {
168 169 170 171 172 173 174 175
        float offset = value.toFloat();
        if (offset < 0.000001f && offset > -0.000001f) {
            // Must be zero, not good
            setAccelCalibrated(false);
        } else {
            setAccelCalibrated(true);
        }
    }
176 177 178 179 180 181 182 183 184 185 186

    // Check differential pressure calibration naively
    if (parameterName.contains("SENS_DPRES_OFF")) {
      float offset = value.toFloat();
      if (offset < 0.000001f && offset > -0.000001f) {
          // Must be zero, not good
          setDiffPressureCalibrated(false);
      } else {
          setDiffPressureCalibrated(true);
      }
    }
187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233
}

void QGCPX4SensorCalibration::setMagCalibrated(bool calibrated)
{
    if (calibrated) {
        ui->magLabel->setText(tr("MAG CALIBRATED"));
        ui->magLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #20AA20;"
                                    "}");
    } else {
        ui->magLabel->setText(tr("MAG UNCALIBRATED"));
        ui->magLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #FF0037;"
                                    "}");
    }
}

void QGCPX4SensorCalibration::setGyroCalibrated(bool calibrated)
{
    if (calibrated) {
        ui->gyroLabel->setText(tr("GYRO CALIBRATED"));
        ui->gyroLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #20AA20;"
                                    "}");
    } else {
        ui->gyroLabel->setText(tr("GYRO UNCALIBRATED"));
        ui->gyroLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #FF0037;"
                                    "}");
    }
}

void QGCPX4SensorCalibration::setAccelCalibrated(bool calibrated)
{
    if (calibrated) {
        ui->accelLabel->setText(tr("ACCEL CALIBRATED"));
        ui->accelLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #20AA20;"
                                    "}");
    } else {
        ui->accelLabel->setText(tr("ACCEL UNCALIBRATED"));
        ui->accelLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #FF0037;"
                                    "}");
    }
}

234 235 236 237 238 239 240 241 242 243 244 245 246 247 248
void QGCPX4SensorCalibration::setDiffPressureCalibrated(bool calibrated)
{
    if (calibrated) {
        ui->diffPressureLabel->setText(tr("DIFF. PRESSURE CALIBRATED"));
        ui->diffPressureLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #20AA20;"
                                    "}");
    } else {
        ui->diffPressureLabel->setText(tr("DIFF. PRESSURE UNCALIBRATED"));
        ui->diffPressureLabel->setStyleSheet("QLabel { color: #FFFFFF;"
                                    "background-color: #FF0037;"
                                    "}");
    }
}

249 250 251 252 253 254 255
void QGCPX4SensorCalibration::setInstructionImage(const QString &path)
{
    instructionIcon.load(path);

    int w = ui->iconLabel->width();
    int h = ui->iconLabel->height();

256 257 258 259 260
    ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation));
}

void QGCPX4SensorCalibration::setAutopilotImage(int index)
{
261 262 263
    Q_UNUSED(index);
    // FIXME: This was referencing a non-existent png. Need to figure out what this was trying to do.
    //setAutopilotImage(QString(":/files/images/px4/calibration/pixhawk_%1.png").arg(index, 2, 10, QChar('0')));
264 265 266 267
}

void QGCPX4SensorCalibration::setGpsImage(int index)
{
268
    setGpsImage(QString(":/files/images/px4/calibration/3dr_gps/gps_%1.png").arg(index, 2, 10, QChar('0')));
269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
}

void QGCPX4SensorCalibration::setAutopilotOrientation(int index)
{
    if (activeUAS) {
        activeUAS->getParamManager()->setPendingParam(0, "SENS_BOARD_ROT", (int)index);
        activeUAS->getParamManager()->sendPendingParameters(true);
    }
}

void QGCPX4SensorCalibration::setGpsOrientation(int index)
{
    if (activeUAS) {
        activeUAS->getParamManager()->setPendingParam(0, "SENS_EXT_MAG_ROT", (int)index);
        activeUAS->getParamManager()->sendPendingParameters(true);
    }
}

void QGCPX4SensorCalibration::setAutopilotImage(const QString &path)
{
289 290 291
    if (autopilotIcon.load(path)) {
        int w = ui->autopilotLabel->width();
        int h = ui->autopilotLabel->height();
292

293 294 295 296
        ui->autopilotLabel->setPixmap(autopilotIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation));
    } else {
        qDebug() << "AutoPilot Icon image did not load" << path;
    }
297 298 299 300
}

void QGCPX4SensorCalibration::setGpsImage(const QString &path)
{
301 302 303
    if (gpsIcon.load(path)) {
        int w = ui->gpsLabel->width();
        int h = ui->gpsLabel->height();
304

305 306 307 308
        ui->gpsLabel->setPixmap(gpsIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation));
    } else {
        qDebug() << "GPS Icon image did not load" << path;
    }
309 310
}

Lorenz Meier's avatar
Lorenz Meier committed
311
void QGCPX4SensorCalibration::updateIcons()
312 313 314
{
    int w = ui->iconLabel->width();
    int h = ui->iconLabel->height();
315 316 317 318 319
    ui->iconLabel->setPixmap(instructionIcon.scaled(w, h, Qt::KeepAspectRatio, Qt::SmoothTransformation));

    int wa = ui->autopilotLabel->width();
    int ha = ui->autopilotLabel->height();
    ui->autopilotLabel->setPixmap(autopilotIcon.scaled(wa, ha, Qt::KeepAspectRatio, Qt::SmoothTransformation));
320

321 322 323
    int wg = ui->gpsLabel->width();
    int hg = ui->gpsLabel->height();
    ui->gpsLabel->setPixmap(gpsIcon.scaled(wg, hg, Qt::KeepAspectRatio, Qt::SmoothTransformation));
Lorenz Meier's avatar
Lorenz Meier committed
324
}
325

Lorenz Meier's avatar
Lorenz Meier committed
326 327 328
void QGCPX4SensorCalibration::resizeEvent(QResizeEvent* event)
{
    updateIcons();
329 330 331 332 333 334 335 336 337
    QWidget::resizeEvent(event);
}

void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas)
{
    if (!uas)
        return;

    if (activeUAS) {
338 339
        disconnect(activeUAS, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString)));
        disconnect(activeUAS, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant)));
340 341 342
        ui->textView->clear();
    }

343 344 345 346
    ui->gyroButton->setEnabled(true);
    ui->magButton->setEnabled(true);
    ui->accelButton->setEnabled(true);

347
    connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(handleTextMessage(int,int,int,QString)));
348
    connect(uas, SIGNAL(parameterChanged(int,int,QString,QVariant)), this, SLOT(parameterChanged(int,int,QString,QVariant)));
349
    connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
350
    activeUAS = uas;
351

352
    updateSystemSpecs(uas->getUASID());
353
    
354
    // If the parameters are ready, we aren't going to get paramterChanged signals. So re-request them in order to make the UI work.
355
    if (uas->getParamManager()->parametersReady()) {
356
        _requestAllSensorParameters();
357
    }
358 359 360 361 362 363
}

void QGCPX4SensorCalibration::updateSystemSpecs(int id)
{
    Q_UNUSED(id);

364 365 366 367 368 369 370 371
    if (activeUAS->isRotaryWing()) {
        // Users are confused by the config button
        ui->diffPressureButton->hide();
        ui->diffPressureLabel->hide();
    } else {
        ui->diffPressureButton->show();
        ui->diffPressureLabel->show();
    }
372 373 374 375
}

void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int severity, QString text)
{
Lorenz Meier's avatar
Lorenz Meier committed
376 377 378 379
    Q_UNUSED(uasid);
    Q_UNUSED(compId);
    Q_UNUSED(severity);

380 381 382 383
    if (text.startsWith("[cmd]") ||
        text.startsWith("[mavlink pm]"))
        return;

Anton Babushkin's avatar
Anton Babushkin committed
384
    if (text.contains("progress <")) {
385 386 387 388 389 390 391 392
        QString percent = text.split("<").last().split(">").first();
        bool ok;
        int p = percent.toInt(&ok);
        if (ok)
            ui->progressBar->setValue(p);
        return;
    }

Lorenz Meier's avatar
Lorenz Meier committed
393 394
    ui->instructionLabel->setText(QString("%1").arg(text));

395 396
    if (text.startsWith("Hold still, starting to measure ")) {
        QString axis = text.section(" ", -2, -2);
Anton Babushkin's avatar
Anton Babushkin committed
397
        setInstructionImage(QString(":/files/images/px4/calibration/accel_%1.png").arg(axis));
Lorenz Meier's avatar
Lorenz Meier committed
398 399
    }

400 401
    if (text.startsWith("pending: ")) {
        QString axis = text.section(" ", 1, 1);
Anton Babushkin's avatar
Anton Babushkin committed
402
        setInstructionImage(QString(":/files/images/px4/calibration/accel_%1.png").arg(axis));
403 404
    }

405 406
    if (text == "rotate in a figure 8 around all axis" /* support for old typo */
            || text == "rotate in a figure 8 around all axes" /* current version */) {
Lorenz Meier's avatar
Lorenz Meier committed
407
        setInstructionImage(":/files/images/px4/calibration/mag_calibration_figure8.png");
408 409
    }

Anton Babushkin's avatar
Anton Babushkin committed
410
    if (text.endsWith(" calibration: done") || text.endsWith(" calibration: failed")) {
411
        // XXX use a confirmation image or something
412
        setInstructionImage(":/files/images/px4/calibration/accel_down.png");
Anton Babushkin's avatar
Anton Babushkin committed
413 414 415 416
        if (text.endsWith(" calibration: done")) {
            ui->progressBar->setValue(100);
        } else {
            ui->progressBar->setValue(0);
417
        }
418

419
        if (activeUAS) {
420
            _requestAllSensorParameters();
421
        }
422 423
    }

Anton Babushkin's avatar
Anton Babushkin committed
424
    if (text.endsWith(" calibration: started")) {
425
        setInstructionImage(":/files/images/px4/calibration/accel_down.png");
Lorenz Meier's avatar
Lorenz Meier committed
426 427
    }

428 429 430 431 432 433 434 435
    // XXX color messages according to severity

    QPlainTextEdit *msgWidget = ui->textView;

    //turn off updates while we're appending content to avoid breaking the autoscroll behavior
    msgWidget->setUpdatesEnabled(false);
    QScrollBar *scroller = msgWidget->verticalScrollBar();

436
    msgWidget->appendHtml(QString("%4").arg(text));
437 438 439 440

    // Ensure text area scrolls correctly
    scroller->setValue(scroller->maximum());
    msgWidget->setUpdatesEnabled(true);
441
}
442

443 444
void QGCPX4SensorCalibration::gyroButtonClicked()
{
445
    setInstructionImage(":/files/images/px4/calibration/accel_down.png");
446 447 448 449 450 451
    activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
    ui->progressBar->setValue(0);
}

void QGCPX4SensorCalibration::magButtonClicked()
{
452
    setInstructionImage(":/files/images/px4/calibration/accel_down.png");
453 454 455 456 457 458
    activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0);
    ui->progressBar->setValue(0);
}

void QGCPX4SensorCalibration::accelButtonClicked()
{
459
    setInstructionImage(":/files/images/px4/calibration/accel_down.png");
460 461
    activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0);
    ui->progressBar->setValue(0);
462 463
}

464 465
void QGCPX4SensorCalibration::diffPressureButtonClicked()
{
466
    setInstructionImage(":/files/images/px4/calibration/accel_down.png");
467 468 469 470
    activeUAS->executeCommand(MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0);
    ui->progressBar->setValue(0);
}

471 472 473 474 475 476
void QGCPX4SensorCalibration::contextMenuEvent(QContextMenuEvent* event)
{
    QMenu menu(this);
    menu.addAction(clearAction);
    menu.exec(event->globalPos());
}
477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507

void QGCPX4SensorCalibration::_requestAllSensorParameters(void)
{
    static const char* rgSensorsCalParamsV1[] = {
        "SENS_ACC_XOFF", "SENS_ACC_YOFF", "SENS_ACC_ZOFF", "SENS_ACC_XSCALE", "SENS_ACC_YSCALE", "SENS_ACC_ZSCALE",
        "SENS_GYRO_XOFF", "SENS_GYRO_YOFF", "SENS_GYRO_ZOFF", "SENS_GYRO_XSCALE", "SENS_GYRO_YSCALE", "SENS_GYRO_ZSCALE",
        "SENS_MAG_XOFF", "SENS_MAG_YOFF", "SENS_MAG_ZOFF", "SENS_MAG_XSCALE", "SENS_MAG_YSCALE", "SENS_MAG_ZSCALE", "SENS_EXT_MAG_ROT",
        "SENS_DPRES_OFF", "SENS_DPRES_ANA",
        "SENS_BOARD_ROT",
        NULL };
    
    static const char* rgSensorsCalParamsV2[] = {
        "CAL_ACC0_ID", "CAL_ACC0_XOFF", "CAL_ACC0_YOFF", "CAL_ACC0_ZOFF", "CAL_ACC0_XSCALE", "CAL_ACC0_YSCALE", "CAL_ACC0_ZSCALE",
        "CAL_GYRO0_ID", "CAL_GYRO0_XOFF", "CAL_GYRO0_YOFF", "CAL_GYRO0_ZOFF", "CAL_GYRO0_XSCALE", "CAL_GYRO0_YSCALE", "CAL_GYRO0_ZSCALE",
        "CAL_MAG0_ID", "CAL_MAG0_XOFF", "CAL_MAG0_YOFF", "CAL_MAG0_ZOFF", "CAL_MAG0_XSCALE", "CAL_MAG0_YSCALE", "CAL_MAG0_ZSCALE", "SENS_EXT_MAG_ROT",
        "SENS_DPRES_OFF", "SENS_DPRES_ANA",
        "SENS_BOARD_ROT",
        NULL };

    Q_ASSERT(activeUAS);
    QGCUASParamManagerInterface* paramMgr = activeUAS->getParamManager();
    
    // Temp hack for parameter mapping
    bool paramsV1 = paramMgr->getComponentForParam("SENS_MAG_XOFF").count();
    static const char** prgParamList = paramsV1 ? rgSensorsCalParamsV1 : rgSensorsCalParamsV2;

    for (size_t i=0; prgParamList[i] != NULL; i++) {
        qDebug() << "Requesting" << prgParamList[i];
        activeUAS->requestParameter(0, prgParamList[i]);
    }
}