Permalink
Browse files

Fixed HIL startup crash, fixed yaw offset

  • Loading branch information...
1 parent f9a4353 commit 31a1a0700935bc9f0e2473f0028dbccb88e1f99c @LorenzMeier LorenzMeier committed Sep 29, 2012
Showing with 11 additions and 6 deletions.
  1. +5 −5 src/comm/QGCXPlaneLink.cc
  2. +5 −1 src/uas/UAS.cc
  3. +1 −0 src/uas/UAS.h
View
@@ -317,7 +317,7 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
pitch = p.f[0] / 180.0f * M_PI;
roll = p.f[1] / 180.0f * M_PI;
- yaw = (p.f[2] - 180.0f) / 180.0f * M_PI;
+ yaw = p.f[2] / 180.0f * M_PI;
}
// else if (p.index == 19)
@@ -552,6 +552,10 @@ void QGCXPlaneLink::setRandomAttitude()
**/
bool QGCXPlaneLink::connectSimulation()
{
+ qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
+
+ start(LowPriority);
+
if (!mav) return false;
if (connectState) return false;
@@ -772,10 +776,6 @@ bool QGCXPlaneLink::connectSimulation()
// qDebug() << "STARTING SIM";
// qDebug() << "STARTING: " << processFgfs << processCall;
-
- qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
-
- start(LowPriority);
return connectState;
}
View
@@ -106,7 +106,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
connectionLost(false),
lastVoltageWarning(0),
lastNonNullTime(0),
- onboardTimeOffsetInvalidCount(0)
+ onboardTimeOffsetInvalidCount(0),
+ hilEnabled(false)
{
for (unsigned int i = 0; i<255;++i)
@@ -2605,6 +2606,8 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
**/
void UAS::startHil()
{
+ if (hilEnabled) return;
+ hilEnabled = true;
// Connect HIL simulation link
simulation->connectSimulation();
mavlink_message_t msg;
@@ -2621,6 +2624,7 @@ void UAS::stopHil()
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
sendMessage(msg);
+ hilEnabled = false;
}
void UAS::shutdown()
View
@@ -680,6 +680,7 @@ public slots:
quint64 lastVoltageWarning; ///< Time at which the last voltage warning occured
quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null
unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong
+ bool hilEnabled; ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting)
protected slots:
/** @brief Write settings to disk */

0 comments on commit 31a1a07

Please sign in to comment.