From 34069414680cff023fc86f69be925b1092b8e76c Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 10 Jul 2010 12:57:59 -0500 Subject: [PATCH 1/3] add slugs messages --- qgroundcontrol.pri | 11 ++++--- qgroundcontrol.pro | 11 +++++-- src/main.cc | 7 ++-- src/uas/SlugsMAV.cc | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++- src/uas/SlugsMAV.h | 1 + src/uas/UAS.cc | 1 + 6 files changed, 112 insertions(+), 11 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 1189c7a..afd0a17 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -120,12 +120,15 @@ win32 { # Special settings for debug #CONFIG += CONSOLE - LIBS += -L$$BASEDIR\lib\sdl\win32 \ - -lmingw32 -lSDLmain -lSDL -mwindows - - INCLUDEPATH += $$BASEDIR/lib/sdl/include #\ + + INCLUDEPATH += $$BASEDIR\lib\sdl\include #\ #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" + LIBS += -L$$BASEDIR\lib\sdl\win32 \ + -lmingw32 -lSDLmain -lSDL -mwindows + + + debug { DESTDIR = $$BASEDIR/bin } diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index f1d37c7..4528dcc 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -42,9 +42,14 @@ DEPENDPATH += . \ plugins INCLUDEPATH += . \ lib/QMapControl \ - ../mavlink/include \ - MAVLink/include \ - mavlink/include + ../mavlink/contrib/slugs/include \ + MAVLink/contrib/slugs/include \ + mavlink/contrib/slugs/include + + + # ../mavlink/include \ + #MAVLink/include \ + #mavlink/include # Input FORMS += src/ui/MainWindow.ui \ diff --git a/src/main.cc b/src/main.cc index 0c65955..9a9d5f2 100644 --- a/src/main.cc +++ b/src/main.cc @@ -33,10 +33,11 @@ This file is part of the PIXHAWK project #include "MainWindow.h" #include "configuration.h" + /* SDL does ugly things to main() */ -#ifdef main -# undef main -#endif +//#ifdef main +//# undef main +//#endif /** * @brief Starts the application diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index d5bf9f3..6da07e3 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -21,7 +21,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) // Let UAS handle the default message set UAS::receiveMessage(link, message); - // Handle your special messages + + // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: @@ -29,6 +30,95 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) qDebug() << "SLUGS RECEIVED HEARTBEAT"; break; } + case MAVLINK_MSG_ID_CPU_LOAD: + { + mavlink_cpu_load_t cpu_load; + quint64 time = getUnixTime(0); + mavlink_msg_cpu_load_decode(&message,&cpu_load); + + emit valueChanged(uasId, "CPU Load", cpu_load.target, time); + emit valueChanged(uasId, "SensorDSC Load", cpu_load.sensLoad, time); + emit valueChanged(uasId, "ControlDSC Load", cpu_load.ctrlLoad, time); + emit valueChanged(uasId, "Battery Volt", cpu_load.batVolt, time); + + break; + } + case MAVLINK_MSG_ID_AIR_DATA: + { + mavlink_air_data_t air_data; + quint64 time = getUnixTime(0); + mavlink_msg_air_data_decode(&message,&air_data); + emit valueChanged(uasId, "Air Data",air_data.target,time); + emit valueChanged(uasId, "Presion Dinamica", air_data.dynamicPressure,time); + emit valueChanged(uasId, "Presion Estatica",air_data.staticPressure, time); + emit valueChanged(uasId, "Temperatura",air_data.temperature,time); + + break; + } + case MAVLINK_MSG_ID_SENSOR_BIAS: + { + mavlink_sensor_bias_t sensor_bias; + quint64 time = getUnixTime(0); + mavlink_msg_sensor_bias_decode(&message,&sensor_bias); + emit valueChanged(uasId,"Sensor Bias",sensor_bias.target, time); + emit valueChanged(uasId,"Acelerometro X",sensor_bias.axBias, time); + emit valueChanged(uasId,"Acelerometro y",sensor_bias.ayBias,time); + emit valueChanged(uasId,"Acelerometro Z",sensor_bias.azBias,time); + emit valueChanged(uasId,"Gyro X",sensor_bias.gxBias,time); + emit valueChanged(uasId,"Gyro Y",sensor_bias.gyBias,time); + emit valueChanged(uasId,"Gyro Z",sensor_bias.gzBias,time); + + break; + } + case MAVLINK_MSG_ID_DIAGNOSTIC: + { + mavlink_diagnostic_t diagnostic; + quint64 time = getUnixTime(0); + mavlink_msg_diagnostic_decode(&message,&diagnostic); + emit valueChanged(uasId,"Diagnostico",diagnostic.target,time); + emit valueChanged(uasId,"Diagnostico F1",diagnostic.diagFl1,time); + emit valueChanged(uasId,"Diagnostico F2",diagnostic.diagFl2,time); + emit valueChanged(uasId,"Diagnostico F3",diagnostic.diagFl3,time); + emit valueChanged(uasId,"Diagnostico S1",diagnostic.diagSh1,time); + emit valueChanged(uasId,"Diagnostico S2",diagnostic.diagSh2,time); + emit valueChanged(uasId,"Diagnostico S3",diagnostic.diagSh3,time); + + break; + } + case MAVLINK_MSG_ID_PILOT_CONSOLE: + { + mavlink_pilot_console_t pilot; + quint64 time = getUnixTime(0); + mavlink_msg_pilot_console_decode(&message,&pilot); + emit valueChanged(uasId,"Mensajes PWM",pilot.target,time); + emit valueChanged(uasId,"Aceleracion Consola",pilot.dt,time); + emit valueChanged(uasId,"Aleron Izq Consola",pilot.dla,time); + emit valueChanged(uasId,"Aleron Der Consola",pilot.dra,time); + emit valueChanged(uasId,"Timon Consola",pilot.dr,time); + emit valueChanged(uasId,"Elevador Consola",pilot.de,time); + + break; + } + case MAVLINK_MSG_ID_PWM_COMMANDS: + { + mavlink_pwm_commands_t pwm; + quint64 time = getUnixTime(0); + mavlink_msg_pwm_commands_decode(&message,&pwm); + emit valueChanged(uasId,"Superficies de Control",pwm.target,time); + emit valueChanged(uasId,"Comando Aceleracion PA",pwm.dt_c,time); + emit valueChanged(uasId,"Comando Aleron Izq PA",pwm.dla_c,time); + emit valueChanged(uasId,"Comando Aleron Der PA",pwm.dra_c,time); + emit valueChanged(uasId,"Comando Timon PA",pwm.dr_c,time); + emit valueChanged(uasId,"Comando elevador Izq PA",pwm.dle_c,time); + emit valueChanged(uasId,"Comando Elevador Der PA",pwm.dre_c,time); + emit valueChanged(uasId,"Comando Flap Izq PA",pwm.dlf_c,time); + emit valueChanged(uasId,"Comando Flap Der PA",pwm.drf_c,time); + emit valueChanged(uasId,"Comando Aux1 PA",pwm.aux1,time); + emit valueChanged(uasId,"Comando Aux2 PA",pwm.aux2,time); + + + break; + } default: qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; break; diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index d983f86..84f04ad 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -6,6 +6,7 @@ class SlugsMAV : public UAS { Q_OBJECT + Q_INTERFACES(UASInterface) public: SlugsMAV(MAVLinkProtocol* mavlink, int id = 0); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 2dee931..67d19ec 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -666,6 +666,7 @@ void UAS::writeParametersToStorage() mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); + //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); sendMessage(msg); } From 5cb1ee847162de73f780d72b34121c6509e88b56 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Mon, 12 Jul 2010 15:24:43 -0500 Subject: [PATCH 2/3] Changes made to try to compile with bugs introduced when addign non standard messages to UAS.cc --- qgroundcontrol.pri | 3 ++- qgroundcontrol.pro | 5 ++--- src/comm/MAVLinkSimulationLink.cc | 18 ++++++++++++++---- src/uas/SlugsMAV.cc | 6 +++++- src/uas/UAS.cc | 34 +++++++++++++++++++++++++++++----- 5 files changed, 52 insertions(+), 14 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index afd0a17..b7d6a33 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -54,10 +54,11 @@ macx { message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) } - QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.5 + QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.6 DESTDIR = $$BASEDIR/bin/mac INCLUDEPATH += -framework SDL \ + $$BASEDIR/../mavlink/contrib/slugs/include \ $$BASEDIR/../mavlink/include LIBS += -framework IOKit \ diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 4528dcc..90fbdff 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -42,9 +42,8 @@ DEPENDPATH += . \ plugins INCLUDEPATH += . \ lib/QMapControl \ - ../mavlink/contrib/slugs/include \ - MAVLink/contrib/slugs/include \ - mavlink/contrib/slugs/include + $$BASEDIR/../mavlink/contrib/slugs/include \ + $$BASEDIR/../mavlink/include # ../mavlink/include \ diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 6fce11a..ca5bd4b 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -186,7 +186,9 @@ void MAVLinkSimulationLink::mainloop() static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second mavlink_attitude_t attitude; - mavlink_raw_aux_t rawAuxValues; + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES + mavlink_raw_aux_t rawAuxValues; + #endif mavlink_raw_imu_t rawImuValues; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -305,7 +307,7 @@ void MAVLinkSimulationLink::mainloop() { rawImuValues.zgyro = d; } - +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES if (keys.value(i, "") == "Pressure") { rawAuxValues.baro = d; @@ -315,7 +317,7 @@ void MAVLinkSimulationLink::mainloop() { rawAuxValues.vbat = d; } - +#endif if (keys.value(i, "") == "roll_IMU") { attitude.roll = d; @@ -459,7 +461,10 @@ void MAVLinkSimulationLink::mainloop() uint8_t visLock = 3; uint8_t posLock = qMax(gpsLock, visLock); + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, attControl, posXYControl, posZControl, posYawControl); + #endif + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); memcpy(stream+streampointer, buffer, bufferlength); streampointer += bufferlength; @@ -493,7 +498,9 @@ void MAVLinkSimulationLink::mainloop() //qDebug() << "BOOT" << "BUF LEN" << bufferlength << "POINTER" << streampointer; // AUX STATUS + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES rawAuxValues.vbat = voltage; +#endif rate1hzCounter = 1; } @@ -632,14 +639,17 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } } break; - +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES case MAVLINK_MSG_ID_MANUAL_CONTROL: { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES mavlink_manual_control_t control; mavlink_msg_manual_control_decode(&msg, &control); qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch; + #endif } break; +#endif case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION"; diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 6da07e3..801a84d 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -21,7 +21,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) // Let UAS handle the default message set UAS::receiveMessage(link, message); - // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { @@ -30,6 +29,9 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) qDebug() << "SLUGS RECEIVED HEARTBEAT"; break; } + +#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES + case MAVLINK_MSG_ID_CPU_LOAD: { mavlink_cpu_load_t cpu_load; @@ -119,6 +121,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) break; } +#endif + default: qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f00419f..ee19485 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -298,12 +298,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "x", pos.x, time); emit valueChanged(uasId, "y", pos.y, time); emit valueChanged(uasId, "z", pos.z, time); - emit valueChanged(uasId, "roll", pos.roll, time); - emit valueChanged(uasId, "pitch", pos.pitch, time); - emit valueChanged(uasId, "yaw", pos.yaw, time); + emit valueChanged(uasId, "Vx", pos.vx, time); + emit valueChanged(uasId, "Vy", pos.vy, time); + emit valueChanged(uasId, "Vz", pos.vz, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - //emit speedChanged(this, pos.roll, pos.pitch, pos.yaw, time); - emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time); + emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + //emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time); // Set internal state if (!positionLock) { @@ -502,9 +502,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES mavlink_message_t msg; mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); sendMessage(msg); + #endif } quint64 UAS::getUnixTime(quint64 time) @@ -681,6 +683,7 @@ void UAS::readParametersFromStorage() void UAS::enableAllDataTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -701,10 +704,12 @@ void UAS::enableAllDataTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableRawSensorDataTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -723,10 +728,12 @@ void UAS::enableRawSensorDataTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableExtendedSystemStatusTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -745,10 +752,12 @@ void UAS::enableExtendedSystemStatusTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableRCChannelDataTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -767,10 +776,12 @@ void UAS::enableRCChannelDataTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableRawControllerDataTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -789,10 +800,12 @@ void UAS::enableRawControllerDataTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableRawSensorFusionTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -811,10 +824,12 @@ void UAS::enableRawSensorFusionTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enablePositionTransmission(bool enabled) { +#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -833,10 +848,12 @@ void UAS::enablePositionTransmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableExtra1Transmission(bool enabled) { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -855,10 +872,12 @@ void UAS::enableExtra1Transmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableExtra2Transmission(bool enabled) { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -877,10 +896,12 @@ void UAS::enableExtra2Transmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::enableExtra3Transmission(bool enabled) { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES // Buffers to write data to mavlink_message_t msg; mavlink_request_data_stream_t stream; @@ -899,6 +920,7 @@ void UAS::enableExtra3Transmission(bool enabled) // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); +#endif } void UAS::setParameter(int component, QString id, float value) @@ -989,12 +1011,14 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double if(mode == (int)MAV_MODE_MANUAL) { + #ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES mavlink_message_t message; mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); + #endif } } From cb30604785f664a1cf71a0e9502391e046aadd04 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Mon, 12 Jul 2010 16:38:36 -0500 Subject: [PATCH 3/3] Removed comments on the SDL _main protection --- src/main.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main.cc b/src/main.cc index 9a9d5f2..dbb457c 100644 --- a/src/main.cc +++ b/src/main.cc @@ -35,9 +35,9 @@ This file is part of the PIXHAWK project /* SDL does ugly things to main() */ -//#ifdef main -//# undef main -//#endif +#ifdef main +# undef main +#endif /** * @brief Starts the application