AccelCalibrationConfig.cc 3.53 KB
Newer Older
1 2 3
#include "AccelCalibrationConfig.h"


4
AccelCalibrationConfig::AccelCalibrationConfig(QWidget *parent) : AP2ConfigWidget(parent)
5 6
{
    ui.setupUi(this);
7 8
    connect(ui.calibrateAccelButton,SIGNAL(clicked()),this,SLOT(calibrateButtonClicked()));

9
    m_accelAckCount=0;
10
    initConnections();
11 12 13 14 15
}

AccelCalibrationConfig::~AccelCalibrationConfig()
{
}
16 17
void AccelCalibrationConfig::activeUASSet(UASInterface *uas)
{
18
    if (m_uas)
19
    {
20
        disconnect(m_uas,SIGNAL(textMessageReceived(int,int,int,QString)),this,SLOT(uasTextMessageReceived(int,int,int,QString)));
21
    }
22 23
    AP2ConfigWidget::activeUASSet(uas);
    if (!uas)
24
    {
25
        return;
26 27
    }
    connect(m_uas,SIGNAL(textMessageReceived(int,int,int,QString)),this,SLOT(uasTextMessageReceived(int,int,int,QString)));
28

29 30 31 32
}

void AccelCalibrationConfig::calibrateButtonClicked()
{
33 34 35 36 37 38
    if (!m_uas)
    {
        showNullMAVErrorMessageBox();
        return;
    }
    if (m_accelAckCount == 0)
39 40 41
    {
        MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
        int confirm = 0;
42 43 44 45 46 47 48
        float param1 = 0.0;
        float param2 = 0.0;
        float param3 = 0.0;
        float param4 = 0.0;
        float param5 = 1.0;
        float param6 = 0.0;
        float param7 = 0.0;
49 50 51
        int component = 1;
        m_uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
    }
52
    else if (m_accelAckCount <= 5)
53
    {
54
        m_uas->executeCommandAck(m_accelAckCount++,true);
55 56 57
    }
    else
    {
58
        m_uas->executeCommandAck(m_accelAckCount++,true);
59
        ui.calibrateAccelButton->setText("Calibrate\nAccelerometer");
60 61 62 63 64 65 66 67 68
        if (m_accelAckCount > 8)
        {
            //We've clicked too many times! Reset.
            for (int i=0;i<8;i++)
            {
                m_uas->executeCommandAck(i,true);
            }
            m_accelAckCount = 0;
        }
69 70 71
    }

}
72 73 74 75 76 77 78 79 80 81 82
void AccelCalibrationConfig::hideEvent(QHideEvent *evt)
{
    if (!m_uas || !m_accelAckCount)
    {
        return;
    }
    for (int i=m_accelAckCount;i<8;i++)
    {
        m_uas->executeCommandAck(i,true); //Clear out extra commands.
    }
}
83 84 85 86 87 88 89
void AccelCalibrationConfig::uasTextMessageReceived(int uasid, int componentid, int severity, QString text)
{
    //command received: " Severity 1
    //Place APM Level and press any key" severity 5
    if (severity == 5)
    {
        //This is a calibration instruction
90
        if (m_accelAckCount == 0)
91 92
        {
            //Calibration Sucessful\r"
93
            ui.calibrateAccelButton->setText("Continue");
94
            m_accelAckCount++;
95
        }
96
        if (m_accelAckCount == 7)
97 98 99 100
        {
            //All finished
            //ui.outputLabel->setText(ui.outputLabel->text() + "\n" + text);
            ui.outputLabel->setText(text);
101
            m_accelAckCount++;
102
        }
103
        if (m_accelAckCount == 8)
104 105 106
        {
            if (text.contains("Calibration") && text.contains("successful"))
            {
107
                m_accelAckCount = 0;
108
            }
109 110 111 112
            else if (text.contains("Calibration") && text.contains("FAILED")) //Failure
            {
                m_accelAckCount = 0;
            }
113 114 115 116
            ui.outputLabel->setText(ui.outputLabel->text() + "\n" + text);
        }
        else
        {
117 118 119 120 121 122 123
            ui.outputLabel->setText(text.replace("press any key","click Continue below"));
            if (!this->isVisible())
            {
                //Clear out!
                m_uas->executeCommandAck(m_accelAckCount++,true);
                ui.calibrateAccelButton->setText("Calibrate\nAccelerometer");
            }
124 125 126 127
        }
    }

}