diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 3d9468c1f3220637a82d3d274d1a34645e6f3acf..5579dc04244f15292b712ee5f5e525b05aad173e 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -139,7 +139,6 @@ FORMS += src/ui/MainWindow.ui \ src/ui/WaypointGlobalView.ui \ src/ui/SlugsDataSensorView.ui \ src/ui/SlugsHilSim.ui \ - src/ui/SlugsDataSensorView.ui \ src/ui/SlugsPIDControl.ui \ src/ui/SlugsVideoCamControl.ui INCLUDEPATH += src \ diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 71efcfe33a742bbc593336dd8931432732226cfb..5e7a6df3b4b3ce68255753022be81be4c315d8e8 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -89,7 +89,8 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P } #else // *nix (Linux, MacOS tested) serial port support - port = new QextSerialPort(porthandle, QextSerialPort::Polling); + //port = new QextSerialPort(porthandle, QextSerialPort::Polling); + port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time port->setBaudRate(baudrate); port->setFlowControl(flow); diff --git a/src/configuration.h b/src/configuration.h index a8c70a607173ecf65c732aec825cf668a656cb09..3420e0f39a6bf6678f63ee613042781b1df6142f 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -1,8 +1,10 @@ #ifndef CONFIGURATION_H #define CONFIGURATION_H +#include "mavlink.h" + /** @brief Polling interval in ms */ -#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES_QGC +#ifdef MAVLINK_ENABLED_SLUGS #define SERIAL_POLL_INTERVAL 7 #else #define SERIAL_POLL_INTERVAL 2 diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 877d1beb4bd731d94ce783805f476ad43702eca2..19beb486adb624593f8518e69d669d2b3624d4d5 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -6,7 +6,46 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id)//, // Place other initializers here { + widgetTimer = new QTimer (this); + widgetTimer->setInterval(SLUGS_UPDATE_RATE); + connect (widgetTimer, SIGNAL(timeout()), + this, SLOT(emitSignals())); + + widgetTimer->start(); + + // clear all the state structures + memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t)); + +#ifdef MAVLINK_ENABLED_SLUGS + + + memset(&mlGpsData, 0, sizeof(mavlink_gps_raw_t)); + memset(&mlCpuLoadData, 0, sizeof(mavlink_cpu_load_t)); + memset(&mlAirData, 0, sizeof(mavlink_air_data_t)); + memset(&mlSensorBiasData, 0, sizeof(mavlink_sensor_bias_t)); + memset(&mlDiagnosticData, 0, sizeof(mavlink_diagnostic_t)); + memset(&mlPilotConsoleData, 0, sizeof(mavlink_pilot_console_t)); + memset(&mlFilteredData ,0, sizeof(mavlink_filtered_data_t)); + memset(&mlBoot ,0, sizeof(mavlink_boot_t)); + memset(&mlGpsDateTime ,0, sizeof(mavlink_gps_date_time_t)); + memset(&mlApMode ,0, sizeof(mavlink_set_mode_t)); + memset(&mlPwmCommands ,0, sizeof(mavlink_pwm_commands_t)); + memset(&mlPidValues ,0, sizeof(mavlink_pid_values_t)); + memset(&mlSinglePid ,0, sizeof(mavlink_pid_t)); + memset(&mlNavigation ,0, sizeof(mavlink_slugs_navigation_t)); + memset(&mlDataLog ,0, sizeof(mavlink_data_log_t)); + memset(&mlPassthrough ,0, sizeof(mavlink_ctrl_srfc_pt_t)); + memset(&mlActionAck,0, sizeof(mavlink_action_ack_t)); + memset(&mlAction ,0, sizeof(mavlink_slugs_action_t)); + + updateRoundRobin = 0; + + + + uasId = id; + + #endif } /** @@ -20,336 +59,186 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) : void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { // Let UAS handle the default message set - UAS::receiveMessage(link, message); + //UAS::receiveMessage(link, message); + // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: - { - qDebug() << "SLUGS RECEIVED HEARTBEAT"; - break; - } - -#ifdef MAVLINK_ENABLED_SLUGS - case MAVLINK_MSG_ID_CPU_LOAD://170 - { - mavlink_cpu_load_t cpu_load; - quint64 time = getUnixTime(0); - mavlink_msg_cpu_load_decode(&message,&cpu_load); - - emit valueChanged(uasId, tr("SensorDSC Load"), cpu_load.sensLoad, time); - emit valueChanged(uasId, tr("ControlDSC Load"), cpu_load.ctrlLoad, time); - emit valueChanged(uasId, tr("Battery Volt"), cpu_load.batVolt, time); - - emit slugsCPULoad(uasId, - cpu_load.sensLoad, - cpu_load.ctrlLoad, - cpu_load.batVolt, - 0); -//qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_CPU_LOAD 170"; - break; - } - case MAVLINK_MSG_ID_AIR_DATA://171 - { - mavlink_air_data_t air_data; - quint64 time = getUnixTime(0); - mavlink_msg_air_data_decode(&message,&air_data); - emit valueChanged(uasId, tr("Dynamic Pressure"), air_data.dynamicPressure,time); - emit valueChanged(uasId, tr("Static Pressure"),air_data.staticPressure, time); - emit valueChanged(uasId, tr("Temp"),air_data.temperature,time); - - emit slugsAirData(uasId, - air_data.dynamicPressure, - air_data.staticPressure, - air_data.temperature, - time); -//qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_AIR_DATA 171"; - break; - } - case MAVLINK_MSG_ID_SENSOR_BIAS://172 - { - mavlink_sensor_bias_t sensor_bias; - quint64 time = getUnixTime(0); - mavlink_msg_sensor_bias_decode(&message,&sensor_bias); - emit valueChanged(uasId,tr("Ax Bias"),sensor_bias.axBias, time); - emit valueChanged(uasId,tr("Ay Bias"),sensor_bias.ayBias,time); - emit valueChanged(uasId,tr("Az Bias"),sensor_bias.azBias,time); - emit valueChanged(uasId,tr("Gx Bias"),sensor_bias.gxBias,time); - emit valueChanged(uasId,tr("Gy Bias"),sensor_bias.gyBias,time); - emit valueChanged(uasId,tr("Gz Bias"),sensor_bias.gzBias,time); - - emit slugsSensorBias(uasId, - sensor_bias.axBias, - sensor_bias.ayBias, - sensor_bias.azBias, - sensor_bias.gxBias, - sensor_bias.gyBias, - sensor_bias.gzBias, - 0); - // qDebug()<<"------------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SENSOR_BIAS 172"; - break; - } - case MAVLINK_MSG_ID_DIAGNOSTIC://173 + case MAVLINK_MSG_ID_HEARTBEAT: + emit heartbeat(this); + // Set new type if it has changed + if (this->type != mavlink_msg_heartbeat_get_type(&message)) { - mavlink_diagnostic_t diagnostic; - quint64 time = getUnixTime(0); - mavlink_msg_diagnostic_decode(&message,&diagnostic); - emit valueChanged(uasId,tr("Diag F1"),diagnostic.diagFl1,time); - emit valueChanged(uasId,tr("Diag F2"),diagnostic.diagFl2,time); - emit valueChanged(uasId,tr("Diag F3"),diagnostic.diagFl3,time); - emit valueChanged(uasId,tr("Diag S1"),diagnostic.diagSh1,time); - emit valueChanged(uasId,tr("Diag S2"),diagnostic.diagSh2,time); - emit valueChanged(uasId,tr("Diag S3"),diagnostic.diagSh3,time); - - emit slugsDiagnostic(uasId, - diagnostic.diagFl1, - diagnostic.diagFl2, - diagnostic.diagFl3, - diagnostic.diagSh1, - diagnostic.diagSh2, - diagnostic.diagSh3, - 0); -//qDebug()<<"------->>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_DIAGNOSTIC 173"; - break; + this->type = mavlink_msg_heartbeat_get_type(&message); + emit systemTypeSet(this, type); } - case MAVLINK_MSG_ID_PILOT_CONSOLE://174 - { - mavlink_pilot_console_t pilot; - quint64 time = getUnixTime(0); - mavlink_msg_pilot_console_decode(&message,&pilot); - emit valueChanged(uasId,"dt",pilot.dt,time); - emit valueChanged(uasId,"dla",pilot.dla,time); - emit valueChanged(uasId,"dra",pilot.dra,time); - emit valueChanged(uasId,"dr",pilot.dr,time); - emit valueChanged(uasId,"de",pilot.de,time); - - emit slugsPilotConsolePWM(uasId, - pilot.dt, - pilot.dla, - pilot.dra, - pilot.dr, - pilot.de, - 0); - // qDebug()<<"---------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PILOT_CONSOLE 174"; - + break; + case MAVLINK_MSG_ID_RAW_IMU: + mavlink_msg_raw_imu_decode(&message, &mlRawImuData); break; - } - case MAVLINK_MSG_ID_PWM_COMMANDS://175 - { - mavlink_pwm_commands_t pwm; - quint64 time = getUnixTime(0); - mavlink_msg_pwm_commands_decode(&message,&pwm); - emit valueChanged(uasId,"dt_c",pwm.dt_c,time); - emit valueChanged(uasId,"dla_c",pwm.dla_c,time); - emit valueChanged(uasId,"dra_c",pwm.dra_c,time); - emit valueChanged(uasId,"dr_c",pwm.dr_c,time); - emit valueChanged(uasId,"dle_c",pwm.dle_c,time); - emit valueChanged(uasId,"dre_c",pwm.dre_c,time); - emit valueChanged(uasId,"dlf_c",pwm.dlf_c,time); - emit valueChanged(uasId,"drf_c",pwm.drf_c,time); - emit valueChanged(uasId,"da1_c",pwm.aux1,time); - emit valueChanged(uasId,"da2_c",pwm.aux2,time); - - emit slugsPWM(uasId, - pwm.dt_c, - pwm.dla_c, - pwm.dra_c, - pwm.dr_c, - pwm.dle_c, - pwm.dre_c, - pwm.dlf_c, - pwm.drf_c, - pwm.aux1, - pwm.aux2, - 0); -//qDebug()<<"----------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PWM_COMMANDS 175"; - break; - } - case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176 - { - mavlink_slugs_navigation_t nav; - quint64 time = getUnixTime(0); - mavlink_msg_slugs_navigation_decode(&message,&nav); - emit valueChanged(uasId,"u_m",nav.u_m,time); - emit valueChanged(uasId,"phi_c",nav.phi_c,time); - emit valueChanged(uasId,"theta_c",nav.theta_c,time); - emit valueChanged(uasId,"psiDot_c",nav.psiDot_c,time); - emit valueChanged(uasId,"ay_body",nav.ay_body,time); - emit valueChanged(uasId,"totalDist",nav.totalDist,time); - emit valueChanged(uasId,"dist2Go",nav.dist2Go,time); - emit valueChanged(uasId,"fromWP",nav.fromWP,time); - emit valueChanged(uasId,"toWP",nav.toWP,time); - - emit slugsNavegation(uasId, - nav.u_m, - nav.phi_c, - nav.theta_c, - nav.psiDot_c, - nav.ay_body, - nav.totalDist, - nav.dist2Go, - nav.fromWP, - nav.toWP, - time); - - // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SLUGS_NAVIGATION 176"; - - break; - } - case MAVLINK_MSG_ID_DATA_LOG://177 - { - mavlink_data_log_t dataLog; - quint64 time = getUnixTime(0); - mavlink_msg_data_log_decode(&message,&dataLog); - emit valueChanged(uasId,"fl_1",dataLog.fl_1,time); - emit valueChanged(uasId,"fl_2",dataLog.fl_2,time); - emit valueChanged(uasId,"fl_3",dataLog.fl_3,time); - emit valueChanged(uasId,"fl_4",dataLog.fl_4,time); - emit valueChanged(uasId,"fl_5",dataLog.fl_5,time); - emit valueChanged(uasId,"fl_6",dataLog.fl_6,time); +#ifdef MAVLINK_ENABLED_SLUGS - emit slugsDataLog(uasId, - dataLog.fl_1, - dataLog.fl_2, - dataLog.fl_3, - dataLog.fl_4, - dataLog.fl_5, - dataLog.fl_6, - time); + case MAVLINK_MSG_ID_BOOT: + mavlink_msg_boot_decode(&message,&mlBoot); + emit slugsBootMsg(uasId, mlBoot); + break; - // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO LOG DATA 177"; + case MAVLINK_MSG_ID_ATTITUDE: + mavlink_msg_attitude_decode(&message, &mlAttitude); + break; + case MAVLINK_MSG_ID_GPS_RAW: + mavlink_msg_gps_raw_decode(&message, &mlGpsData); break; - } - - case MAVLINK_MSG_ID_FILTERED_DATA://178 - { - - mavlink_filtered_data_t fil; - quint64 time = getUnixTime(0); - mavlink_msg_filtered_data_decode(&message,&fil); - emit valueChanged(uasId,"aX",fil.aX,time); - emit valueChanged(uasId,"aY",fil.aY,time); - emit valueChanged(uasId,"aZ",fil.aZ,time); - emit valueChanged(uasId,"gX",fil.gX,time); - emit valueChanged(uasId,"gY",fil.gY,time); - emit valueChanged(uasId,"gZ",fil.gZ,time); - emit valueChanged(uasId,"mX",fil.mX,time); - emit valueChanged(uasId,"mY",fil.mY,time); - emit valueChanged(uasId,"mZ",fil.mZ,time); - - emit slugsFilteredData(uasId, - fil.aX, - fil.aY, - fil.aZ, - fil.gX, - fil.gY, - fil.gZ, - fil.mX, - fil.mY, - fil.mZ, - time); - + case MAVLINK_MSG_ID_ACTION_ACK: // 62 + mavlink_msg_action_ack_decode(&message,&mlActionAck); + break; - // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO filtering data 178"; + case MAVLINK_MSG_ID_CPU_LOAD: //170 + mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData); + break; + case MAVLINK_MSG_ID_AIR_DATA: //171 + mavlink_msg_air_data_decode(&message,&mlAirData); break; - } - case MAVLINK_MSG_ID_GPS_DATE_TIME://179 - { - mavlink_gps_date_time_t gps; - quint64 time = getUnixTime(0); - mavlink_msg_gps_date_time_decode(&message,&gps); - emit valueChanged(uasId,"year",gps.year,time); - emit valueChanged(uasId,"month",gps.month,time); - emit valueChanged(uasId,"day",gps.day,time); - emit valueChanged(uasId,"hour",gps.hour,time); - emit valueChanged(uasId,"min",gps.min,time); - emit valueChanged(uasId,"sec",gps.sec,time); - emit valueChanged(uasId,"visSat",gps.visSat,time); + case MAVLINK_MSG_ID_SENSOR_BIAS: //172 + mavlink_msg_sensor_bias_decode(&message,&mlSensorBiasData); + break; - emit slugsGPSDateTime(uasId, - gps.year, - gps.month, - gps.day, - gps.hour, - gps.min, - gps.sec, - gps.visSat, - time); + case MAVLINK_MSG_ID_DIAGNOSTIC: //173 + mavlink_msg_diagnostic_decode(&message,&mlDiagnosticData); + break; + case MAVLINK_MSG_ID_PILOT_CONSOLE: //174 + mavlink_msg_pilot_console_decode(&message,&mlPilotConsoleData); + break; + case MAVLINK_MSG_ID_PWM_COMMANDS: //175 + mavlink_msg_pwm_commands_decode(&message,&mlPwmCommands); + break; - // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_GPS_DATE_TIME 179"; + case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176 + mavlink_msg_slugs_navigation_decode(&message,&mlNavigation); + break; + case MAVLINK_MSG_ID_DATA_LOG: //177 + mavlink_msg_data_log_decode(&message,&mlDataLog); + break; + case MAVLINK_MSG_ID_FILTERED_DATA: //178 + mavlink_msg_filtered_data_decode(&message,&mlFilteredData); + break; + case MAVLINK_MSG_ID_GPS_DATE_TIME: //179 + mavlink_msg_gps_date_time_decode(&message,&mlGpsDateTime); break; - } - case MAVLINK_MSG_ID_ACTION_ACK: - { - qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_ACTION_ACK"; - mavlink_action_ack_t ack; - - mavlink_msg_action_ack_decode(&message,&ack); - emit slugsActionAck(uasId,ack.action,ack.result); - } -// case MAVLINK_MSG_ID_MID_LVL_CMDS://180 -// { + case MAVLINK_MSG_ID_MID_LVL_CMDS: //180 -// // qDebug()<<"------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_MID_LVL_CMDS 180"; + break; + case MAVLINK_MSG_ID_CTRL_SRFC_PT: //181 + break; -// break; -// } -// case MAVLINK_MSG_ID_CTRL_SRFC_PT://181 -// { + case MAVLINK_MSG_ID_PID: //182 + break; -// // qDebug()<<"--------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_CTRL_SRFC_PT 181"; + case MAVLINK_MSG_ID_SLUGS_ACTION: //183 + break; +#endif -// break; -// } -// case MAVLINK_MSG_ID_PID://182 -// { + default: +// qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; + break; + } +} -// // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_PID 182"; +void SlugsMAV::emitSignals (void){ +#ifdef MAVLINK_ENABLED_SLUGS + switch(updateRoundRobin){ + case 1: + emit slugsCPULoad(uasId, mlCpuLoadData); + emit slugsSensorBias(uasId,mlSensorBiasData); + break; + + case 2: + emit slugsAirData(uasId, mlAirData); + emit slugsDiagnostic(uasId,mlDiagnosticData); + break; + + case 3: + emit slugsPilotConsolePWM(uasId,mlPilotConsoleData); + emit slugsPWM(uasId, mlPwmCommands); + break; + + case 4: + emit slugsNavegation(uasId, mlNavigation); + emit slugsDataLog(uasId, mlDataLog); + break; + + case 5: + emit slugsFilteredData(uasId,mlFilteredData); + emit slugsGPSDateTime(uasId, mlGpsDateTime); + break; + case 6: + emit slugsActionAck(uasId,mlActionAck); + emit emitGpsSignals(); + break; + } -// break; -// } -// case MAVLINK_MSG_ID_SLUGS_ACTION://183 -// { + emit slugsAttitude(uasId, mlAttitude); + emit attitudeChanged(this, + mlAttitude.roll, + mlAttitude.pitch, + mlAttitude.yaw, + 0.0); +#endif + emit slugsRawImu(uasId, mlRawImuData); -// // qDebug()<<"-------->>>>>>>>>>>>>>> EMITIENDO MAVLINK_MSG_ID_SLUGS_ACTION 183"; + // wrap around + updateRoundRobin = updateRoundRobin > 10? 1: updateRoundRobin + 1; -// break; -// } -#endif +} - default: - //qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; - break; +#ifdef MAVLINK_ENABLED_SLUGS +void SlugsMAV::emitGpsSignals (void){ + + if (mlGpsData.fix_type > 0){ + emit globalPositionChanged(this, + mlGpsData.lon, + mlGpsData.lat, + mlGpsData.alt, + 0.0); + + // Smaller than threshold and not NaN + if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ + emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); + } else { + emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); + } } } + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index f25e6b28b339636c3e2895cbe91a66dc1f36be39..4fc690f92b614c720951dcda0c500d9d42fb9da0 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -26,7 +26,9 @@ This file is part of the QGROUNDCONTROL project #include "UAS.h" #include "mavlink.h" +#include +#define SLUGS_UPDATE_RATE 100 // in ms class SlugsMAV : public UAS { Q_OBJECT @@ -38,111 +40,81 @@ public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); + void emitSignals (void); + + + signals: - // ESPECIAL SLUGS MESSAGE - void slugsCPULoad(int systemId, - uint8_t sensLoad, - uint8_t ctrlLoad, - uint8_t batVolt, - quint64 time); - - void slugsAirData(int systemId, - float dinamicPressure, - float staticPresure, - uint16_t temperature, - quint64 time); - - void slugsSensorBias(int systemId, - double axBias, - double ayBias, - double azBias, - double gxBias, - double gyBias, - double gzBias, - quint64 time); - - void slugsDiagnostic(int systemId, - double diagFl1, - double diagFl2, - double diagFl3, - int16_t diagSh1, - int16_t diagSh2, - int16_t diagSh3, - quint64 time); - - void slugsPilotConsolePWM(int systemId, - uint16_t dt, - uint16_t dla, - uint16_t dra, - uint16_t dr, - uint16_t de, - quint64 time); - - void slugsPWM(int systemId, - uint16_t dt_c, - uint16_t dla_c, - uint16_t dra_c, - uint16_t dr_c, - uint16_t dle_c, - uint16_t dre_c, - uint16_t dlf_c, - uint16_t drf_c, - uint16_t da1_c, - uint16_t da2_c, - quint64 time); - - void slugsNavegation(int systemId, - double u_m, - double phi_c, - double theta_c, - double psiDot_c, - double ay_body, - double totalDist, - double dist2Go, - uint8_t fromWP, - uint8_t toWP, - quint64 time); - - void slugsDataLog(int systemId, - double logfl_1, - double logfl_2, - double logfl_3, - double logfl_4, - double logfl_5, - double logfl_6, - quint64 time); - - - void slugsFilteredData(int systemId, - double filaX, - double filaY, - double filaZ, - double filgX, - double filgY, - double filgZ, - double filmX, - double filmY, - double filmZ, - quint64 time); - - void slugsGPSDateTime(int systemId, - uint8_t gpsyear, - uint8_t gpsmonth, - uint8_t gpsday, - uint8_t gpshour, - uint8_t gpsmin, - uint8_t gpssec, - uint8_t gpsvisSat, - quint64 time); + + void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData); + +#ifdef MAVLINK_ENABLED_SLUGS + + void slugsCPULoad(int systemId, const mavlink_cpu_load_t& cpuLoad); + void slugsAirData(int systemId, const mavlink_air_data_t& airData); + void slugsSensorBias(int systemId, const mavlink_sensor_bias_t& sensorBias); + void slugsDiagnostic(int systemId, const mavlink_diagnostic_t& diagnostic); + void slugsPilotConsolePWM(int systemId, const mavlink_pilot_console_t& pilotConsole); + void slugsPWM(int systemId, const mavlink_pwm_commands_t& pwmCommands); + void slugsNavegation(int systemId, const mavlink_slugs_navigation_t& slugsNavigation); + void slugsDataLog(int systemId, const mavlink_data_log_t& dataLog); + void slugsFilteredData(int systemId, const mavlink_filtered_data_t& filteredData); + void slugsGPSDateTime(int systemId, const mavlink_gps_date_time_t& gpsDateTime); + void slugsActionAck(int systemId, const mavlink_action_ack_t& actionAck); + + void slugsBootMsg(int uasId, mavlink_boot_t& boot); + void slugsAttitude(int uasId, mavlink_attitude_t& attitude); + +#endif + +protected: + + typedef struct _mavlink_pid_values_t { + float P[11]; + float I[11]; + float D[11]; + }mavlink_pid_values_t; + + unsigned char updateRoundRobin; + QTimer* widgetTimer; + + + mavlink_raw_imu_t mlRawImuData; + +#ifdef MAVLINK_ENABLED_SLUGS + mavlink_gps_raw_t mlGpsData; + mavlink_attitude_t mlAttitude; + mavlink_cpu_load_t mlCpuLoadData; + mavlink_air_data_t mlAirData; + mavlink_sensor_bias_t mlSensorBiasData; + mavlink_diagnostic_t mlDiagnosticData; + mavlink_pilot_console_t mlPilotConsoleData; + mavlink_filtered_data_t mlFilteredData; + mavlink_boot_t mlBoot; + mavlink_gps_date_time_t mlGpsDateTime; + mavlink_mid_lvl_cmds_t mlMidLevelCommands; + mavlink_set_mode_t mlApMode; + mavlink_pwm_commands_t mlPwmCommands; + mavlink_pid_values_t mlPidValues; + mavlink_pid_t mlSinglePid; + + mavlink_slugs_navigation_t mlNavigation; + mavlink_data_log_t mlDataLog; + mavlink_ctrl_srfc_pt_t mlPassthrough; + mavlink_action_ack_t mlActionAck; + + mavlink_slugs_action_t mlAction; // Standart messages MAVLINK used by SLUGS - void slugsActionAck(int systemId, - uint8_t action, - uint8_t result); +private: + + void emitGpsSignals (void); + int uasId; +#endif // if SLUGS }; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 347244ea1b2252fa1676514d3a9d54f8420346a5..703ae4f219fa814585d9d8bea86dce9d841f3810 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -611,11 +611,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) throttle << radioMsg.throttle[i]; QPointer radioData = new RadioCalibrationData(aileron, - elevator, - rudder, - gyro, - pitch, - throttle); emit radioCalibrationReceived(radioData); delete radioData; } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 3481f7675c07e8640e8a247351d26ad6aad8ec14..f20ed035fd6d997d8b72c943383266a07c267eee 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -227,8 +227,14 @@ void MainWindow::connectWidgets() connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); } - if (slugsHilSimWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), dynamic_cast(slugsHilSimWidget->widget()), SLOT(activeUasSet(UASInterface*)) ); + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); + } + + if (slugsDataWidget && slugsDataWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); } @@ -558,16 +564,16 @@ void MainWindow::UASCreated(UASInterface* uas) PxQuadMAV* mav = dynamic_cast(uas); if (mav) loadPixhawkView(); - SlugsMAV* mav2 = dynamic_cast(uas); - if (mav2) - { - dynamic_cast(slugsDataWidget->widget())->addUAS(uas); - loadSlugsView(); - + if (slugsDataWidget) { + SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); + if (dataWidget) { + SlugsMAV* mav2 = dynamic_cast(uas); + if (mav2) { + dataWidget->addUAS(uas); + loadSlugsView(); + } + } } - - - } } diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index 53558f533f0a2cc49e5dbd939cda23b4552afb44..fae39cc0cb9a00da4f7bb1c9c3890f7f3d9786fc 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -12,130 +12,9 @@ SlugsDataSensorView::SlugsDataSensorView(QWidget *parent) : { ui->setupUi(this); - - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); - activeUAS = NULL; - loadParameters(); - - this->setVisible(false); - - // timer for refresh UI - updateTimer = new QTimer(this); - connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); - updateTimer->start(200); -} - -void SlugsDataSensorView::loadParameters() -{ - - //Global Position - Latitude = 0; - Longitude = 0; - Height = 0; - timeGlobalPosition = 0; - - Xpos = 0; - Ypos = 0; - Zpos = 0; - TimeActualPosition = 0; - - VXpos = 0; - VYpos = 0; - VZpos = 0; - TimeActualSpeed =0; - - roll = 0; - pitch = 0; - yaw = 0; - TimeActualAttitude = 0; - - //Sensor Biases - //Acelerometer - Axb = 0; - Ayb = 0; - Azb = 0; - //Gyro - Gxb = 0; - Gyb = 0; - Gzb = 0; - TimeActualBias = 0; - - //Diagnostic Message - diagFl1 = 0; - diagFl2 = 0; - diagFl3 = 0; - diagSh1 = 0; - diagSh2 = 0; - diagSh3 = 0; - timeDiagnostic = 0; - - //CPU Load - sensLoad = 0; - ctrlLoad = 0; - batVolt = 0; - timeCpuLoad = 0; - - //navigation - u_m = 0; - phi_c = 0; - theta_c = 0; - psiDot_c = 0; - ay_body = 0; - totalDist = 0; - dist2Go = 0; - fromWP = 0; - toWP = 0; - timeNavigation = 0; - - // Data Log - Logfl_1 = 0; - Logfl_2 = 0; - Logfl_3 = 0; - Logfl_4 = 0; - Logfl_5 = 0; - Logfl_6 = 0; - timeDataLog = 0; - - //pwm commands - dt_c = 0; ///< AutoPilot's throttle command - dla_c = 0; ///< AutoPilot's left aileron command - dra_c = 0; ///< AutoPilot's right aileron command - dr_c = 0; ///< AutoPilot's rudder command - dle_c = 0; ///< AutoPilot's left elevator command - dre_c = 0; ///< AutoPilot's right elevator command - dlf_c = 0; ///< AutoPilot's left flap command - drf_c = 0; ///< AutoPilot's right flap command - aux1 = 0; ///< AutoPilot's aux1 command - aux2 = 0; ///< AutoPilot's aux2 command - timePWMCommand = 0; - - - //filtered data - aX = 0; ///< Accelerometer X value (m/s^2) - aY = 0; ///< Accelerometer Y value (m/s^2) - aZ = 0; ///< Accelerometer Z value (m/s^2) - gX = 0; ///< Gyro X value (rad/s) - gY = 0; ///< Gyro Y value (rad/s) - gZ = 0; ///< Gyro Z value (rad/s) - mX = 0; ///< Magnetometer X (normalized to 1) - mY = 0; ///< Magnetometer Y (normalized to 1) - mZ = 0; ///< Magnetometer Z (normalized to 1) - timeFiltered = 0; - - //gps date and time - year = 0 ; ///< Year reported by Gps - month =0; ///< Month reported by Gps - day = 0; ///< Day reported by Gps - hour = 0; ///< Hour reported by Gps - min = 0; ///< Min reported by Gps - sec = 0; ///< Sec reported by Gps - visSat = 0; ///< Visible sattelites reported by Gps - timeGPSDateTime = 0; - - } SlugsDataSensorView::~SlugsDataSensorView() @@ -147,141 +26,64 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) { SlugsMAV* slugsMav = dynamic_cast(uas); - if (slugsMav != NULL) - { - //connect standar messages - connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChange(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); - connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + if (slugsMav != NULL) { + connect(slugsMav, SIGNAL(slugsRawImu(int, const mavlink_raw_imu_t&)), this, SLOT(slugRawDataChanged(int, const mavlink_raw_imu_t&))); + + #ifdef MAVLINK_ENABLED_SLUGS + + //connect standar messages + connect(slugsMav, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugLocalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); - //connect slugs especial messages - connect(slugsMav, SIGNAL(slugsSensorBias(int,double,double,double,double,double,double,quint64)), this, SLOT(slugsSensorBiasChanged(int,double,double,double,double,double,double,quint64))); - connect(slugsMav, SIGNAL(slugsDiagnostic(int,double,double,double,int16_t,int16_t,int16_t,quint64)), this, SLOT(slugsDiagnosticMessageChanged(int,double,double,double,int16_t,int16_t,int16_t,quint64))); - connect(slugsMav, SIGNAL(slugsCPULoad(int,uint8_t,uint8_t,uint8_t,quint64)), this, SLOT(slugsCpuLoadChanged(int,uint8_t,uint8_t,uint8_t,quint64))); - connect(slugsMav, SIGNAL(slugsNavegation(int,double,double,double,double,double,double,double,uint8_t,uint8_t,quint64)),this,SLOT(slugsNavegationChanged(int,double,double,double,double,double,double,double,uint8_t,uint8_t,quint64))); - connect(slugsMav, SIGNAL(slugsDataLog(int,double,double,double,double,double,double,quint64)), this, SLOT(slugsDataLogChanged(int,double,double,double,double,double,double,quint64))); - connect(slugsMav, SIGNAL(slugsPWM(int,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,quint64)),this,SLOT(slugsPWMChanged(int,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,uint16_t,quint64))); - connect(slugsMav, SIGNAL(slugsFilteredData(int,double,double,double,double,double,double,double,double,double,quint64)),this,SLOT(slugsFilteredDataChanged(int,double,double,double,double,double,double,double,double,double,quint64))); - connect(slugsMav, SIGNAL(slugsGPSDateTime(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64)),this,SLOT(slugsGPSDateTimeChanged(int,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,uint8_t,quint64))); + //connect slugs especial messages + connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&))); + connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&))); + connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&))); + connect(slugsMav, SIGNAL(slugsNavegation(int,const mavlink_slugs_navigation_t&)),this,SLOT(slugsNavegationChanged(int,const mavlink_slugs_navigation_t&))); + connect(slugsMav, SIGNAL(slugsDataLog(int,const mavlink_data_log_t&)), this, SLOT(slugsDataLogChanged(int,const mavlink_data_log_t&))); + connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); + connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); + connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); + + #endif // MAVLINK_ENABLED_SLUGS // Set this UAS as active if it is the first one - if(activeUAS == 0) - { + if(activeUAS == 0) { activeUAS = uas; } + } } -void SlugsDataSensorView::setActiveUAS(UASInterface* uas) -{ - activeUAS = uas; - +void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t &rawData){ + Q_UNUSED(uasId); + ui->m_Axr->setText(QString::number(rawData.xacc)); + ui->m_Ayr->setText(QString::number(rawData.yacc)); + ui->m_Azr->setText(QString::number(rawData.zacc)); } - -void SlugsDataSensorView::refresh() -{ - if(activeUAS) - { - //refresh Global Position - ui->m_GpsLatitude->setText(QString::number(Latitude, 'f', 4)); - ui->m_GpsLongitude->setText(QString::number(Longitude, 'f', 4)); - ui->m_Gpsheigth->setText(QString::number(Height, 'f', 4)); - - //refresh GPS Date and Time - ui->m_GpsDate->setText(QString::number(day)+ "-" + QString::number(month)+ "-" + QString::number(year)); - ui->m_GpsTime->setText(QString::number(hour)+ "-" + QString::number(min)+ "-" + QString::number(sec)); - ui->m_GpsSat->setText(QString::number(visSat)); - ui->m_GpsCog->setText("No Data"); - ui->m_GpsSog->setText("No Data"); - - - //refresh UI position data - ui->ed_x->setPlainText(QString::number(Xpos, 'f', 4)); - ui->ed_y->setPlainText(QString::number(Ypos, 'f', 4)); - ui->ed_z->setPlainText(QString::number(Zpos, 'f', 4)); - - //refresh UI speed position data - ui->ed_vx->setPlainText(QString::number(VXpos,'f',4)); - ui->ed_vy->setPlainText(QString::number(VYpos,'f',4)); - ui->ed_vz->setPlainText(QString::number(VZpos,'f',4)); - - //refresh UI attitude data - ui->m_SlugAttitudeRoll_plainTextEdit->setPlainText(QString::number(roll,'f',4)); - ui->m_SlugAttitudePitch_plainTextEdit->setPlainText(QString::number(pitch,'f',4)); - ui->m_SlugAttitudeYaw_plainTextEdit->setPlainText(QString::number(yaw,'f',4)); - - //refresh UI sensor bias acelerometer data - ui->m_AxBiases->setText(QString::number(Axb, 'f', 4)); - ui->m_AyBiases->setText(QString::number(Ayb, 'f', 4)); - ui->m_AzBiases->setText(QString::number(Azb, 'f', 4)); - ui->m_GxBiases->setText(QString::number(Gxb, 'f', 4)); - ui->m_GyBiases->setText(QString::number(Gyb, 'f', 4)); - ui->m_GzBiases->setText(QString::number(Gzb, 'f', 4)); - - //refresh UI diagnostic data - ui->m_SlugsFl1_textEdit->setText(QString::number(diagFl1, 'f', 4)); - ui->m_SlugsFl2_textEdit->setText(QString::number(diagFl2, 'f', 4)); - ui->m_SlugsFl3_textEdit->setText(QString::number(diagFl3, 'f', 4)); - ui->m_SlugsSh1_textEdit->setText(QString::number(diagSh1, 'f', 4)); - ui->m_SlugsSh2_textEdit->setText(QString::number(diagSh2, 'f', 4)); - ui->m_SlugsSh3_textEdit->setText(QString::number(diagSh3, 'f', 4)); - - //refresh UI navegation data - ui->m_SlugsUm_textEdit->setText(QString::number(u_m, 'f', 4)); - ui->m_SlugsPitchC_textEdit->setText(QString::number(pitch, 'f', 4)); - ui->m_SlugsPsidC_textEdit->setText(QString::number(psiDot_c, 'f', 4)); - ui->m_SlugsPhiC_textEdit->setText(QString::number(phi_c, 'f', 4)); - ui->m_SlugsAyBody_textEdit->setText(QString::number(ay_body, 'f', 4)); - ui->m_SlugsFromWP_textEdit->setText(QString::number(fromWP, 'f', 4)); - ui->m_SlugsToWP_textEdit->setText(QString::number(toWP, 'f', 4)); - ui->m_SlugsTotRun_textEdit->setText(QString::number(totalDist, 'f', 4)); - ui->m_SlugsDistToGround_textEdit->setText(QString::number(dist2Go, 'f', 4)); - - //refresh UI Data Log - ui->m_logFl1_textEdit->setText(QString::number(Logfl_1, 'f', 4)); - ui->m_logFl2_textEdit->setText(QString::number(Logfl_2, 'f', 4)); - ui->m_logFl3_textEdit->setText(QString::number(Logfl_3, 'f', 4)); - ui->m_logFl4_textEdit->setText(QString::number(Logfl_4, 'f', 4)); - ui->m_logFl5_textEdit->setText(QString::number(Logfl_5, 'f', 4)); - ui->m_logFl6_textEdit->setText(QString::number(Logfl_6, 'f', 4)); - - //refresh UI PWM Commands - ui->m_pwmThro->setText(QString::number(dt_c, 'f', 4)); - ui->m_pwmThroTrim->setText(QString::number(dre_c, 'f', 4)); - ui->m_pwmAile->setText(QString::number(dla_c, 'f', 4)); - ui->m_pwmAileTrim->setText(QString::number(dlf_c, 'f', 4)); - ui->m_pwmElev->setText(QString::number(dle_c, 'f', 4)); - ui->m_pwmElevTrim->setText(QString::number(drf_c, 'f', 4)); - ui->m_pwmRudd->setText(QString::number(dr_c, 'f', 4)); - ui->m_pwmRuddTrim->setText(QString::number(aux1, 'f', 4)); - ui->m_pwmFailSafe->setText(QString::number(dre_c, 'f', 4)); - ui->m_pwmAvailable->setText("No Data"); - - - } - +void SlugsDataSensorView::setActiveUAS(UASInterface* uas){ + activeUAS = uas; } +#ifdef MAVLINK_ENABLED_SLUGS + void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas, double lat, double lon, double alt, - quint64 time) - -{ - Q_UNUSED( uas); - Latitude = lat; - Longitude = lon; - Height = alt; - timeGlobalPosition = time; - - + quint64 time) { + Q_UNUSED(uas); + Q_UNUSED(time); + ui->m_GpsLatitude->setText(QString::number(lat)); + ui->m_GpsLongitude->setText(QString::number(lon)); + ui->m_GpsHeight->setText(QString::number(alt)); } @@ -289,14 +91,13 @@ void SlugsDataSensorView::slugLocalPositionChanged(UASInterface* uas, double x, double y, double z, - quint64 time) -{ - Q_UNUSED( uas); + quint64 time) { + Q_UNUSED(uas); + Q_UNUSED(time); - Xpos = x; - Ypos = y; - Zpos = z; - TimeActualPosition = time; + ui->ed_x->setPlainText(QString::number(x)); + ui->ed_y->setPlainText(QString::number(y)); + ui->ed_z->setPlainText(QString::number(z)); } @@ -304,14 +105,14 @@ void SlugsDataSensorView::slugSpeedLocalPositionChanged(UASInterface* uas, double vx, double vy, double vz, - quint64 time) -{ + quint64 time) { Q_UNUSED( uas); + Q_UNUSED(time); + + ui->ed_vx->setPlainText(QString::number(vx)); + ui->ed_vy->setPlainText(QString::number(vy)); + ui->ed_vz->setPlainText(QString::number(vz)); - VXpos = vx; - VYpos = vy; - VZpos = vz; - TimeActualSpeed = time; } void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas, @@ -321,190 +122,118 @@ void SlugsDataSensorView::slugAttitudeChanged(UASInterface* uas, quint64 time) { Q_UNUSED( uas); + Q_UNUSED(time); + + ui->m_Roll->setPlainText(QString::number(slugroll)); + ui->m_Pitch->setPlainText(QString::number(slugpitch)); + ui->m_Yaw->setPlainText(QString::number(slugyaw)); - roll = slugroll; - pitch = slugpitch; - yaw = slugyaw; - TimeActualAttitude = time; } -void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, - double axb, - double ayb, - double azb, - double gxb, - double gyb, - double gzb, - quint64 time) -{ +void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, + const mavlink_sensor_bias_t& sensorBias){ Q_UNUSED( systemId); - Axb = axb; - Ayb = ayb; - Azb = azb; - Gxb = gxb; - Gyb = gyb; - Gzb = gzb; - TimeActualBias = time; - + ui->m_AxBiases->setText(QString::number(sensorBias.axBias)); + ui->m_AyBiases->setText(QString::number(sensorBias.ayBias)); + ui->m_AzBiases->setText(QString::number(sensorBias.azBias)); + ui->m_GxBiases->setText(QString::number(sensorBias.gxBias)); + ui->m_GyBiases->setText(QString::number(sensorBias.gyBias)); + ui->m_GzBiases->setText(QString::number(sensorBias.gzBias)); } void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId, - double diagfl1, - double diagfl2, - double diagfl3, - int16_t diagsh1, - int16_t diagsh2, - int16_t diagsh3, - quint64 time) -{ + const mavlink_diagnostic_t& diagnostic){ Q_UNUSED(systemId); - diagFl1 = diagfl1; - diagFl2 = diagfl2; - diagFl3 = diagfl3; - diagSh1 = diagsh1; - diagSh2 = diagsh2; - diagSh3 = diagsh3; - timeDiagnostic = time; + ui->m_Fl1->setText(QString::number(diagnostic.diagFl1)); + ui->m_Fl2->setText(QString::number(diagnostic.diagFl2)); + ui->m_Fl3->setText(QString::number(diagnostic.diagFl2)); + ui->m_Sh1->setText(QString::number(diagnostic.diagSh1)); + ui->m_Sh2->setText(QString::number(diagnostic.diagSh2)); + ui->m_Sh3->setText(QString::number(diagnostic.diagSh3)); } void SlugsDataSensorView::slugsCpuLoadChanged(int systemId, - uint8_t sensload, - uint8_t ctrlload, - uint8_t batvolt, - quint64 time) -{ + const mavlink_cpu_load_t& cpuLoad){ Q_UNUSED(systemId); - sensLoad = sensload; - ctrlLoad = ctrlload; - batVolt = batvolt; - timeCpuLoad = time; + ui->ed_sens->setText(QString::number(cpuLoad.sensLoad)); + ui->ed_control->setText(QString::number(cpuLoad.ctrlLoad)); + ui->ed_batvolt->setText(QString::number(cpuLoad.batVolt)); } void SlugsDataSensorView::slugsNavegationChanged(int systemId, - double navu_m, - double navphi_c, - double navtheta_c, - double navpsiDot_c, - double navay_body, - double navtotalDist, - double navdist2Go, - uint8_t navfromWP, - uint8_t navtoWP, - quint64 time) -{ + const mavlink_slugs_navigation_t& slugsNavigation){ Q_UNUSED(systemId); - u_m = navu_m; - phi_c = navphi_c; - theta_c = navtheta_c; - psiDot_c = navpsiDot_c; - ay_body = navay_body; - totalDist = navtotalDist; - dist2Go = navdist2Go; - fromWP = navfromWP; - toWP = navtoWP; - timeNavigation = time; + ui->m_Um->setText(QString::number(slugsNavigation.u_m)); + ui->m_PhiC->setText(QString::number(slugsNavigation.phi_c)); + ui->m_PitchC->setText(QString::number(slugsNavigation.theta_c)); + ui->m_PsidC->setText(QString::number(slugsNavigation.psiDot_c)); + ui->m_AyBody->setText(QString::number(slugsNavigation.ay_body)); + ui->m_TotRun->setText(QString::number(slugsNavigation.totalDist)); + ui->m_DistToGo->setText(QString::number(slugsNavigation.dist2Go)); + ui->m_FromWP->setText(QString::number(slugsNavigation.fromWP)); + ui->m_ToWP->setText(QString::number(slugsNavigation.toWP)); } void SlugsDataSensorView::slugsDataLogChanged(int systemId, - double logfl_1, - double logfl_2, - double logfl_3, - double logfl_4, - double logfl_5, - double logfl_6, - quint64 time) -{ + const mavlink_data_log_t& dataLog){ Q_UNUSED(systemId); - Logfl_1 = logfl_1; - Logfl_2 = logfl_2; - Logfl_3 = logfl_3; - Logfl_4 = logfl_4; - Logfl_5 = logfl_5; - Logfl_6 = logfl_6; - timeDataLog = time; + ui->m_logFl1->setText(QString::number(dataLog.fl_1)); + ui->m_logFl2->setText(QString::number(dataLog.fl_2)); + ui->m_logFl3->setText(QString::number(dataLog.fl_3)); + ui->m_logFl4->setText(QString::number(dataLog.fl_4)); + ui->m_logFl5->setText(QString::number(dataLog.fl_5)); + ui->m_logFl6->setText(QString::number(dataLog.fl_6)); } void SlugsDataSensorView::slugsPWMChanged(int systemId, - uint16_t vdt_c, - uint16_t vdla_c, - uint16_t vdra_c, - uint16_t vdr_c, - uint16_t vdle_c, - uint16_t vdre_c, - uint16_t vdlf_c, - uint16_t vdrf_c, - uint16_t vda1_c, - uint16_t vda2_c, - quint64 time) -{ + const mavlink_pwm_commands_t& pwmCommands){ Q_UNUSED(systemId); - dt_c = vdt_c; ///< AutoPilot's throttle command - dla_c = vdla_c; ///< AutoPilot's left aileron command - dra_c = vdra_c; ///< AutoPilot's right aileron command - dr_c = vdr_c; ///< AutoPilot's rudder command - dle_c = vdle_c; ///< AutoPilot's left elevator command - dre_c = vdre_c; ///< AutoPilot's right elevator command - dlf_c = vdlf_c; ///< AutoPilot's left flap command - drf_c = vdrf_c; ///< AutoPilot's right flap command - aux1 = vda1_c; ///< AutoPilot's aux1 command - aux2 = vda2_c; ///< AutoPilot's aux2 command - timePWMCommand = time; + ui->m_pwmThro->setText(QString::number(pwmCommands.dt_c)); + ui->m_pwmAile->setText(QString::number(pwmCommands.dla_c)); + ui->m_pwmElev->setText(QString::number(pwmCommands.dle_c)); + ui->m_pwmRudd->setText(QString::number(pwmCommands.dr_c)); + + ui->m_pwmThroTrim->setText(QString::number(pwmCommands.dre_c)); + ui->m_pwmAileTrim->setText(QString::number(pwmCommands.dlf_c)); + ui->m_pwmElevTrim->setText(QString::number(pwmCommands.drf_c)); + ui->m_pwmRuddTrim->setText(QString::number(pwmCommands.aux1)); } void SlugsDataSensorView::slugsFilteredDataChanged(int systemId, - double filaX, - double filaY, - double filaZ, - double filgX, - double filgY, - double filgZ, - double filmX, - double filmY, - double filmZ, - quint64 time) -{ + const mavlink_filtered_data_t& filteredData){ Q_UNUSED(systemId); - - aX = filaX; ///< Accelerometer X value (m/s^2) - aY = filaY; ///< Accelerometer Y value (m/s^2) - aZ = filaZ; ///< Accelerometer Z value (m/s^2) - gX = filgX; ///< Gyro X value (rad/s) - gY = filgY; ///< Gyro Y value (rad/s) - gZ = filgZ; ///< Gyro Z value (rad/s) - mX = filmX; ///< Magnetometer X (normalized to 1) - mY = filmY; ///< Magnetometer Y (normalized to 1) - mZ = filmZ; ///< Magnetometer Z (normalized to 1) - timeFiltered = time; + ui->m_Axf->setText(QString::number(filteredData.aX)); + ui->m_Ayf->setText(QString::number(filteredData.aY)); + ui->m_Azf->setText(QString::number(filteredData.aZ)); + ui->m_Gxf->setText(QString::number(filteredData.gX)); + ui->m_Gyf->setText(QString::number(filteredData.gY)); + ui->m_Gzf->setText(QString::number(filteredData.gZ)); + ui->m_Mxf->setText(QString::number(filteredData.mX)); + ui->m_Myf->setText(QString::number(filteredData.mY)); + ui->m_Mzf->setText(QString::number(filteredData.mZ)); } void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, - uint8_t gpsyear, - uint8_t gpsmonth, - uint8_t gpsday, - uint8_t gpshour, - uint8_t gpsmin, - uint8_t gpssec, - uint8_t gpsvisSat, - quint64 time) -{ + const mavlink_gps_date_time_t& gpsDateTime){ Q_UNUSED(systemId); + ui->m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" + + QString::number(gpsDateTime.day) + "/" + + QString::number(gpsDateTime.year)); - year = gpsyear ; ///< Year reported by Gps - month = gpsmonth; ///< Month reported by Gps - day = gpsday; ///< Day reported by Gps - hour = gpshour; ///< Hour reported by Gps - min = gpsmin; ///< Min reported by Gps - sec = gpssec; ///< Sec reported by Gps - visSat = gpsvisSat; ///< Visible sattelites reported by Gps - timeGPSDateTime = time; + ui->m_GpsTime->setText(QString::number(gpsDateTime.hour) + ":" + + QString::number(gpsDateTime.min) + ":" + + QString::number(gpsDateTime.sec)); + ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); } + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h index 6f23ff528c130d1c254275a63783b32d624e8600..6cd40b01bbd70d9dca789d52c40a78c5b8cd5aad 100644 --- a/src/ui/SlugsDataSensorView.h +++ b/src/ui/SlugsDataSensorView.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include "UASInterface.h" #include "SlugsMAV.h" +#include "mavlink.h" namespace Ui { class SlugsDataSensorView; @@ -67,17 +68,10 @@ public slots: */ void setActiveUAS(UASInterface* uas); - /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets - */ - - void refresh(); - - + void slugRawDataChanged (int uasId, const mavlink_raw_imu_t& rawData); +#ifdef MAVLINK_ENABLED_SLUGS /** * @brief Adds the UAS for data display * @@ -120,240 +114,62 @@ public slots: double alt, quint64 time); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the sensor bias widget */ void slugsSensorBiasChanged(int systemId, - double axb, - double ayb, - double azb, - double gxb, - double gyb, - double gzb, - quint64 time); + const mavlink_sensor_bias_t& sensorBias); + /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the diagnostic widget */ void slugsDiagnosticMessageChanged(int systemId, - double diagfl1, - double diagfl2, - double diagfl3, - int16_t diagsh1, - int16_t diagsh2, - int16_t diagsh3, - quint64 time); + const mavlink_diagnostic_t& diagnostic); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the CPU load widget */ void slugsCpuLoadChanged(int systemId, - uint8_t sensload, - uint8_t ctrlload, - uint8_t batvolt, - quint64 time); + const mavlink_cpu_load_t& cpuLoad); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the Navigation widget */ void slugsNavegationChanged(int systemId, - double navu_m, - double navphi_c, - double navtheta_c, - double navpsiDot_c, - double navay_body, - double navtotalDist, - double navdist2Go, - uint8_t navfromWP, - uint8_t navtoWP, - quint64 time); + const mavlink_slugs_navigation_t& slugsNavigation); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the Data Log widget */ void slugsDataLogChanged(int systemId, - double logfl_1, - double logfl_2, - double logfl_3, - double logfl_4, - double logfl_5, - double logfl_6, - quint64 time); + const mavlink_data_log_t& dataLog); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the PWM Commands widget */ - void slugsPWMChanged(int systemId, - uint16_t vdt_c, - uint16_t vdla_c, - uint16_t vdra_c, - uint16_t vdr_c, - uint16_t vdle_c, - uint16_t vdre_c, - uint16_t vdlf_c, - uint16_t vdrf_c, - uint16_t vda1_c, - uint16_t vda2_c, - quint64 time); + const mavlink_pwm_commands_t& pwmCommands); /** - * @brief Adds the UAS for data display - * - * Adds the UAS and makes all the correct connections for data display on the Widgets + * @brief Updates the filtered sensor measurements widget */ void slugsFilteredDataChanged(int systemId, - double filaX, - double filaY, - double filaZ, - double filgX, - double filgY, - double filgZ, - double filmX, - double filmY, - double filmZ, - quint64 time); - - - + const mavlink_filtered_data_t& filteredData); + /** + * @brief Updates the gps Date Time widget + */ void slugsGPSDateTimeChanged(int systemId, - uint8_t gpsyear, - uint8_t gpsmonth, - uint8_t gpsday, - uint8_t gpshour, - uint8_t gpsmin, - uint8_t gpssec, - uint8_t gpsvisSat, - quint64 time); + const mavlink_gps_date_time_t& gpsDateTime); +#endif // MAVLINK_ENABLED_SLUGS protected: - QTimer* updateTimer; UASInterface* activeUAS; - //Global Position - double Latitude; - double Longitude; - double Height; - quint64 timeGlobalPosition; - - // Position and Attitude - //Position - double Xpos; - double Ypos; - double Zpos; - quint64 TimeActualPosition; - //Speed - double VXpos; - double VYpos; - double VZpos; - quint64 TimeActualSpeed; - //Attitude - double roll; - double pitch; - double yaw; - quint64 TimeActualAttitude; - - //Sensor Biases - //Acelerometer - double Axb; - double Ayb; - double Azb; - - //Gyro - double Gxb; - double Gyb; - double Gzb; - quint64 TimeActualBias; - - //Diagnostic - double diagFl1; - double diagFl2; - double diagFl3; - int16_t diagSh1; - int16_t diagSh2; - int16_t diagSh3; - quint64 timeDiagnostic; - - //CPU Load - uint8_t sensLoad; - uint8_t ctrlLoad; - uint8_t batVolt; - quint64 timeCpuLoad; - - //navigation data - double u_m; - double phi_c; - double theta_c; - double psiDot_c; - double ay_body; - double totalDist; - double dist2Go; - uint8_t fromWP; - uint8_t toWP; - quint64 timeNavigation; - - // Data Log - double Logfl_1; - double Logfl_2; - double Logfl_3; - double Logfl_4; - double Logfl_5; - double Logfl_6; - quint64 timeDataLog; - - //pwm commands - uint16_t dt_c; ///< AutoPilot's throttle command - uint16_t dla_c; ///< AutoPilot's left aileron command - uint16_t dra_c; ///< AutoPilot's right aileron command - uint16_t dr_c; ///< AutoPilot's rudder command - uint16_t dle_c; ///< AutoPilot's left elevator command - uint16_t dre_c; ///< AutoPilot's right elevator command - uint16_t dlf_c; ///< AutoPilot's left flap command - uint16_t drf_c; ///< AutoPilot's right flap command - uint16_t aux1; ///< AutoPilot's aux1 command - uint16_t aux2; ///< AutoPilot's aux2 command - quint64 timePWMCommand; - - //filtered data - double aX; ///< Accelerometer X value (m/s^2) - double aY; ///< Accelerometer Y value (m/s^2) - double aZ; ///< Accelerometer Z value (m/s^2) - double gX; ///< Gyro X value (rad/s) - double gY; ///< Gyro Y value (rad/s) - double gZ; ///< Gyro Z value (rad/s) - double mX; ///< Magnetometer X (normalized to 1) - double mY; ///< Magnetometer Y (normalized to 1) - double mZ; ///< Magnetometer Z (normalized to 1) - quint64 timeFiltered; - - //gps date and time - uint8_t year; ///< Year reported by Gps - uint8_t month; ///< Month reported by Gps - uint8_t day; ///< Day reported by Gps - uint8_t hour; ///< Hour reported by Gps - uint8_t min; ///< Min reported by Gps - uint8_t sec; ///< Sec reported by Gps - uint8_t visSat; ///< Visible sattelites reported by Gps - quint64 timeGPSDateTime; - - private: Ui::SlugsDataSensorView *ui; - void loadParameters(); }; diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui index 8c61108fb347415f7583116a3cbba48a98ce9fbc..917e4571738f72967e0f48d57d1ea724e850e682 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -6,7 +6,7 @@ 0 0 - 394 + 399 667 @@ -31,11 +31,11 @@ Form - + - 3 + 2 @@ -496,7 +496,7 @@ - + 0 @@ -556,7 +556,7 @@ - + 0 @@ -616,7 +616,7 @@ - + 0 @@ -708,7 +708,7 @@ - + 0 @@ -768,7 +768,7 @@ - + 0 @@ -828,7 +828,7 @@ - + 0 @@ -888,7 +888,7 @@ - + 0 @@ -948,7 +948,7 @@ - + 0 @@ -1021,7 +1021,7 @@ - + 0 @@ -1081,7 +1081,7 @@ - + 0 @@ -1141,7 +1141,7 @@ - + 0 @@ -1201,7 +1201,7 @@ - + 0 @@ -1269,7 +1269,7 @@ - + 0 @@ -1311,7 +1311,7 @@ - + 0 @@ -1353,7 +1353,7 @@ - + 0 @@ -1395,7 +1395,7 @@ - + 0 @@ -1437,7 +1437,7 @@ - + 0 @@ -1479,7 +1479,7 @@ - + 0 @@ -1541,7 +1541,7 @@ - + 0 @@ -1583,7 +1583,7 @@ - + 0 @@ -1625,7 +1625,7 @@ - + 0 @@ -1667,7 +1667,7 @@ - + 0 @@ -1709,7 +1709,7 @@ - + 0 @@ -1751,7 +1751,7 @@ - + 0 @@ -2068,7 +2068,7 @@ - + 60 @@ -2715,58 +2715,6 @@ - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 10 - 75 - true - - - - FailSafe - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 10 - 75 - true - - - - Available - - - @@ -2798,6 +2746,12 @@ + + 60 + 18 + + + 80 18 @@ -2814,6 +2768,12 @@ + + 60 + 18 + + + 80 18 @@ -2831,21 +2791,11 @@ - 80 + 60 18 - - - - - - - 60 - 18 - - - + 80 18 @@ -2862,6 +2812,12 @@ + + 60 + 18 + + + 80 18 @@ -2878,6 +2834,12 @@ + + 60 + 18 + + + 80 18 @@ -2894,6 +2856,12 @@ + + 60 + 18 + + + 80 18 @@ -2911,29 +2879,16 @@ - 80 + 60 18 - - - - - - - 60 - 18 - - - + 80 18 - - - @@ -2949,6 +2904,1584 @@ Tab 3 + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + CPU Load + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Sensor + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Control + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Batt Volt + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + 0 + 0 + + + + + 132123 + 123123 + + + + Air Data + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Dynamic + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Static + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Temperature + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + + + + 311 + 213 + + + + + 371 + 213 + + + + Filtered Data + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + 0 + 0 + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + 311 + 213 + + + + + 361 + 213 + + + + Raw Data + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ax + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Ay + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Az + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gy + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Gz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mx + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + My + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 10 + 75 + true + + + + Mz + + + + + + + + 60 + 18 + + + + + 80 + 18 + + + + + 10 + + + + QFrame::WinPanel + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index 6fe74d575e2cdb916e039aa47990dc18b2e83960..001a9e734d4b9d61c6ddade2c2729e15491b16b6 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -44,11 +44,13 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas) { SlugsMAV* slugsMav = dynamic_cast(uas); +#ifdef MAVLINK_ENABLED_SLUGS if (slugsMav != NULL) { - connect(slugsMav,SIGNAL(slugsActionAck(int,uint8_t,uint8_t)),this,SLOT(recibeMensaje(int,uint8_t,uint8_t))); + connect(slugsMav,SIGNAL(slugsActionAck(int,const mavlink_action_ack_t&)),this,SLOT(recibeMensaje(int,mavlink_action_ack_t))); } +#endif // MAVLINK_ENABLED_SLUG // Set this UAS as active if it is the first one if(activeUAS == 0) { @@ -154,6 +156,12 @@ void SlugsPIDControl::changeColor_RED_AirSpeed_groupBox(QString text) void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() { + + SlugsMAV* slugsMav = dynamic_cast(activeUAS); + + if (slugsMav != NULL) + { + //create the packet pidMessage.target = activeUAS->getUASID(); pidMessage.idx = 0; @@ -161,16 +169,13 @@ void SlugsPIDControl::changeColor_GREEN_AirSpeed_groupBox() pidMessage.iVal = ui->dT_I_set->text().toFloat(); pidMessage.dVal = ui->dT_D_set->text().toFloat(); - UAS *uas = dynamic_cast(UASManager::instance()->getActiveUAS()); - mavlink_message_t msg; mavlink_msg_pid_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &pidMessage); - uas->sendMessage(msg); - - + slugsMav->sendMessage(msg); ui->AirSpeedHold_groupBox->setStyleSheet(GREENcolorStyle); + } } @@ -335,7 +340,11 @@ void SlugsPIDControl::connect_Pitch2dT_LineEdit() connect(ui->P2dT_FF_set,SIGNAL(textChanged(QString)),this,SLOT(changeColor_RED_Pitch2dT_groupBox(QString))); } -void SlugsPIDControl::recibeMensaje(int systemId, uint8_t action, uint8_t result) +#ifdef MAVLINK_ENABLED_SLUGS + +void SlugsPIDControl::recibeMensaje(int systemId, const mavlink_action_ack_t& action) { - ui->recepcion_label->setText("Mensaje Recibido: " + QString::number(action)); + ui->recepcion_label->setText(QString::number(action.action)); } + +#endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h index 700a677604a9ead3fcab964ce53164236270766d..600ffc6f058783b3312bf6139b3f3600fbcfb0b9 100644 --- a/src/ui/SlugsPIDControl.h +++ b/src/ui/SlugsPIDControl.h @@ -6,7 +6,7 @@ #include "UASInterface.h" #include "QGCMAVLink.h" #include "SlugsMAV.h" - +#include "mavlink.h" namespace Ui { class SlugsPIDControl; @@ -184,9 +184,11 @@ public slots: //Create, send and get Messages PID // void createMessagePID(); +#ifdef MAVLINK_ENABLED_SLUGS - void recibeMensaje(int systemId, uint8_t action, uint8_t result); + void recibeMensaje(int systemId, const mavlink_action_ack_t& action); +#endif // MAVLINK_ENABLED_SLUG private: Ui::SlugsPIDControl *ui; diff --git a/src/ui/slugshilsim.cc b/src/ui/slugshilsim.cc index 97f232479ed7b1916d9a7461173687f8ed4ebfe1..5cf3175e62dd21dd2cb3814a83790c50abb37640 100644 --- a/src/ui/slugshilsim.cc +++ b/src/ui/slugshilsim.cc @@ -41,7 +41,10 @@ SlugsHilSim::SlugsHilSim(QWidget *parent) : rxSocket = new QUdpSocket(this); txSocket = new QUdpSocket(this); + hilLink = NULL; + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*))); + connect(ui->cb_mavlinkLinks, SIGNAL(currentIndexChanged(int)), this, SLOT(linkSelected(int))); connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode())); connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram())); @@ -54,23 +57,15 @@ SlugsHilSim::~SlugsHilSim() delete ui; } -void SlugsHilSim::linkAdded(void){ - -// ui->cb_mavlinkLinks->clear(); - -// QList linkList; -// linkList.append(LinkManager::instance()->getLinks()) ; - -// for (int i = 0; i< linkList.size(); i++){ -// ui->cb_mavlinkLinks->addItem((linkList.takeFirst())->getName()); -// } - -} - void SlugsHilSim::addToCombo(LinkInterface* theLink){ ui->cb_mavlinkLinks->addItem(theLink->getName()); linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink); + + if (hilLink == NULL){ + hilLink = theLink; + } + } void SlugsHilSim::putInHilMode(void){ @@ -86,8 +81,7 @@ void SlugsHilSim::putInHilMode(void){ msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); msgBox.setDefaultButton(QMessageBox::No); - if(msgBox.exec() == QMessageBox::Yes) - { + if(msgBox.exec() == QMessageBox::Yes) { rxSocket->disconnectFromHost(); rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt()); //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt()); @@ -99,7 +93,6 @@ void SlugsHilSim::putInHilMode(void){ ui->bt_startHil->setText(buttonCaption); - } else { ui->bt_startHil->setChecked(false); } @@ -170,9 +163,13 @@ void SlugsHilSim::processHilDatagram(const QByteArray* datagram){ tmpGpsRaw.fix_type = datagram->at(i++); tmpGpsTime.visSat = datagram->at(i++); - //mavlink_msg_gps_date_time_pack(); + mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsTime); + activeUas->sendMessage(hilLink, msg); + + memset(&msg, 0, sizeof(mavlink_message_t)); - //activeUas->sendMessage(); + mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,MG::SYSTEM::COMPID, &msg, &tmpGpsRaw); + activeUas->sendMessage(hilLink,msg); // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model i++; @@ -190,11 +187,6 @@ float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned ch tmpF2C.chData[2] = datagram->at((*i)++); tmpF2C.chData[3] = datagram->at((*i)++); - -// if (uas != NULL) { -// //activeUas = uas; -// } - return tmpF2C.flData; } @@ -207,3 +199,7 @@ uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigne return tmpU2C.uiData; } + +void SlugsHilSim::linkSelected(int cbIndex){ + //hilLink = linksAvailable +} diff --git a/src/ui/slugshilsim.h b/src/ui/slugshilsim.h index 7434aec4b14e82ca4a07845161a53afa939c7086..dc0d8d1d7a90909656c29145de053c7231470cb3 100644 --- a/src/ui/slugshilsim.h +++ b/src/ui/slugshilsim.h @@ -60,7 +60,6 @@ protected: UAS* activeUas; public slots: - void linkAdded (void); /** * @brief Adds a link to the combo box listing so the user can select a link @@ -98,6 +97,12 @@ public slots: */ void activeUasSet(UASInterface* uas); + /** + * @brief Called when the Link combobox selects a new link. + * + * @param uas The new index of the selected link + */ + void linkSelected (int cbIndex); public slots: