Commit a0d526bf authored by lm's avatar lm

Cleaned up compile environment

parent 49d61d9e
...@@ -73,6 +73,8 @@ bool MAVLinkXMLParser::generate() ...@@ -73,6 +73,8 @@ bool MAVLinkXMLParser::generate()
QList< QPair<QString, QString> > cFiles; QList< QPair<QString, QString> > cFiles;
QString lcmStructDefs = ""; QString lcmStructDefs = "";
QString pureFileName;
// Run through root children // Run through root children
while(!n.isNull()) while(!n.isNull())
{ {
...@@ -103,6 +105,7 @@ bool MAVLinkXMLParser::generate() ...@@ -103,6 +105,7 @@ bool MAVLinkXMLParser::generate()
{ {
QFileInfo rInfo(this->fileName); QFileInfo rInfo(this->fileName);
fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName; fileName = rInfo.absoluteDir().canonicalPath() + "/" + fileName;
pureFileName = rInfo.baseName().split(".").first();
} }
QFile file(fileName); QFile file(fileName);
...@@ -365,6 +368,7 @@ bool MAVLinkXMLParser::generate() ...@@ -365,6 +368,7 @@ bool MAVLinkXMLParser::generate()
// Mark all code as C code // Mark all code as C code
mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n";
mainHeader += "\n#include \"protocol.h\"\n"; mainHeader += "\n#include \"protocol.h\"\n";
mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n";
QString includeLine = "#include \"%1\"\n"; QString includeLine = "#include \"%1\"\n";
QString mainHeaderName = "mavlink.h"; QString mainHeaderName = "mavlink.h";
QString messagesDirName = "generated"; QString messagesDirName = "generated";
......
...@@ -22,10 +22,36 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -22,10 +22,36 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
//qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid;
// Only compile this portion if matching MAVLink packets have been compiled
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
// Handle your special messages if (message.sysid == uasId)
switch (msg->msgid)
{ {
QString uasState;
QString stateDescription;
QString patternPath;
switch (message.msgid)
{
case MAVLINK_MSG_ID_RAW_AUX:
{
mavlink_raw_aux_t raw;
mavlink_msg_raw_aux_decode(&message, &raw);
quint64 time = getUnixTime(0);
emit valueChanged(uasId, "Pressure", raw.baro, time);
emit valueChanged(uasId, "Temperature", raw.temp, time);
}
break;
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
QByteArray b;
b.resize(256);
mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
b.append('\0');
QString path = QString(b);
bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
}
break;
case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT: case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
{ {
mavlink_watchdog_heartbeat_t payload; mavlink_watchdog_heartbeat_t payload;
...@@ -103,10 +129,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -103,10 +129,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Do nothing // Do nothing
break; break;
} }
}
#endif
} }
void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command) void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
mavlink_watchdog_command_t payload; mavlink_watchdog_command_t payload;
payload.target_system_id = uasId; payload.target_system_id = uasId;
payload.watchdog_id = watchdogId; payload.watchdog_id = watchdogId;
...@@ -116,4 +146,5 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c ...@@ -116,4 +146,5 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload); mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
sendMessage(msg); sendMessage(msg);
#endif
} }
...@@ -266,15 +266,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -266,15 +266,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "Mag. Z", raw.zmag, time); emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
} }
break; break;
case MAVLINK_MSG_ID_RAW_AUX:
{
mavlink_raw_aux_t raw;
mavlink_msg_raw_aux_decode(&message, &raw);
quint64 time = getUnixTime(0);
emit valueChanged(uasId, "Pressure", raw.baro, time);
emit valueChanged(uasId, "Temperature", raw.temp, time);
}
break;
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
//std::cerr << std::endl; //std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
...@@ -349,7 +340,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -349,7 +340,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
//qDebug() << "GOT GPS STATUS FOR "<< pos.satellites_visible << "SATELLITES"; //qDebug() << "GOT GPS STATUS FOR "<< pos.satellites_visible << "SATELLITES";
for(int i = 0; i < (int)pos.satellites_visible; i++) for(int i = 0; i < (int)pos.satellites_visible; i++)
{ {
emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_azimuth[i], pos.satellite_direction[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i])); emit gpsSatelliteStatusChanged(uasId, i, pos.satellite_elevation[i], pos.satellite_azimuth[i], pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
} }
} }
break; break;
...@@ -390,17 +381,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) ...@@ -390,17 +381,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit textMessageReceived(uasId, severity, text); emit textMessageReceived(uasId, severity, text);
} }
break; break;
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
QByteArray b;
b.resize(256);
mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
b.append('\0');
QString path = QString(b);
bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
}
break;
default: default:
{ {
if (!unknownPackets.contains(message.msgid)) if (!unknownPackets.contains(message.msgid))
......
...@@ -146,7 +146,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) ...@@ -146,7 +146,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
//} //}
} }
void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used) void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float azimuth, float snr, bool used)
{ {
Q_UNUSED(uasid); Q_UNUSED(uasid);
//qDebug() << "UPDATED SATELLITE"; //qDebug() << "UPDATED SATELLITE";
...@@ -159,12 +159,12 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float dire ...@@ -159,12 +159,12 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float azimuth, float dire
if (gpsSatellites.at(satid) == NULL) if (gpsSatellites.at(satid) == NULL)
{ {
gpsSatellites.insert(satid, new GPSSatellite(satid, azimuth, direction, snr, used)); gpsSatellites.insert(satid, new GPSSatellite(satid, elevation, azimuth, snr, used));
} }
else else
{ {
// Satellite exists, update it // Satellite exists, update it
gpsSatellites.at(satid)->update(satid, azimuth, direction, snr, used); gpsSatellites.at(satid)->update(satid, elevation, azimuth, snr, used);
} }
} }
...@@ -230,8 +230,8 @@ void HSIDisplay::drawGPS() ...@@ -230,8 +230,8 @@ void HSIDisplay::drawGPS()
painter.setPen(color); painter.setPen(color);
painter.setBrush(brush); painter.setBrush(brush);
float xPos = xCenter + sin(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; float xPos = xCenter + sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * (sat->elevation/180.0f * M_PI) * radius;
float yPos = yCenter + cos(sat->direction/180.0f * M_PI) * (sat->azimuth/180.0f * M_PI) * radius; float yPos = yCenter + cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * (sat->elevation/180.0f * M_PI) * radius;
drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter); drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);
} }
......
...@@ -68,28 +68,28 @@ protected: ...@@ -68,28 +68,28 @@ protected:
class GPSSatellite class GPSSatellite
{ {
public: public:
GPSSatellite(int id, float azimuth, float direction, float snr, bool used) : GPSSatellite(int id, float elevation, float azimuth, float snr, bool used) :
id(id), id(id),
elevation(elevation),
azimuth(azimuth), azimuth(azimuth),
direction(direction),
snr(snr), snr(snr),
used(used) used(used)
{ {
} }
void update(int id, float azimuth, float direction, float snr, bool used) void update(int id, float elevation, float azimuth, float snr, bool used)
{ {
this->id = id; this->id = id;
this->elevation = elevation;
this->azimuth = azimuth; this->azimuth = azimuth;
this->direction = direction;
this->snr = snr; this->snr = snr;
this->used = used; this->used = used;
} }
int id; int id;
float elevation;
float azimuth; float azimuth;
float direction;
float snr; float snr;
bool used; bool used;
......
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