Commit 5cb1ee84 authored by Mariano Lizarraga's avatar Mariano Lizarraga

Changes made to try to compile with bugs introduced when addign non standard messages to UAS.cc

parent aba75569
...@@ -54,10 +54,11 @@ macx { ...@@ -54,10 +54,11 @@ macx {
message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) 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 DESTDIR = $$BASEDIR/bin/mac
INCLUDEPATH += -framework SDL \ INCLUDEPATH += -framework SDL \
$$BASEDIR/../mavlink/contrib/slugs/include \
$$BASEDIR/../mavlink/include $$BASEDIR/../mavlink/include
LIBS += -framework IOKit \ LIBS += -framework IOKit \
......
...@@ -42,9 +42,8 @@ DEPENDPATH += . \ ...@@ -42,9 +42,8 @@ DEPENDPATH += . \
plugins plugins
INCLUDEPATH += . \ INCLUDEPATH += . \
lib/QMapControl \ lib/QMapControl \
../mavlink/contrib/slugs/include \ $$BASEDIR/../mavlink/contrib/slugs/include \
MAVLink/contrib/slugs/include \ $$BASEDIR/../mavlink/include
mavlink/contrib/slugs/include
# ../mavlink/include \ # ../mavlink/include \
......
...@@ -186,7 +186,9 @@ void MAVLinkSimulationLink::mainloop() ...@@ -186,7 +186,9 @@ void MAVLinkSimulationLink::mainloop()
static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second
mavlink_attitude_t attitude; 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; mavlink_raw_imu_t rawImuValues;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
...@@ -305,7 +307,7 @@ void MAVLinkSimulationLink::mainloop() ...@@ -305,7 +307,7 @@ void MAVLinkSimulationLink::mainloop()
{ {
rawImuValues.zgyro = d; rawImuValues.zgyro = d;
} }
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
if (keys.value(i, "") == "Pressure") if (keys.value(i, "") == "Pressure")
{ {
rawAuxValues.baro = d; rawAuxValues.baro = d;
...@@ -315,7 +317,7 @@ void MAVLinkSimulationLink::mainloop() ...@@ -315,7 +317,7 @@ void MAVLinkSimulationLink::mainloop()
{ {
rawAuxValues.vbat = d; rawAuxValues.vbat = d;
} }
#endif
if (keys.value(i, "") == "roll_IMU") if (keys.value(i, "") == "roll_IMU")
{ {
attitude.roll = d; attitude.roll = d;
...@@ -459,7 +461,10 @@ void MAVLinkSimulationLink::mainloop() ...@@ -459,7 +461,10 @@ void MAVLinkSimulationLink::mainloop()
uint8_t visLock = 3; uint8_t visLock = 3;
uint8_t posLock = qMax(gpsLock, visLock); 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); 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); bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
memcpy(stream+streampointer, buffer, bufferlength); memcpy(stream+streampointer, buffer, bufferlength);
streampointer += bufferlength; streampointer += bufferlength;
...@@ -493,7 +498,9 @@ void MAVLinkSimulationLink::mainloop() ...@@ -493,7 +498,9 @@ void MAVLinkSimulationLink::mainloop()
//qDebug() << "BOOT" << "BUF LEN" << bufferlength << "POINTER" << streampointer; //qDebug() << "BOOT" << "BUF LEN" << bufferlength << "POINTER" << streampointer;
// AUX STATUS // AUX STATUS
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
rawAuxValues.vbat = voltage; rawAuxValues.vbat = voltage;
#endif
rate1hzCounter = 1; rate1hzCounter = 1;
} }
...@@ -632,14 +639,17 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) ...@@ -632,14 +639,17 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
} }
} }
break; break;
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
case MAVLINK_MSG_ID_MANUAL_CONTROL: case MAVLINK_MSG_ID_MANUAL_CONTROL:
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_manual_control_t control; mavlink_manual_control_t control;
mavlink_msg_manual_control_decode(&msg, &control); mavlink_msg_manual_control_decode(&msg, &control);
qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch; qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch;
#endif
} }
break; break;
#endif
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{ {
qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION"; qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION";
......
...@@ -21,7 +21,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -21,7 +21,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Let UAS handle the default message set // Let UAS handle the default message set
UAS::receiveMessage(link, message); UAS::receiveMessage(link, message);
// Handle your special messages mavlink_message_t* msg = &message; // Handle your special messages mavlink_message_t* msg = &message;
switch (message.msgid) switch (message.msgid)
{ {
...@@ -30,6 +29,9 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -30,6 +29,9 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
qDebug() << "SLUGS RECEIVED HEARTBEAT"; qDebug() << "SLUGS RECEIVED HEARTBEAT";
break; break;
} }
#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES
case MAVLINK_MSG_ID_CPU_LOAD: case MAVLINK_MSG_ID_CPU_LOAD:
{ {
mavlink_cpu_load_t cpu_load; mavlink_cpu_load_t cpu_load;
...@@ -119,6 +121,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -119,6 +121,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
break; break;
} }
#endif
default: default:
qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
break; break;
......
...@@ -298,12 +298,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -298,12 +298,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "x", pos.x, time); emit valueChanged(uasId, "x", pos.x, time);
emit valueChanged(uasId, "y", pos.y, time); emit valueChanged(uasId, "y", pos.y, time);
emit valueChanged(uasId, "z", pos.z, time); emit valueChanged(uasId, "z", pos.z, time);
emit valueChanged(uasId, "roll", pos.roll, time); emit valueChanged(uasId, "Vx", pos.vx, time);
emit valueChanged(uasId, "pitch", pos.pitch, time); emit valueChanged(uasId, "Vy", pos.vy, time);
emit valueChanged(uasId, "yaw", pos.yaw, time); emit valueChanged(uasId, "Vz", pos.vz, time);
emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
//emit speedChanged(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); //emit attitudeChanged(this, pos.roll, pos.pitch, pos.yaw, time);
// Set internal state // Set internal state
if (!positionLock) if (!positionLock)
{ {
...@@ -502,9 +502,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -502,9 +502,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw);
sendMessage(msg); sendMessage(msg);
#endif
} }
quint64 UAS::getUnixTime(quint64 time) quint64 UAS::getUnixTime(quint64 time)
...@@ -681,6 +683,7 @@ void UAS::readParametersFromStorage() ...@@ -681,6 +683,7 @@ void UAS::readParametersFromStorage()
void UAS::enableAllDataTransmission(bool enabled) void UAS::enableAllDataTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -701,10 +704,12 @@ void UAS::enableAllDataTransmission(bool enabled) ...@@ -701,10 +704,12 @@ void UAS::enableAllDataTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableRawSensorDataTransmission(bool enabled) void UAS::enableRawSensorDataTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -723,10 +728,12 @@ void UAS::enableRawSensorDataTransmission(bool enabled) ...@@ -723,10 +728,12 @@ void UAS::enableRawSensorDataTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableExtendedSystemStatusTransmission(bool enabled) void UAS::enableExtendedSystemStatusTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -745,10 +752,12 @@ void UAS::enableExtendedSystemStatusTransmission(bool enabled) ...@@ -745,10 +752,12 @@ void UAS::enableExtendedSystemStatusTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableRCChannelDataTransmission(bool enabled) void UAS::enableRCChannelDataTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -767,10 +776,12 @@ void UAS::enableRCChannelDataTransmission(bool enabled) ...@@ -767,10 +776,12 @@ void UAS::enableRCChannelDataTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableRawControllerDataTransmission(bool enabled) void UAS::enableRawControllerDataTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -789,10 +800,12 @@ void UAS::enableRawControllerDataTransmission(bool enabled) ...@@ -789,10 +800,12 @@ void UAS::enableRawControllerDataTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableRawSensorFusionTransmission(bool enabled) void UAS::enableRawSensorFusionTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -811,10 +824,12 @@ void UAS::enableRawSensorFusionTransmission(bool enabled) ...@@ -811,10 +824,12 @@ void UAS::enableRawSensorFusionTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enablePositionTransmission(bool enabled) void UAS::enablePositionTransmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -833,10 +848,12 @@ void UAS::enablePositionTransmission(bool enabled) ...@@ -833,10 +848,12 @@ void UAS::enablePositionTransmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableExtra1Transmission(bool enabled) void UAS::enableExtra1Transmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -855,10 +872,12 @@ void UAS::enableExtra1Transmission(bool enabled) ...@@ -855,10 +872,12 @@ void UAS::enableExtra1Transmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableExtra2Transmission(bool enabled) void UAS::enableExtra2Transmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -877,10 +896,12 @@ void UAS::enableExtra2Transmission(bool enabled) ...@@ -877,10 +896,12 @@ void UAS::enableExtra2Transmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::enableExtra3Transmission(bool enabled) void UAS::enableExtra3Transmission(bool enabled)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Buffers to write data to // Buffers to write data to
mavlink_message_t msg; mavlink_message_t msg;
mavlink_request_data_stream_t stream; mavlink_request_data_stream_t stream;
...@@ -899,6 +920,7 @@ void UAS::enableExtra3Transmission(bool enabled) ...@@ -899,6 +920,7 @@ void UAS::enableExtra3Transmission(bool enabled)
// Send message twice to increase chance of reception // Send message twice to increase chance of reception
sendMessage(msg); sendMessage(msg);
sendMessage(msg); sendMessage(msg);
#endif
} }
void UAS::setParameter(int component, QString id, float value) void UAS::setParameter(int component, QString id, float value)
...@@ -989,12 +1011,14 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double ...@@ -989,12 +1011,14 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double
if(mode == (int)MAV_MODE_MANUAL) if(mode == (int)MAV_MODE_MANUAL)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_message_t message; 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); 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); sendMessage(message);
qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; 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()); emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
#endif
} }
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment