ApmPlaneLevel.cc 1.53 KB
Newer Older
1 2 3 4 5 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 55 56 57 58 59
#include "ApmPlaneLevel.h"
#include <QMessageBox>

ApmPlaneLevel::ApmPlaneLevel(QWidget *parent) : AP2ConfigWidget(parent)
{
    ui.setupUi(this);
    connect(ui.levelPushButton,SIGNAL(clicked()),this,SLOT(levelClicked()));
    connect(ui.manualLevelCheckBox,SIGNAL(toggled(bool)),this,SLOT(manualCheckBoxToggled(bool)));
}

ApmPlaneLevel::~ApmPlaneLevel()
{
}
void ApmPlaneLevel::levelClicked()
{
    QMessageBox::information(0,"Warning","Be sure the plane is completly level, then click ok");
    MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
    int confirm = 0;
    float param1 = 1;
    float param2 = 0;
    float param3 = 1;
    float param4 = 0;
    float param5 = 0;
    float param6 = 0;
    float param7 = 0;
    int component = 1;
    m_uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
    QMessageBox::information(0,"Warning","Leveling completed");
}

void ApmPlaneLevel::manualCheckBoxToggled(bool checked)
{
    if (!m_uas)
    {
        return;
    }
    if (checked)
    {
        m_uas->getParamManager()->setParameter(1,"MANUAL_LEVEL",1);
    }
    else
    {
        m_uas->getParamManager()->setParameter(1,"MANUAL_LEVEL",0);
    }
}
void ApmPlaneLevel::parameterChanged(int uas, int component, QString parameterName, QVariant value)
{
    if (parameterName == "MANUAL_LEVEL")
    {
        if (value.toInt() == 1)
        {
            ui.manualLevelCheckBox->setChecked(true);
        }
        else
        {
            ui.manualLevelCheckBox->setChecked(false);
        }
    }
}