Browse Source

Moved debug message to common message set

QGC4.4
lm 15 years ago
parent
commit
e2f510f3c5
  1. 11
      src/uas/PxQuadMAV.cc
  2. 13
      src/uas/UAS.cc

11
src/uas/PxQuadMAV.cc

@ -104,17 +104,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid); emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
} }
break; break;
case MAVLINK_MSG_ID_DEBUG_VECT:
{
mavlink_debug_vect_t vect;
mavlink_msg_debug_vect_decode(msg, &vect);
QString str((const char*)vect.name);
quint64 time = getUnixTime(vect.usec);
emit valueChanged(uasId, str+".x", vect.x, time);
emit valueChanged(uasId, str+".y", vect.y, time);
emit valueChanged(uasId, str+".z", vect.z, time);
}
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
{ {
mavlink_vision_position_estimate_t pos; mavlink_vision_position_estimate_t pos;

13
src/uas/UAS.cc

@ -565,6 +565,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit textMessageReceived(uasId, message.compid, severity, text); emit textMessageReceived(uasId, message.compid, severity, text);
} }
break; break;
case MAVLINK_MSG_ID_DEBUG_VECT:
{
mavlink_debug_vect_t vect;
mavlink_msg_debug_vect_decode(&message, &vect);
QString str((const char*)vect.name);
quint64 time = getUnixTime(vect.usec);
emit valueChanged(uasId, str+".x", vect.x, time);
emit valueChanged(uasId, str+".y", vect.y, time);
emit valueChanged(uasId, str+".z", vect.z, time);
}
break;
//#ifdef MAVLINK_ENABLED_PIXHAWK //#ifdef MAVLINK_ENABLED_PIXHAWK
// case MAVLINK_MSG_ID_POINT_OF_INTEREST: // case MAVLINK_MSG_ID_POINT_OF_INTEREST:
// { // {
@ -1213,14 +1224,12 @@ 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
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
// } // }
} }

Loading…
Cancel
Save