gcs-nf/App/CommandBox/CommandBox.cpp

#include "CommandBox.h"
#include "ui_CommandBox.h"

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

    this->setTop(true);


    //use_ext_ins参数名称
    ui->comboBox_IMU->addItem("内置惯导",0);
    ui->comboBox_IMU->addItem("外置惯导",1);
    ui->comboBox_IMU->setCurrentIndex(0);



    ui->comboBox_ActuatorType->addItem("扫频试验",0);
    ui->comboBox_ActuatorType->addItem("滞环试验",1);
    ui->comboBox_ActuatorType->addItem("死区测试",2);
    ui->comboBox_ActuatorType->addItem("阶跃测试",3);
    ui->comboBox_ActuatorType->setCurrentIndex(0);


    /*
    ui->comboBox_sel_ch->addItem("关闭",0x00);
    ui->comboBox_sel_ch->addItem("左副翼",0x01);
    ui->comboBox_sel_ch->addItem("右副翼",0x02);
    ui->comboBox_sel_ch->addItem("左升降",0x03);
    ui->comboBox_sel_ch->addItem("右升降",0x04);
    ui->comboBox_sel_ch->addItem("方向舵",0x05);
    ui->comboBox_sel_ch->setCurrentIndex(0);
    */


    //ui->pushButton_AirHeatOff->hide();
    //ui->pushButton_AirHeatOn->hide();
    //ui->pushButton_HeatOn3->hide();
    //ui->pushButton_HeatOn4->hide();



    m_ra = 0;
    m_la = 0;
    m_ru = 0;
    m_re = 0;
    m_le = 0;


/*
    QRegExp *reg = new QRegExp("^(-?\d+)(\.\d+)?$");

    ui->lineEdit_msl_in->setValidator(QRegExpValidator(reg));
    ui->lineEdit_Ma_in->setValidator(QRegExpValidator(reg));
    ui->lineEdit_Ps_in->setValidator(QRegExpValidator(reg));
    ui->lineEdit_qbar_in->setValidator(QRegExpValidator(reg));
    */


}

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


void CommandBox::setFloatFlag(bool flag)
{
    floatflag = flag;
}

float CommandBox::param_getvalue(QVariant param,uint8_t type)
{
        float value;
        union {
            uint8_t   u8[8];
            int8_t    i8[8];
            uint16_t  u16[4];
            int16_t   i16[4];
            uint32_t  u32[2];
            int32_t   i32[2];
            uint64_t  u64;
            int64_t   i64;
            float     f[2];
            double    d;
        }src;

        switch (type)
        {
        case MAV_PARAM_TYPE_UINT8:
        {
            src.u8[0] =  param.toUInt();
            ((uint8_t *)&value)[0] = src.u8[0];

        }break;
        case MAV_PARAM_TYPE_INT8:
        {
            src.i8[0] =  param.toUInt();
            ((int8_t *)&value)[0] = src.i8[0];
        }break;
        case MAV_PARAM_TYPE_UINT16:
        {
            src.u16[0] =  param.toUInt();
            ((uint16_t *)&value)[0] = src.u16[0];
        }break;
        case MAV_PARAM_TYPE_INT16:
        {
            src.i16[0] =  param.toUInt();
            ((int16_t *)&value)[0] = src.i16[0];
        }break;
        case MAV_PARAM_TYPE_UINT32:
        {
            src.u32[0] =  param.toUInt();
            ((uint32_t *)&value)[0] = (uint32_t)src.u32[0];
        }break;
        case MAV_PARAM_TYPE_INT32:
        {
            src.i32[0] =  param.toUInt();
            ((int32_t *)&value)[0] = (int32_t)src.i32[0];
        }break;
        case MAV_PARAM_TYPE_UINT64:
        {
            src.u64 =  param.toUInt();
            ((uint64_t *)&value)[0] = (uint64_t )src.u64;
        }break;
        case MAV_PARAM_TYPE_INT64:
        {
            src.i64 =  param.toUInt();
            ((int64_t *)&value)[0] = (int64_t)src.i64;
        }break;
        case MAV_PARAM_TYPE_REAL32:
        {
            src.f[0] =  param.toFloat();
            ((float *)&value)[0] = (float)src.f[0];
        }break;
        case MAV_PARAM_TYPE_REAL64:
        {
            src.d =  param.toDouble();
            ((double *)&value)[0]= (double)src.d;
        }break;
         default:
        {
            src.f[0] =  param.toFloat();
            ((float *)&value)[0] = (float)src.i32[0];
        }break;

        }

        return value;
}




void CommandBox::commandAccepted(bool flag,uint16_t command,uint8_t result)
{
    bool isFundSender = false;
    QString string;
    //找到消息的发送者


    QObjectList list = ui->frame->children();

   // qDebug() << list;

    //qDebug() << CommandBox.findChildren();

    if(command == m_command)
    {
            isFundSender = true;


            switch (result) {
            case MAV_RESULT_ACCEPTED:
                string.append(tr("%1,%2 accepted and executed").arg(command).arg(m_name));
                break;
            case MAV_RESULT_TEMPORARILY_REJECTED:
                string.append(tr("%1,%2 rejected").arg(command).arg(m_name));
                break;
            case MAV_RESULT_DENIED:
                string.append(tr("%1,%2 permanently denied").arg(command).arg(m_name));
                break;
            case MAV_RESULT_UNSUPPORTED:
                string.append(tr("%1,%2 unsupported").arg(command).arg(m_name));
                break;
            case MAV_RESULT_FAILED:
                string.append(tr("%1,%2 failed").arg(command).arg(m_name));
                break;
            case MAV_RESULT_IN_PROGRESS:
                string.append(tr("%1,%2 being executed").arg(command).arg(m_name));
                break;
            case 0xFE:
                string.append(tr("%1,%2 time out").arg(command).arg(m_name));
                break;
            case 0xFF:
                string.append(tr("Please do not operate too fast, the last instruction has not been sent"));
                break;
            default:
                break;
            }

            ui->lineEdit_CommandName->clear();
            m_name.clear();
            m_param1 = 0;
            m_param2 = 0;
            m_param3 = 0;
            m_param4 = 0;
            m_param5 = 0;
            m_param6 = 0;
            m_param7 = 0;
            m_command = 0;
            m_confirm = 0;

    }

    if(string.size() > 0)
    {
        QVariant flag;
        Config *cfg = new Config();

        cfg->getTTS(&flag);

        if(flag.toBool())
        {
            QTextToSpeech *tts = new QTextToSpeech();
            tts->say(string);
            tts->deleteLater();

        }
            emit showMessage(string);

    }
}


//二次确认
void CommandBox::setSecondConfirm(QVariant value)
{
     if(value.toBool() == true)
     {
         emit cmd_long(m_param1,m_param2,m_param3,m_param4,m_param5,m_param6,m_param7,m_command,m_confirm);
     }
     else
     {
         ui->lineEdit_CommandName->clear();
         m_name.clear();
         m_param1 = 0;
         m_param2 = 0;
         m_param3 = 0;
         m_param4 = 0;
         m_param5 = 0;
         m_param6 = 0;
         m_param7 = 0;
         m_command = 0;
         m_confirm = 0;

     }
}

void CommandBox::on_pushButton_SendCommand_clicked()
{
    if(cmdType == 0)
    {
        if(m_command == 0)
        {
            emit showMessage(tr("please set command first ,command :%1").arg(m_command));
            return;
        }
    }

    if(cmdType == 0)
    {
         emit showMessage(tr("发送指令 ") + QString::number(m_command) + " " +  m_name,5000);
         emit cmd_long(m_param1,m_param2,m_param3,m_param4,m_param5,m_param6,m_param7,m_command,m_confirm);
    }
    else if(cmdType == 1)
    {

        emit showMessage(tr("修改参数 ") + " " + m_id.toLatin1().data() +  " " + QString::number(m_value),5000);


         emit WriteCmd(m_sysid,m_compid,m_id.toLatin1().data(),m_type,m_value);
         //qDebug() << m_type << m_value;
    }
}

void CommandBox::on_pushButton_Cancel_clicked()
{
    ui->lineEdit_CommandName->clear();
    m_name.clear();
        m_param1 = 0;
        m_param2 = 0;
        m_param3 = 0;
        m_param4 = 0;
        m_param5 = 0;
        m_param6 = 0;
        m_param7 = 0;
        m_command = 0;
        m_confirm = 0;
}


void CommandBox::on_commandClicked(QString name,
                                   float param1,float param2,float param3,float param4,
                                   float param5,float param6,float param7,
                                   uint16_t command,uint8_t confirmation)
{

      m_name    = name;
      m_param1  = param1;
      m_param2  = param2;
      m_param3  = param3;
      m_param4  = param4;
      m_param5  = param5;
      m_param6  = param6;
      m_param7  = param7;
      m_command = command;
      m_confirm = confirmation;

       ui->lineEdit_CommandName->setText(name);

       cmdType = 0;
 }

void CommandBox::on_paramClicked(uint8_t sysid, uint8_t compid , QString id, uint8_t type, float value)
{

    m_sysid = sysid;
    m_compid = compid;
    m_id = id;
    m_type = type;
    m_value = value;

    ui->lineEdit_CommandName->setText(id + " " + QString::number(value));

    cmdType = 1;
 }





//舵机指令

void CommandBox::on_pushButton_IMU_clicked()
{
    if((ui->comboBox_IMU->currentIndex() != -1)&&(ui->comboBox_sys->currentIndex() != -1))
    {
        uint8_t sysid = ui->comboBox_sys->currentData(Qt::UserRole).toInt();
        uint8_t compid = 1;
        QString id;
        uint8_t type = MAV_PARAM_TYPE_INT8;

        id.clear();
        id.append("use_ext_ins");

        float value = param_getvalue(ui->comboBox_IMU->currentData(Qt::UserRole).toInt(),type);

        on_paramClicked(sysid,compid,id,type,value);

    }
    else
    {
        QMessageBox *box = new QMessageBox(this);

        box->setWindowTitle(tr("警告"));
        box->setText(tr("请设定目标"));
        box->show();
    }
}


void CommandBox::on_pushButton_IAS_SEL_0_clicked()
{
    if(ui->comboBox_sys->currentIndex() != -1)
    {

        uint8_t sysid = ui->comboBox_sys->currentData(Qt::UserRole).toInt();
        uint8_t compid = 1;
        QString id;
        uint8_t type = MAV_PARAM_TYPE_UINT8;

        id.clear();
        id.append("IAS_SEL");


        float value = param_getvalue(0,type);

        on_paramClicked(sysid,compid,id,type,value);

    }
    else
    {
        QMessageBox *box = new QMessageBox(this);

        box->setWindowTitle(tr("警告"));
        box->setText(tr("请设定目标"));
        box->show();
    }
}

void CommandBox::on_pushButton_IAS_SEL_1_clicked()
{
    if(ui->comboBox_sys->currentIndex() != -1)
    {

        uint8_t sysid = ui->comboBox_sys->currentData(Qt::UserRole).toInt();
        uint8_t compid = 1;
        QString id;
        uint8_t type = MAV_PARAM_TYPE_UINT8;

        id.clear();
        id.append("IAS_SEL");

        float value = param_getvalue(1,type);

        on_paramClicked(sysid,compid,id,type,value);

    }
    else
    {
        QMessageBox *box = new QMessageBox(this);

        box->setWindowTitle(tr("警告"));
        box->setText(tr("请设定目标"));
        box->show();
    }
}

void CommandBox::on_pushButton_IAS_SEL_2_clicked()
{
    if(ui->comboBox_sys->currentIndex() != -1)
    {
        uint8_t sysid = ui->comboBox_sys->currentData(Qt::UserRole).toInt();
        uint8_t compid = 1;
        QString id;
        uint8_t type = MAV_PARAM_TYPE_UINT8;

        id.clear();
        id.append("IAS_SEL");

        float value = param_getvalue(2,type);

        on_paramClicked(sysid,compid,id,type,value);

    }
    else
    {
        QMessageBox *box = new QMessageBox(this);

        box->setWindowTitle(tr("警告"));
        box->setText(tr("请设定目标"));
        box->show();
    }
}




void CommandBox::addVehicles(int sysid, int compid)
{
    Q_UNUSED(compid)

    if(ui->comboBox_sys->findData(sysid) == -1)
    {
        ui->comboBox_sys->addItem(QString::number(sysid),sysid);
    }
}

void CommandBox::on_pushButton_setFuel_clicked()
{
    double value = ui->doubleSpinBox_Fuel->value();

    on_commandClicked(tr("%1 %2kg").arg(ui->pushButton_setFuel->text()).arg(QString::number(value)),
                      value,0,0,0,0,0,0,20009,0);
}


void CommandBox::on_pushButton_AngleZero_clicked()//动压归零
{
    on_commandClicked(tr("设定 %1").arg(ui->pushButton_AngleZero->text()),
                      0,0,1,0,0,0,0,20005,0);
}


void CommandBox::on_pushButton_ZeroComplete_clicked()
{
    on_commandClicked(tr("设定 %1").arg(ui->pushButton_ZeroComplete->text()),
                      0,0,0,0,0,0,0,20005,0);
}


void CommandBox::on_pushButton_HeatOn1_clicked()
{
    on_commandClicked(tr("设定 %1").arg(ui->pushButton_HeatOn1->text()),
                      1,0,0,0,0,0,0,20005,0);
}


void CommandBox::on_pushButton_HeatOn2_clicked()
{
    on_commandClicked(tr("设定 %1").arg(ui->pushButton_HeatOn2->text()),
                      0,1,0,0,0,0,0,20005,0);
}

//伺服指令
void CommandBox::on_pushButton_AirHeatOn_clicked()
{
    on_commandClicked(ui->pushButton_AirHeatOn->text(),
                      1,1,0,0,0,0,0,20005,0);
}

void CommandBox::on_pushButton_HeatOff_clicked()
{
    on_commandClicked(ui->pushButton_HeatOff->text(),
                      0,0,0,0,0,0,0,20005,0);
}


void CommandBox::on_pushButton_setStructSweepParameter_clicked()
{

    float msl_in = ui->lineEdit_msl_in->text().toDouble();
    float ma_in = ui->lineEdit_Ma_in->text().toDouble();
    float ps_in = ui->lineEdit_Ps_in->text().toDouble();
    float qbar_in = ui->lineEdit_qbar_in->text().toDouble();



    on_commandClicked(tr("设定 %1").arg(ui->pushButton_setStructSweepParameter->text()),
                      msl_in,ma_in,ps_in,qbar_in,0,0,0,20014,0);

    qDebug() << "StructSweep:" <<msl_in << ma_in << ps_in << qbar_in;
}


void CommandBox::on_pushButton_test_cmd_clicked()
{
   uint8_t ch = 0;
   float Amp;
   float start;
   float stop;
   float K;
   float type;
   float hlre;


   type = ui->comboBox_ActuatorType->currentData().toFloat();

   //ch = ui->comboBox_sel_ch->currentData(Qt::UserRole).toInt();

   /* 1 ail
    * 2 ele
    * 4 rud
    * 8 dsp
    *
    * */

   ch |= ui->checkBox_ail->isChecked()?(1):(0);
   ch |= ui->checkBox_ele->isChecked()?(2):(0);
   ch |= ui->checkBox_rud->isChecked()?(4):(0);
   ch |= ui->checkBox_dsp->isChecked()?(8):(0);

   qDebug() << "ch" << ch;

   Amp  = ui->doubleSpinBox_A_deg->value();
   start = ui->doubleSpinBox__Start_frq_Hz->value();
   stop = ui->doubleSpinBox_Stop_frq_Hz->value();
   K = ui->doubleSpinBox_K_frq_Hz->value();

   quint16 hldn = ui->spinBox_hldn->value();
   quint16 recn = ui->spinBox_recn->value();

   hlre = ((quint16)recn << 8)|hldn;

    on_commandClicked(tr("设定 %1").arg(ui->pushButton_test_cmd->text()),
                      ch,Amp,start,stop,K,type,hlre,20013,0);


    qDebug() << "Servo Test:" <<ch << Amp << start << stop << K << type << hlre;
}



void CommandBox::on_pushButton_test_stop_clicked()
{


    on_commandClicked(tr("设定 %1").arg(ui->pushButton_test_stop->text()),
                      0,0,0,0,0,0,0,20008,0);
}