zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Adapted to new MAVLink messages

QGC4.4
lm 15 years ago
parent
acd864237c
commit
09ebae051e
2 changed files with 26 additions and 31 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 26
      src/uas/PxQuadMAV.cc
  2. 31
      src/uas/UAS.cc

26
src/uas/PxQuadMAV.cc
Unescape View File

@ -1,4 +1,5 @@ @@ -1,4 +1,5 @@
#include "PxQuadMAV.h"
#include "GAudioOutput.h"
PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) :
UAS(mavlink, id)
@ -60,6 +61,31 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -60,6 +61,31 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, str+".z", vect.z, MG::TIME::getGroundTimeNow());
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
emit valueChanged(uasId, "vis. time", pos.usec, time);
emit valueChanged(uasId, "vis. roll", pos.roll, time);
emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
emit valueChanged(uasId, "vis. x", pos.x, time);
emit valueChanged(uasId, "vis. y", pos.y, time);
emit valueChanged(uasId, "vis. z", pos.z, time);
emit valueChanged(uasId, "vis. vx", pos.vx, time);
emit valueChanged(uasId, "vis. vy", pos.vy, time);
emit valueChanged(uasId, "vis. vz", pos.vz, time);
emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
// Set internal state
if (!positionLock)
{
// If position was not locked before, notify positive
GAudioOutput::instance()->notifyPositive();
}
positionLock = true;
}
break;
default:
// Do nothing
break;

31
src/uas/UAS.cc
Unescape View File

@ -306,37 +306,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -306,37 +306,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time);
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{
mavlink_vision_position_estimate_t pos;
mavlink_msg_vision_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
emit valueChanged(uasId, "vis. time", pos.usec, time);
emit valueChanged(uasId, "vis. roll", pos.roll, time);
emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
emit valueChanged(uasId, "vis. x", pos.x, time);
emit valueChanged(uasId, "vis. y", pos.y, time);
emit valueChanged(uasId, "vis. z", pos.z, time);
// FIXME Only for testing for now
emit valueChanged(uasId, "vis. rot r1", pos.r1, time);
emit valueChanged(uasId, "vis. rot r2", pos.r2, time);
emit valueChanged(uasId, "vis. rot r3", pos.r3, time);
emit valueChanged(uasId, "vis. rot r4", pos.r4, time);
emit valueChanged(uasId, "vis. rot r5", pos.r5, time);
emit valueChanged(uasId, "vis. rot r6", pos.r6, time);
emit valueChanged(uasId, "vis. rot r7", pos.r7, time);
emit valueChanged(uasId, "vis. rot r8", pos.r8, time);
emit valueChanged(uasId, "vis. rot r9", pos.r9, time);
// Set internal state
if (!positionLock)
{
// If position was not locked before, notify positive
GAudioOutput::instance()->notifyPositive();
}
positionLock = true;
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION:
//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;

Write Preview
Loading…
Cancel
Save