From 5a8a8b2b031e9a3371ab7ad94b29e79df01ed239 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Fri, 26 Nov 2010 14:41:54 -0600 Subject: [PATCH] not working send messages in SlugsPIDControl --- src/uas/SlugsMAV.cc | 16 +- src/uas/SlugsMAV.h | 7 +- src/ui/MainWindow.cc | 2 +- src/ui/SlugsPIDControl.cpp | 294 ++++++++++++++++++++++++++++++++-- src/ui/SlugsPIDControl.h | 57 +++++++ src/ui/SlugsPIDControl.ui | 64 ++++---- src/ui/SlugsVideoCamControl.h | 8 +- 7 files changed, 391 insertions(+), 57 deletions(-) diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 19beb486a..ff02444f8 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -39,6 +39,7 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : memset(&mlActionAck,0, sizeof(mavlink_action_ack_t)); memset(&mlAction ,0, sizeof(mavlink_slugs_action_t)); + updateRoundRobin = 0; @@ -154,6 +155,12 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_PID: //182 + memset(&mlSinglePid,0,sizeof(mavlink_pid_t)); + mavlink_msg_pid_decode(&message, &mlSinglePid); + qDebug() << "\nSLUGS RECEIVED PID Message = "<show(); } diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index 001a9e734..2b25d9c42 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -22,15 +22,6 @@ SlugsPIDControl::SlugsPIDControl(QWidget *parent) : setRedColorStyle(); setGreenColorStyle(); -// connect_set_pushButtons(); -// connect_AirSpeed_LineEdit(); -// connect_PitchFollowei_LineEdit(); -// connect_RollControl_LineEdit(); -// connect_HeigthError_LineEdit(); -// connect_YawDamper_LineEdit(); -// connect_Pitch2dT_LineEdit(); - - } /** @@ -48,6 +39,7 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas) if (slugsMav != NULL) { connect(slugsMav,SIGNAL(slugsActionAck(int,const mavlink_action_ack_t&)),this,SLOT(recibeMensaje(int,mavlink_action_ack_t))); + connect(slugsMav,SIGNAL(slugsPidValues(int,mavlink_pid_t)),this, SLOT(receivePidValues(int,mavlink_pid_t)) ); } #endif // MAVLINK_ENABLED_SLUG @@ -73,6 +65,7 @@ void SlugsPIDControl::connect_editLinesPDIvalues() if(activeUAS) { connect_set_pushButtons(); + connect_get_pushButtons(); connect_AirSpeed_LineEdit(); connect_PitchFollowei_LineEdit(); connect_RollControl_LineEdit(); @@ -140,6 +133,21 @@ void SlugsPIDControl::connect_set_pushButtons() } +/** + *@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 void SlugsPIDControl::connect_AirSpeed_LineEdit() { connect(ui->dT_P_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_AirSpeed_groupBox(QString))); @@ -178,7 +186,16 @@ void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() } } +void SlugsPIDControl::get_AirSpeed_PID() +{ + qDebug() << "\nSend Message = Air Speed "; + sendMessagePIDStatus(0); + +} + + +// Functions for PitchFollowei GroupBox void SlugsPIDControl::connect_PitchFollowei_LineEdit() { connect(ui->dE_P_set, SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_PitchFollowei_groupBox(QString))); @@ -195,9 +212,34 @@ void SlugsPIDControl::changeColor_RED_PitchFollowei_groupBox(QString text) void SlugsPIDControl::changeColor_GREEN_PitchFollowei_groupBox() { - ui->PitchFlowei_groupBox->setStyleSheet(GREENcolorStyle); + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //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); + + ui->PitchFlowei_groupBox->setStyleSheet(GREENcolorStyle); + } } +void SlugsPIDControl::get_PitchFollowei_PID() +{ + qDebug() << "\nSend Message = Pitch Followei "; + sendMessagePIDStatus(2); + +} + + // Functions for Roll Control GroupBox /** * @brief Change color style to red when PID values of Roll Control are edited @@ -218,7 +260,24 @@ void SlugsPIDControl::changeColor_RED_RollControl_groupBox(QString text) */ void SlugsPIDControl::changeColor_GREEN_RollControl_groupBox() { - ui->RollControl_groupBox->setStyleSheet(GREENcolorStyle); + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //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); + + ui->RollControl_groupBox->setStyleSheet(GREENcolorStyle); + } } /** @@ -233,6 +292,13 @@ void SlugsPIDControl::connect_RollControl_LineEdit() connect(ui->dA_D_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_RollControl_groupBox(QString))); } +void SlugsPIDControl::get_RollControl_PID() +{ + qDebug() << "\nSend Message = Roll Control "; + sendMessagePIDStatus(4); +} + + // Functions for Heigth Error GroupBox /** @@ -254,7 +320,24 @@ void SlugsPIDControl::changeColor_RED_HeigthError_groupBox(QString text) */ void SlugsPIDControl::changeColor_GREEN_HeigthError_groupBox() { - ui->HeightErrorLoPitch_groupBox->setStyleSheet(GREENcolorStyle); + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //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); + + ui->HeightErrorLoPitch_groupBox->setStyleSheet(GREENcolorStyle); + } } /** @@ -269,6 +352,12 @@ void SlugsPIDControl::connect_HeigthError_LineEdit() connect(ui->HELPComm_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_HeigthError_groupBox(QString))); } +void SlugsPIDControl::get_HeigthError_PID() +{ + qDebug() << "\nSend Message = Heigth Error "; + sendMessagePIDStatus(1); +} + // Functions for Yaw Damper GroupBox /** @@ -290,7 +379,24 @@ void SlugsPIDControl::changeColor_RED_YawDamper_groupBox(QString text) */ void SlugsPIDControl::changeColor_GREEN_YawDamper_groupBox() { - ui->YawDamper_groupBox->setStyleSheet(GREENcolorStyle); + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //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); + + ui->YawDamper_groupBox->setStyleSheet(GREENcolorStyle); + } } /** @@ -307,6 +413,13 @@ void SlugsPIDControl::connect_YawDamper_LineEdit() } +void SlugsPIDControl::get_YawDamper_PID() +{ + qDebug() << "\nSend Message = Yaw Damper "; + sendMessagePIDStatus(3); +} + + // Functions for Pitch to dT GroupBox /** * @brief Change color style to red when PID values of Pitch to dT are edited @@ -327,7 +440,23 @@ void SlugsPIDControl::changeColor_RED_Pitch2dT_groupBox(QString text) */ void SlugsPIDControl::changeColor_GREEN_Pitch2dT_groupBox() { - ui->Pitch2dTFFterm_groupBox->setStyleSheet(GREENcolorStyle); + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //create the packet + pidMessage.target = activeUAS->getUASID(); + pidMessage.idx = 8; + 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); + ui->Pitch2dTFFterm_groupBox->setStyleSheet(GREENcolorStyle); + } } /** @@ -340,11 +469,146 @@ void SlugsPIDControl::connect_Pitch2dT_LineEdit() connect(ui->P2dT_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_Pitch2dT_groupBox(QString))); } +void SlugsPIDControl::get_Pitch2dT_PID() +{ + qDebug() << "\nSend Message = Pitch to dT "; + sendMessagePIDStatus(8); +} + #ifdef MAVLINK_ENABLED_SLUGS void SlugsPIDControl::recibeMensaje(int systemId, const mavlink_action_ack_t& action) { - ui->recepcion_label->setText(QString::number(action.action)); + 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) + { + case 1: + 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; + case 2: + 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; + case 3: + 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; + case 4: + 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; + case 5: + 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; + case 9: + ui->P2dT_FF_get->setText(QString::number(pidValues.pVal)); + break; + + default: + qDebug() << "\nSLUGS RECEIVED AND SHOW PID type ID = " << pidValues.idx; + break; + + } +} + + +void SlugsPIDControl::sendMessagePIDStatus(int PIDtype) +{ + //ToDo remplace actionId values + + + SlugsMAV* slugsMav = dynamic_cast(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; + + + } + } } #endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h index 600ffc6f0..d701de07d 100644 --- a/src/ui/SlugsPIDControl.h +++ b/src/ui/SlugsPIDControl.h @@ -48,6 +48,13 @@ public slots: */ void connect_set_pushButtons(); + /** + * @brief Connect Set pushButtons to change the color GroupBox + * + * @param + */ + void connect_get_pushButtons(); + /** * @brief Connect Edition Lines for PID Values * @@ -55,6 +62,13 @@ public slots: */ void connect_editLinesPDIvalues(); + /** + * @brief send a PDI request message to UAS + * + * @param + */ + void sendMessagePIDStatus(int PIDtype); + // Fuctions for Air Speed GroupBox /** * @brief Change color style to red when PID values of Air Speed are edited @@ -75,6 +89,13 @@ public slots: * @param */ void connect_AirSpeed_LineEdit(); + /** + * @brief get message PID Air Speed(loop index = 0) from UAS + * + * @param + */ + void get_AirSpeed_PID(); + // Functions for Pitch Followei GroupBox /** @@ -96,6 +117,13 @@ public slots: * @param */ void connect_PitchFollowei_LineEdit(); + /** + * @brief get message PID Pitch Followei(loop index = 2) from UAS + * + * @param + */ + void get_PitchFollowei_PID(); + // Functions for Roll Control GroupBox /** @@ -117,6 +145,13 @@ public slots: * @param */ void connect_RollControl_LineEdit(); + /** + * @brief get message PID Roll Control(loop index = 4) from UAS + * + * @param + */ + void get_RollControl_PID(); + // Functions for Heigth Error GroupBox /** @@ -138,6 +173,12 @@ public slots: * @param */ void connect_HeigthError_LineEdit(); + /** + * @brief get message PID Heigth Error(loop index = 1) from UAS + * + * @param + */ + void get_HeigthError_PID(); // Functions for Yaw Damper GroupBox /** @@ -159,6 +200,14 @@ public slots: * @param */ void connect_YawDamper_LineEdit(); + /** + * @brief get message PID Yaw Damper(loop index = 3) from UAS + * + * @param + */ + void get_YawDamper_PID(); + + // Functions for Pitch to dT GroupBox /** @@ -180,6 +229,12 @@ public slots: * @param */ void connect_Pitch2dT_LineEdit(); + /** + * @brief get message PID Pitch2dT(loop index = 8) from UAS + * + * @param + */ + void get_Pitch2dT_PID(); //Create, send and get Messages PID @@ -187,6 +242,7 @@ public slots: #ifdef MAVLINK_ENABLED_SLUGS void recibeMensaje(int systemId, const mavlink_action_ack_t& action); + void receivePidValues(int systemId, const mavlink_pid_t& pidValues); #endif // MAVLINK_ENABLED_SLUG @@ -206,6 +262,7 @@ private: //SlugsMav Message mavlink_pid_t pidMessage; + mavlink_slugs_action_t actionSlugs; }; #endif // SLUGSPIDCONTROL_H diff --git a/src/ui/SlugsPIDControl.ui b/src/ui/SlugsPIDControl.ui index 419b3c7b9..e7971956a 100644 --- a/src/ui/SlugsPIDControl.ui +++ b/src/ui/SlugsPIDControl.ui @@ -72,14 +72,14 @@ - dT_P_set + 0.0 - dT_P_get + 0.0 true @@ -89,14 +89,14 @@ - dT_I_set + 0.0 - dT_I_get + 0.0 true @@ -106,14 +106,14 @@ - dT_D_set + 0.0 - dT_D_get + 0.0 true @@ -242,14 +242,14 @@ - dE_P_set + 0.0 - dE_P_get + 0.0 true @@ -259,14 +259,14 @@ - dE_I_set + 0.0 - dE_I_get + 0.0 true @@ -276,14 +276,14 @@ - dE_D_set + 0.0 - dE_D_get + 0.0 true @@ -405,14 +405,14 @@ - dA_P_set + 0.0 - dA_P_get + 0.0 true @@ -422,14 +422,14 @@ - dA_I_set + 0.0 - dA_I_get + 0.0 true @@ -439,14 +439,14 @@ - dA_D_set + 0.0 - dA_D_get + 0.0 true @@ -545,14 +545,14 @@ - HELPComm_P_set + 0.0 - HELPComm_P_get + 0.0 true @@ -562,14 +562,14 @@ - HELPComm_I_set + 0.0 - HELPComm_I_get + 0.0 true @@ -646,14 +646,14 @@ - HELPComm_FF_set + 0.0 - HELPComm_FF_get + 0.0 true @@ -775,14 +775,14 @@ - dR_P_set + 0.0 - dR_P_get + 0.0 true @@ -792,14 +792,14 @@ - dR_I_set + 0.0 - dR_I_get + 0.0 true @@ -809,14 +809,14 @@ - dR_D_set + 0.0 - dR_D_get + 0.0 true @@ -910,14 +910,14 @@ - P2dT_FF_set + 0.0 - P2dT_FF_get + 0.0 true diff --git a/src/ui/SlugsVideoCamControl.h b/src/ui/SlugsVideoCamControl.h index fe7b24750..0f67187a1 100644 --- a/src/ui/SlugsVideoCamControl.h +++ b/src/ui/SlugsVideoCamControl.h @@ -23,13 +23,7 @@ public: protected: virtual void mousePressEvent(QMouseEvent* event); virtual void mouseReleaseEvent(QMouseEvent* event); - //void mouseMoveEvent(QMouseEvent* event); - void mouseMoveEvent(QMouseEvent* event); - // virtual void wheelEvent(QWheelEvent* event); - //virtual void resizeEvent(QResizeEvent* event); - - - + void mouseMoveEvent(QMouseEvent* event); private: Ui::SlugsVideoCamControl *ui; -- 2.22.0