SlugsPIDControl.cpp 20.9 KB
Newer Older
1 2 3 4 5 6 7
#include "SlugsPIDControl.h"
#include "ui_SlugsPIDControl.h"


#include <QPalette>
#include<QColor>
#include <QDebug>
8 9 10
#include <UASManager.h>
#include <UAS.h>
#include "LinkManager.h"
11 12 13 14 15 16

SlugsPIDControl::SlugsPIDControl(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SlugsPIDControl)
{
    ui->setupUi(this);
17 18 19 20 21

   connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUasSet(UASInterface*)));

    activeUAS = NULL;

22
    setRedColorStyle();
23
    setGreenColorStyle();
24

25 26 27 28 29 30 31 32 33 34 35 36 37
    refreshTimerGet = new QTimer(this);
    refreshTimerGet->setInterval(100); // 20 Hz
    connect(refreshTimerGet, SIGNAL(timeout()), this, SLOT(slugsGetGeneral()));


    refreshTimerSet = new QTimer(this);
    refreshTimerSet->setInterval(100); // 20 Hz
    connect(refreshTimerSet, SIGNAL(timeout()), this, SLOT(slugsSetGeneral()));


    counterRefreshGet = 1;
    counterRefreshSet = 1;

38 39
}

40 41 42 43 44 45 46 47 48
/**
 * @brief Called when the a new UAS is set to active.
 *
 * Called when the a new UAS is set to active.
 *
 * @param uas The new active UAS
 */
void SlugsPIDControl::activeUasSet(UASInterface* uas)
{
49
    #ifdef MAVLINK_ENABLED_SLUGS
50 51 52 53
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(uas);

    if (slugsMav != NULL)
    {
54
        connect(slugsMav,SIGNAL(slugsActionAck(int,const mavlink_action_ack_t&)),this,SLOT(recibeMensaje(int,mavlink_action_ack_t)));
55
        connect(slugsMav,SIGNAL(slugsPidValues(int,mavlink_pid_t)),this, SLOT(receivePidValues(int,mavlink_pid_t)) );
56 57

        connect(ui->setGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartSet()));
58
        connect(ui->getGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartGet()));
59 60
    }

61
#endif // MAVLINK_ENABLED_SLUG
62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
    // Set this UAS as active if it is the first one
        if(activeUAS == 0)
        {
            activeUAS = uas;
            systemId = activeUAS->getUASID();
            connect_editLinesPDIvalues();

            //qDebug()<<"------------------->Active UAS ID: "<<uas->getUASID();
        }

}

/**
 * @brief Connect Edition Lines for PID Values
 *
 * @param
 */
void SlugsPIDControl::connect_editLinesPDIvalues()
{
    if(activeUAS)
    {
       connect_set_pushButtons();
84
       connect_get_pushButtons();
85 86 87 88 89 90 91 92 93
       connect_AirSpeed_LineEdit();
       connect_PitchFollowei_LineEdit();
       connect_RollControl_LineEdit();
       connect_HeigthError_LineEdit();
       connect_YawDamper_LineEdit();
       connect_Pitch2dT_LineEdit();
    }
}

94 95 96 97 98 99
SlugsPIDControl::~SlugsPIDControl()
{
    delete ui;
}

/**
100
 *@brief Set the background color RED style for the GroupBox PID when change lineEdit information
101 102
 *
 */
103
void SlugsPIDControl::setRedColorStyle()
104 105 106
{
    // GroupBox Color
    QColor groupColor = QColor(231,72,28);
107 108

    QString borderColor = "#FA4A4F";
109 110 111 112

    groupColor = groupColor.darker(475);


113
    REDcolorStyle = REDcolorStyle.sprintf("QGroupBox {background-color: #%02X%02X%02X; border: 5px solid %s; }",
114 115 116 117 118
                                    groupColor.red(), groupColor.green(), groupColor.blue(), borderColor.toStdString().c_str());

}

/**
119
 *@brief Set the background color GREEN style for the GroupBox PID when change lineEdit information
120 121
 *
 */
122
void SlugsPIDControl::setGreenColorStyle()
123
{
124
    // create Green color style
125 126 127 128 129 130
    QColor groupColor = QColor(30,124,16);
    QString borderColor = "#24AC23";

    groupColor = groupColor.darker(475);


131
    GREENcolorStyle = GREENcolorStyle.sprintf("QGroupBox {background-color: #%02X%02X%02X; border: 5px solid %s; }",
132 133 134 135 136
                                    groupColor.red(), groupColor.green(), groupColor.blue(), borderColor.toStdString().c_str());

}

/**
137
 *@brief Connection Signal and Slot of the set buttons on the widget
138
 */
139
void SlugsPIDControl::connect_set_pushButtons()
140 141
{
    //ToDo connect buttons set and get. Before create the slots
142 143 144 145 146 147
   connect(ui->dT_PID_set_pushButton, SIGNAL(clicked()),this,SLOT(changeColor_GREEN_AirSpeed_groupBox()));
   connect(ui->dE_PID_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_PitchFollowei_groupBox()));
   connect(ui->dA_PID_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_RollControl_groupBox()));
   connect(ui->HELPComm_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_HeigthError_groupBox()));
   connect(ui->dR_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_YawDamper_groupBox()));
   connect(ui->Pitch2dT_PDI_set_pushButton,SIGNAL(clicked()),this,SLOT(changeColor_GREEN_Pitch2dT_groupBox()));
148

149 150 151

}

152 153 154 155 156 157 158 159 160 161 162 163 164 165 166
/**
 *@brief Connection Signal and Slot of the set buttons on the widget
 */
void SlugsPIDControl::connect_get_pushButtons()
{
   connect(ui->dT_PID_get_pushButton, SIGNAL(clicked()),this,SLOT(get_AirSpeed_PID()));
   connect(ui->dE_PID_get_pushButton,SIGNAL(clicked()),this,SLOT(get_PitchFollowei_PID()));
   connect(ui->dR_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_YawDamper_PID()));
   connect(ui->dA_PID_get_pushButton,SIGNAL(clicked()),this,SLOT(get_RollControl_PID()));
   connect(ui->Pitch2dT_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_Pitch2dT_PID()));
   connect(ui->HELPComm_PDI_get_pushButton,SIGNAL(clicked()),this,SLOT(get_HeigthError_PID()));

}

// Functions for Air Speed GroupBox
167 168
void SlugsPIDControl::connect_AirSpeed_LineEdit()
{
169 170 171
    connect(ui->dT_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString)));
    connect(ui->dT_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString)));
    connect(ui->dT_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString)));
172 173
}

174
void SlugsPIDControl::changeColor_RED_AirSpeed_groupBox(QString text)
175
{
176
    Q_UNUSED(text);
177 178 179 180
    ui->AirSpeedHold_groupBox->setStyleSheet(REDcolorStyle);

}

181 182
void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox()
{
183 184 185 186 187 188

  SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

  if (slugsMav != NULL)
  {

189
    //create the packet
190
#ifdef MAVLINK_ENABLED_SLUGS
191 192 193 194 195 196 197 198 199
    pidMessage.target = activeUAS->getUASID();
    pidMessage.idx = 0;
    pidMessage.pVal = ui->dT_P_set->text().toFloat();
    pidMessage.iVal = ui->dT_I_set->text().toFloat();
    pidMessage.dVal = ui->dT_D_set->text().toFloat();

    mavlink_message_t msg;

    mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
200
    slugsMav->sendMessage(msg);
201
#endif
202

203
    ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle);
204
  }
205 206
}

207 208 209 210 211 212 213 214
void SlugsPIDControl::get_AirSpeed_PID()
{
     qDebug() << "\nSend Message = Air Speed ";
   sendMessagePIDStatus(0);

}


215

216
// Functions for PitchFollowei GroupBox
217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232
void SlugsPIDControl::connect_PitchFollowei_LineEdit()
{
    connect(ui->dE_P_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString)));
    connect(ui->dE_I_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString)));
    connect(ui->dE_D_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString)));
}

void SlugsPIDControl::changeColor_RED_PitchFollowei_groupBox(QString text)
{
    Q_UNUSED(text);
    ui->PitchFlowei_groupBox->setStyleSheet(REDcolorStyle);

}

void SlugsPIDControl::changeColor_GREEN_PitchFollowei_groupBox()
{
233 234 235 236
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
237
        #ifdef MAVLINK_ENABLED_SLUGS
238 239 240 241 242 243 244 245 246 247 248
      //create the packet
      pidMessage.target = activeUAS->getUASID();
      pidMessage.idx = 2;
      pidMessage.pVal = ui->dE_P_set->text().toFloat();
      pidMessage.iVal = ui->dE_I_set->text().toFloat();
      pidMessage.dVal = ui->dE_D_set->text().toFloat();

      mavlink_message_t msg;

      mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
      slugsMav->sendMessage(msg);
249
#endif
250 251 252

      ui->PitchFlowei_groupBox->setStyleSheet(GREENcolorStyle);
  }
253 254
}

255 256 257 258 259 260 261 262
void SlugsPIDControl::get_PitchFollowei_PID()
{
    qDebug() << "\nSend Message = Pitch Followei ";
  sendMessagePIDStatus(2);

}


263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282
// Functions for Roll Control GroupBox
/**
     * @brief Change color style to red when PID values of Roll Control are edited
     *
     *
     * @param
     */
void SlugsPIDControl::changeColor_RED_RollControl_groupBox(QString text)
{
    Q_UNUSED(text);
    ui->RollControl_groupBox->setStyleSheet(REDcolorStyle);
}

/**
         * @brief Change color style to green when PID values of Roll Control are send to UAS
         *
         * @param
         */
void SlugsPIDControl::changeColor_GREEN_RollControl_groupBox()
{
283 284 285 286
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
287
        #ifdef MAVLINK_ENABLED_SLUGS
288 289 290 291 292 293 294 295 296 297 298
          //create the packet
          pidMessage.target = activeUAS->getUASID();
          pidMessage.idx = 4;
          pidMessage.pVal = ui->dA_P_set->text().toFloat();
          pidMessage.iVal = ui->dA_I_set->text().toFloat();
          pidMessage.dVal = ui->dA_D_set->text().toFloat();

          mavlink_message_t msg;

          mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
          slugsMav->sendMessage(msg);
299
#endif
300 301 302

        ui->RollControl_groupBox->setStyleSheet(GREENcolorStyle);
    }
303 304 305 306 307 308 309 310 311 312 313 314 315 316
}

/**
         * @brief Connects the SIGNALS from the editline to SLOT RollControl_groupBox
         *
         * @param
         */
void SlugsPIDControl::connect_RollControl_LineEdit()
{
    connect(ui->dA_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString)));
    connect(ui->dA_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString)));
    connect(ui->dA_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString)));
}

317 318 319 320 321 322 323
void SlugsPIDControl::get_RollControl_PID()
{
    qDebug() << "\nSend Message = Roll Control ";
  sendMessagePIDStatus(4);
}


324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344

// Functions for Heigth Error GroupBox
/**
     * @brief Change color style to red when PID values of Heigth Error are edited
     *
     *
     * @param
     */
void SlugsPIDControl::changeColor_RED_HeigthError_groupBox(QString text)
{
    Q_UNUSED(text);
    ui->HeightErrorLoPitch_groupBox->setStyleSheet(REDcolorStyle);
}

/**
         * @brief Change color style to green when PID values of Heigth Error are send to UAS
         *
         * @param
         */
void SlugsPIDControl::changeColor_GREEN_HeigthError_groupBox()
{
345 346 347 348
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
349
        #ifdef MAVLINK_ENABLED_SLUGS
350 351 352 353 354 355 356 357 358 359 360
          //create the packet
          pidMessage.target = activeUAS->getUASID();
          pidMessage.idx = 1;
          pidMessage.pVal = ui->HELPComm_P_set->text().toFloat();
          pidMessage.iVal = ui->HELPComm_I_set->text().toFloat();
          pidMessage.dVal = ui->HELPComm_FF_set->text().toFloat();

          mavlink_message_t msg;

          mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
          slugsMav->sendMessage(msg);
361
#endif
362 363 364

        ui->HeightErrorLoPitch_groupBox->setStyleSheet(GREENcolorStyle);
    }
365 366 367 368 369 370 371 372 373 374 375 376 377 378
}

/**
         * @brief Connects the SIGNALS from the editline to SLOT HeigthError_groupBox
         *
         * @param
         */
void SlugsPIDControl::connect_HeigthError_LineEdit()
{
    connect(ui->HELPComm_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString)));
    connect(ui->HELPComm_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString)));
    connect(ui->HELPComm_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString)));
}

379 380 381 382 383 384
void SlugsPIDControl::get_HeigthError_PID()
{
    qDebug() << "\nSend Message = Heigth Error ";
  sendMessagePIDStatus(1);
}

385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405

// Functions for Yaw Damper GroupBox
/**
     * @brief Change color style to red when PID values of Yaw Damper are edited
     *
     *
     * @param
     */
void SlugsPIDControl::changeColor_RED_YawDamper_groupBox(QString text)
{
    Q_UNUSED(text);
    ui->YawDamper_groupBox->setStyleSheet(REDcolorStyle);
}

/**
         * @brief Change color style to green when PID values of Yaw Damper are send to UAS
         *
         * @param
         */
void SlugsPIDControl::changeColor_GREEN_YawDamper_groupBox()
{
406 407 408 409
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
410
        #ifdef MAVLINK_ENABLED_SLUGS
411 412 413 414 415 416 417 418 419 420 421
          //create the packet
          pidMessage.target = activeUAS->getUASID();
          pidMessage.idx = 3;
          pidMessage.pVal = ui->dR_P_set->text().toFloat();
          pidMessage.iVal = ui->dR_I_set->text().toFloat();
          pidMessage.dVal = ui->dR_D_set->text().toFloat();

          mavlink_message_t msg;

          mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
          slugsMav->sendMessage(msg);
422
#endif
423 424 425

        ui->YawDamper_groupBox->setStyleSheet(GREENcolorStyle);
    }
426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441
}

/**
         * @brief Connects the SIGNALS from the editline to SLOT YawDamper_groupBox
         *
         * @param
         */
void SlugsPIDControl::connect_YawDamper_LineEdit()
{
    connect(ui->dR_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString)));
    connect(ui->dR_I_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString)));
    connect(ui->dR_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_YawDamper_groupBox(QString)));


}

442 443 444 445 446 447 448
void SlugsPIDControl::get_YawDamper_PID()
{
    qDebug() << "\nSend Message = Yaw Damper ";
  sendMessagePIDStatus(3);
}


449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468
// Functions for Pitch to dT GroupBox
/**
     * @brief Change color style to red when PID values of Pitch to dT are edited
     *
     *
     * @param
     */
void SlugsPIDControl::changeColor_RED_Pitch2dT_groupBox(QString text)
{
    Q_UNUSED(text);
    ui->Pitch2dTFFterm_groupBox->setStyleSheet(REDcolorStyle);
}

/**
         * @brief Change color style to green when PID values of Pitch to dT are send to UAS
         *
         * @param
         */
void SlugsPIDControl::changeColor_GREEN_Pitch2dT_groupBox()
{
469 470 471 472
    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
473
#ifdef MAVLINK_ENABLED_SLUGS
474 475 476
          //create the packet
          pidMessage.target = activeUAS->getUASID();
          pidMessage.idx = 8;
477 478 479
          pidMessage.pVal = ui->P2dT_FF_set->text().toFloat();
          pidMessage.iVal = 0;//ui->dR_I_set->text().toFloat();
          pidMessage.dVal = 0;//ui->dR_D_set->text().toFloat();
480 481 482 483 484

          mavlink_message_t msg;

          mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage);
          slugsMav->sendMessage(msg);
485
#endif
486 487
        ui->Pitch2dTFFterm_groupBox->setStyleSheet(GREENcolorStyle);
    }
488 489 490 491 492 493 494 495 496 497 498 499
}

/**
         * @brief Connects the SIGNALS from the editline to SLOT Pitch2dT_groupBox
         *
         * @param
         */
void SlugsPIDControl::connect_Pitch2dT_LineEdit()
{
    connect(ui->P2dT_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_Pitch2dT_groupBox(QString)));
}

500 501 502 503 504 505
void SlugsPIDControl::get_Pitch2dT_PID()
{
    qDebug() << "\nSend Message = Pitch to dT ";
  sendMessagePIDStatus(8);
}

506 507 508
#ifdef MAVLINK_ENABLED_SLUGS

void SlugsPIDControl::recibeMensaje(int systemId, const mavlink_action_ack_t& action)
509
{
510 511 512 513 514 515 516 517 518 519
    ui->recepcion_label->setText(QString::number(action.action) + ":" + QString::number(action.result));
}

void SlugsPIDControl::receivePidValues(int systemId, const mavlink_pid_t &pidValues)
{
    Q_UNUSED(systemId);

     qDebug() << "\nACTUALIZANDO GUI = " << pidValues.idx;
    switch(pidValues.idx)
    {
520
    case 0:
521 522 523 524
        ui->dT_P_get->setText(QString::number(pidValues.pVal));
        ui->dT_I_get->setText(QString::number(pidValues.iVal));
        ui->dT_D_get->setText(QString::number(pidValues.dVal));
        break;
525
    case 1:
526 527 528 529
        ui->HELPComm_P_get->setText(QString::number(pidValues.pVal));
        ui->HELPComm_I_get->setText(QString::number(pidValues.iVal));
        ui->HELPComm_FF_get->setText(QString::number(pidValues.dVal));
        break;
530
    case 2:
531 532 533 534
        ui->dE_P_get->setText(QString::number(pidValues.pVal));
        ui->dE_I_get->setText(QString::number(pidValues.iVal));
        ui->dE_D_get->setText(QString::number(pidValues.dVal));
        break;
535
    case 3:
536 537 538 539
        ui->dR_P_get->setText(QString::number(pidValues.pVal));
        ui->dR_I_get->setText(QString::number(pidValues.iVal));
        ui->dR_D_get->setText(QString::number(pidValues.dVal));
        break;
540
    case 4:
541 542 543 544
        ui->dA_P_get->setText(QString::number(pidValues.pVal));
        ui->dA_I_get->setText(QString::number(pidValues.iVal));
        ui->dA_D_get->setText(QString::number(pidValues.dVal));
        break;
545
    case 8:
546
        ui->P2dT_FF_get->setText(QString::number(pidValues.pVal));
547

548 549 550 551 552 553 554 555
        break;

    default:
        qDebug() << "\nSLUGS RECEIVED AND SHOW PID type ID = " << pidValues.idx;
          break;

    }
}
556
#endif // MAVLINK_ENABLED_SLUG
557 558 559 560


void SlugsPIDControl::sendMessagePIDStatus(int PIDtype)
{
561
#ifdef MAVLINK_ENABLED_SLUGS
562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642
    //ToDo remplace actionId values


    SlugsMAV* slugsMav = dynamic_cast<SlugsMAV*>(activeUAS);

    if (slugsMav != NULL)
    {
        mavlink_message_t msg;
        qDebug() << "\n Send Message SLUGS PID with loop index  = " << PIDtype;

        switch(PIDtype)
        {
            case 0: //Air Speed PID values Request
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 0;



                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;

            case 1: //Heigth Error lo Pitch Comm PID values request
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 1;


                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;

            case 2://Pitch Followei PID values Request
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 2;


                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;

            case 3:// Yaw Damper PID values  request
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 3;


                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;

            case 4: // Roll Control PID values request
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 4;


                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;

            case 8: // Pitch to dT FF term
                actionSlugs.target = activeUAS->getUASID();
                actionSlugs.actionId = 9;
                actionSlugs.actionVal = 8;


                mavlink_msg_slugs_action_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &actionSlugs);
                slugsMav->sendMessage(msg);
                break;


            default:
                qDebug() << "\nSLUGS RECEIVED PID type ID = " << PIDtype;
                  break;


        }
    }
643 644
#else
    Q_UNUSED(PIDtype);
645
    #endif // MAVLINK_ENABLED_SLUG
646
}
647

648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714
void SlugsPIDControl::slugsGetGeneral()
{
    valuesMutex.lock();
    switch(counterRefreshGet)
    {
    case 1:
       ui->dT_PID_get_pushButton->click();
        break;
    case 2:
        ui->HELPComm_PDI_get_pushButton->click();
        break;
    case 3:
        ui->dE_PID_get_pushButton->click();
        break;
    case 4:
        ui->dR_PDI_get_pushButton->click();
        break;
    case 5:
        ui->dA_PID_get_pushButton->click();
        break;
    case 6:
        ui->Pitch2dT_PDI_get_pushButton->click();
        break;
    default:
         refreshTimerGet->stop();
        break;


    }

    counterRefreshGet++;
    valuesMutex.unlock();

}

void SlugsPIDControl::slugsSetGeneral()
{
    valuesMutex.lock();
    switch(counterRefreshSet)
    {
    case 1:
        ui->dT_PID_set_pushButton->click();
        break;
    case 2:
        ui->HELPComm_PDI_set_pushButton->click();
        break;
    case 3:
        ui->dE_PID_set_pushButton->click();
        break;
    case 4:
        ui->dR_PDI_set_pushButton->click();
        break;
    case 5:
        ui->dA_PID_set_pushButton->click();
        break;
    case 6:
        ui->Pitch2dT_PDI_set_pushButton->click();
        break;
    default:
        refreshTimerSet->stop();
        break;

    }

    counterRefreshSet++;
    valuesMutex.unlock();
}
715 716


717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735
void SlugsPIDControl::slugsTimerStartSet()
{
    counterRefreshSet = 1;
    refreshTimerSet->start();

}

void SlugsPIDControl::slugsTimerStartGet()
{
    counterRefreshGet = 1;
    refreshTimerGet->start();

}
void SlugsPIDControl::slugsTimerStop()
{
//    refreshTimerGet->stop();
//     counterRefresh = 1;

}