Commit fb859599 authored by Lorenz Meier's avatar Lorenz Meier

Fixed state HIL, fixed sensor HIL XPlane x direction velocity vector

parent ec4cfb8e
......@@ -567,8 +567,8 @@ void QGCXPlaneLink::readBytes()
}
else if (p.index == 21 && xPlaneVersion == 10)
{
vx = p.f[3];
vy = -p.f[5];
vy = p.f[3];
vx = -p.f[5];
vz = p.f[4];
}
else if (p.index == 12)
......
......@@ -112,7 +112,9 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
lastNonNullTime(0),
onboardTimeOffsetInvalidCount(0),
hilEnabled(false),
lastSendTimeGPS(0)
sensorHil(false),
lastSendTimeGPS(0),
lastSendTimeSensors(0)
{
for (unsigned int i = 0; i<255;++i)
......@@ -2733,7 +2735,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
{
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
{
if (sensorHil) {
if (QGC::groundTimeMilliseconds() - lastSendTimeSensors < 100) {
// Emit attitude for cross-check
emit attitudeChanged(this, 201, roll, pitch, yaw, getUnixTime());
emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime());
......@@ -2777,6 +2779,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
fields_changed);
sendMessage(msg);
sensorHil = true;
lastSendTimeSensors = QGC::groundTimeMilliseconds();
}
else
{
......
......@@ -716,6 +716,7 @@ protected:
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)
bool sensorHil; ///< True if sensor HIL is enabled
quint64 lastSendTimeGPS; ///< Last HIL GPS message sent
quint64 lastSendTimeSensors;
protected slots:
/** @brief Write settings to disk */
......
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