From f9318458a517070af354f2f4c3b9dcfa7b658e9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Jul 2013 19:42:25 +0200 Subject: [PATCH] Updated MAVLink, fixed up HIL --- src/comm/MAVLinkSimulationMAV.cc | 44 ++++++- src/comm/QGCFlightGearLink.cc | 10 +- src/comm/QGCHilLink.h | 10 +- src/comm/QGCJSBSimLink.cc | 4 +- src/comm/QGCXPlaneLink.cc | 47 ++++--- src/comm/QGCXPlaneLink.h | 3 +- src/uas/UAS.cc | 194 +++++++++++++++++++++------- src/uas/UAS.h | 12 +- src/uas/UASInterface.h | 6 +- src/ui/QGCHilXPlaneConfiguration.cc | 6 +- src/ui/WaypointViewOnlyView.cc | 1 - src/ui/map/QGCMapWidget.cc | 2 +- 12 files changed, 250 insertions(+), 89 deletions(-) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index d65ea66dd..a9f16eb80 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -411,9 +411,47 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) { mavlink_hil_state_t state; mavlink_msg_hil_state_decode(&msg, &state); - roll = state.roll; - pitch = state.pitch; - yaw = state.yaw; + + double a = state.attitude_quaternion[0]; + double b = state.attitude_quaternion[1]; + double c = state.attitude_quaternion[2]; + double d = state.attitude_quaternion[3]; + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + float dcm[3][3]; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2.0 * (b * c - a * d); + dcm[0][2] = 2.0 * (a * c + b * d); + dcm[1][0] = 2.0 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2.0 * (c * d - a * b); + dcm[2][0] = 2.0 * (b * d - a * c); + dcm[2][1] = 2.0 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; + + float phi, theta, psi; + theta = asin(-dcm[2][0]); + + if (fabs(theta - M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + roll = phi; + pitch = theta; + yaw = psi; rollspeed = state.rollspeed; pitchspeed = state.pitchspeed; yawspeed = state.yawspeed; diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 2c1ec4b49..61341b048 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -242,6 +242,10 @@ void QGCFlightGearLink::readBytes() // Parse string float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; double lat, lon, alt; + + // XXX add + float ind_airspeed = 0.0f; + float true_airspeed = 0.0f; double vx, vy, vz, xacc, yacc, zacc; lat = values.at(1).toDouble(); @@ -265,7 +269,7 @@ void QGCFlightGearLink::readBytes() // Send updated state emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, xacc, yacc, zacc); + vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); // // Echo data for debugging purposes // std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; @@ -299,7 +303,7 @@ bool QGCFlightGearLink::disconnectSimulation() disconnect(process, SIGNAL(error(QProcess::ProcessError)), this, SLOT(processError(QProcess::ProcessError))); disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); if (process) { @@ -346,7 +350,7 @@ bool QGCFlightGearLink::connectSimulation() terraSync = new QProcess(this); connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); + connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); UAS* uas = dynamic_cast(mav); diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h index 155c9f5b0..788165097 100644 --- a/src/comm/QGCHilLink.h +++ b/src/comm/QGCHilLink.h @@ -91,16 +91,20 @@ signals: /** @brief State update from simulation */ void hilStateChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float xacc, float yacc, float zacc); + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); - void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites); + void hilGroundTruthChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, double lat, double lon, double alt, + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); + + void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites); void sensorHilRawImuChanged(quint64 time_us, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, - quint16 fields_updated); + quint32 fields_updated); /** @brief Remote host and port changed */ void remoteChanged(const QString& hostPort); diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc index a0dcf1131..8f0f23084 100644 --- a/src/comm/QGCJSBSimLink.cc +++ b/src/comm/QGCJSBSimLink.cc @@ -271,7 +271,7 @@ bool QGCJSBSimLink::disconnectSimulation() disconnect(process, SIGNAL(error(QProcess::ProcessError)), this, SLOT(processError(QProcess::ProcessError))); disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); if (process) { @@ -311,7 +311,7 @@ bool QGCJSBSimLink::connectSimulation() process = new QProcess(this); connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); - connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); + connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); UAS* uas = dynamic_cast(mav); diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 7e42a07a3..c71859b9f 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -54,7 +54,7 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress simUpdateLast(QGC::groundTimeMilliseconds()), simUpdateLastText(QGC::groundTimeMilliseconds()), simUpdateHz(0), - _sensorHilEnabled(false) + _sensorHilEnabled(true) { this->localHost = localHost; this->localPort = localPort/*+mav->getUASID()*/; @@ -468,7 +468,8 @@ void QGCXPlaneLink::readBytes() if (p.index == 3) { - airspeed = p.f[6] * 0.44704f; + ind_airspeed = p.f[5] * 0.44704f; + true_airspeed = p.f[6] * 0.44704f; groundspeed = p.f[7] * 0.44704; //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; @@ -634,19 +635,25 @@ void QGCXPlaneLink::readBytes() emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); - } - - int gps_fix_type = 3; - float eph = 0.3; - float epv = 0.6; - float vel = sqrt(vx*vx + vy*vy + vz*vz); - float cog = atan2(vy, vx); - int satellites = 8; - emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites); - emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, + // XXX make these GUI-configurable and add randomness + int gps_fix_type = 3; + float eph = 0.3; + float epv = 0.6; + float vel = sqrt(vx*vx + vy*vy + vz*vz); + float cog = atan2(vy, vx); + int satellites = 8; + + emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); + } else { + emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, - vx, vy, vz, xacc, yacc, zacc); + vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); + } + + emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, + pitchspeed, yawspeed, lat, lon, alt, + vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); } if (!oldConnectionState && xPlaneConnected) @@ -694,9 +701,10 @@ bool QGCXPlaneLink::disconnectSimulation() disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); disconnect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float))); - disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); - disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int))); - disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16))); + disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); + disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); UAS* uas = dynamic_cast(mav); if (uas) @@ -878,9 +886,10 @@ bool QGCXPlaneLink::connectSimulation() connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t))); connect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float))); - connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float))); - connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int))); - connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16))); + connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); + connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); + connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); UAS* uas = dynamic_cast(mav); if (uas) diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index ec7f362d2..696d3c2b1 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -193,7 +193,8 @@ protected: float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; double lat, lon, alt; float vx, vy, vz, xacc, yacc, zacc; - float airspeed; + float ind_airspeed; + float true_airspeed; float groundspeed; float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index a14e67f1e..2831bc476 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -26,6 +26,7 @@ #include "QGCMAVLink.h" #include "LinkManager.h" #include "SerialLink.h" +#include #ifdef QGC_PROTOBUF_ENABLED #include @@ -647,29 +648,70 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!wrongComponent) { lastAttitude = time; - //roll = QGC::limitAngleToPMPIf(attitude.roll); setRoll(QGC::limitAngleToPMPIf(attitude.roll)); - //pitch = QGC::limitAngleToPMPIf(attitude.pitch); setPitch(QGC::limitAngleToPMPIf(attitude.pitch)); - //yaw = QGC::limitAngleToPMPIf(attitude.yaw); setYaw(QGC::limitAngleToPMPIf(attitude.yaw)); - // // Emit in angles - - // // Convert yaw angle to compass value - // // in 0 - 360 deg range - // float compass = (yaw/M_PI)*180.0+360.0f; - // if (compass > -10000 && compass < 10000) - // { - // while (compass > 360.0f) { - // compass -= 360.0f; - // } - // } - // else - // { - // // Set to 0, since it is an invalid value - // compass = 0.0f; - // } + attitudeKnown = true; + emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); + emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } + } + break; + case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: + { + mavlink_attitude_quaternion_t attitude; + mavlink_msg_attitude_quaternion_decode(&message, &attitude); + quint64 time = getUnixReferenceTime(attitude.time_boot_ms); + + double a = attitude.q1; + double b = attitude.q2; + double c = attitude.q3; + double d = attitude.q4; + + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + float dcm[3][3]; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2.0 * (b * c - a * d); + dcm[0][2] = 2.0 * (a * c + b * d); + dcm[1][0] = 2.0 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2.0 * (c * d - a * b); + dcm[2][0] = 2.0 * (b * d - a * c); + dcm[2][1] = 2.0 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; + + float phi, theta, psi; + theta = asin(-dcm[2][0]); + + if (fabs(theta - M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi), + QGC::limitAngleToPMPIf(theta), + QGC::limitAngleToPMPIf(psi), time); + + if (!wrongComponent) + { + lastAttitude = time; + setRoll(QGC::limitAngleToPMPIf(phi)); + setPitch(QGC::limitAngleToPMPIf(theta)); + setYaw(QGC::limitAngleToPMPIf(psi)); attitudeKnown = true; emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); @@ -2889,6 +2931,66 @@ void UAS::enableHilXPlane(bool enable) } } +/** +* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) +* @param roll Roll angle (rad) +* @param pitch Pitch angle (rad) +* @param yaw Yaw angle (rad) +* @param rollspeed Roll angular speed (rad/s) +* @param pitchspeed Pitch angular speed (rad/s) +* @param yawspeed Yaw angular speed (rad/s) +* @param lat Latitude, expressed as * 1E7 +* @param lon Longitude, expressed as * 1E7 +* @param alt Altitude in meters, expressed as * 1000 (millimeters) +* @param vx Ground X Speed (Latitude), expressed as m/s * 100 +* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 +* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 +* @param xacc X acceleration (mg) +* @param yacc Y acceleration (mg) +* @param zacc Z acceleration (mg) +*/ +void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, double lat, double lon, double alt, + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) +{ + float q[4]; + + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); + + // Emit attitude for cross-check + emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); + emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); + emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); + + emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); + emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); + emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); + + emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime()); + emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime()); + emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime()); + + emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime()); + emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime()); + emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime()); + + emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime()); + emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); +} + /** * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) @@ -2909,31 +3011,32 @@ void UAS::enableHilXPlane(bool enable) */ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float xacc, float yacc, float zacc) + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) { if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { - 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()); - emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); - emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); - - emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); - emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); - emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); + float q[4]; + + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); - emit valueChanged(uasId, "vx sim", "rad", vx*100, getUnixTime()); - emit valueChanged(uasId, "vy sim", "rad", vy*100, getUnixTime()); - emit valueChanged(uasId, "vz sim", "rad", vz*100, getUnixTime()); - } else { - mavlink_message_t msg; - mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, - lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); - sendMessage(msg); - } + mavlink_message_t msg; + mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, q, rollspeed, pitchspeed, yawspeed, + lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); + sendMessage(msg); } else { @@ -2946,17 +3049,16 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa } void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed) + float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) { if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { mavlink_message_t msg; - mavlink_msg_highres_imu_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); sendMessage(msg); - sensorHil = true; lastSendTimeSensors = QGC::groundTimeMilliseconds(); } else @@ -2969,7 +3071,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl } } -void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites) +void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) { // Only send at 10 Hz max rate if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) @@ -2985,8 +3087,8 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi course = (course / M_PI) * 180.0f; mavlink_message_t msg; - mavlink_msg_gps_raw_int_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, course*1e2, satellites); + mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); lastSendTimeGPS = QGC::groundTimeMilliseconds(); sendMessage(msg); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 64a133b77..9823351f2 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -696,11 +696,15 @@ public slots: /** @brief Send the full HIL state to the MAV */ void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, - float vx, float vy, float vz, float xacc, float yacc, float zacc); + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); + + void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, + float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); /** @brief RAW sensors for sensor HIL */ - void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollRotationRate, float pitchRotationRate, float yawRotationRate, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed); + void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, + float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed); /** * @param time_us @@ -714,7 +718,7 @@ public slots: * @param cog course over ground, in radians, -pi..pi * @param satellites */ - void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites); + void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites); /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 83f07f749..1434df03c 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -369,14 +369,14 @@ public slots: /** @brief Send the full HIL state to the MAV */ virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float xacc, float yacc, float zacc) = 0; + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) = 0; /** @brief RAW sensors for sensor HIL */ virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed) = 0; + float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) = 0; /** @brief Send raw GPS for sensor HIL */ - virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites) = 0; + virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) = 0; protected: diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc index 6941af040..292d2671c 100644 --- a/src/ui/QGCHilXPlaneConfiguration.cc +++ b/src/ui/QGCHilXPlaneConfiguration.cc @@ -29,9 +29,9 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QWidget * ui->airframeComboBox->setCurrentIndex(link->getAirFrameIndex()); // XXX not implemented yet ui->airframeComboBox->hide(); - ui->sensorHilCheckBox->setChecked(link->sensorHilEnabled()); - connect(link, SIGNAL(sensorHilChanged(bool)), ui->sensorHilCheckBox, SLOT(setChecked(bool))); - connect(ui->sensorHilCheckBox, SIGNAL(clicked(bool)), link, SLOT(enableSensorHIL(bool))); + ui->sensorHilCheckBox->setChecked(xplane->sensorHilEnabled()); + connect(xplane, SIGNAL(sensorHilChanged(bool)), ui->sensorHilCheckBox, SLOT(setChecked(bool))); + connect(ui->sensorHilCheckBox, SIGNAL(clicked(bool)), xplane, SLOT(enableSensorHIL(bool))); connect(link, SIGNAL(versionChanged(int)), this, SLOT(setVersion(int))); } diff --git a/src/ui/WaypointViewOnlyView.cc b/src/ui/WaypointViewOnlyView.cc index 3ae64851b..5454ee565 100644 --- a/src/ui/WaypointViewOnlyView.cc +++ b/src/ui/WaypointViewOnlyView.cc @@ -77,7 +77,6 @@ void WaypointViewOnlyView::setCurrent(bool state) void WaypointViewOnlyView::updateValues() { - qDebug() << "Trof: WaypointViewOnlyView::updateValues() ID:" << wp->getId(); // Check if we just lost the wp, delete the widget // accordingly if (!wp) diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index d52994aae..627b47d7a 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -595,7 +595,7 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint) */ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) { - qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED"; + //qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED"; // Source of the event was in this widget, do nothing if (firingWaypointChange == wp) { return; -- 2.22.0