From b52e4403287d6ffc29250bac2b5c0a09a8a632f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 2 Sep 2014 13:26:06 +0200 Subject: [PATCH] add distance sensor output for hil --- .../Protocol/qgroundcontrol-fixed-wing.xml | 25 +++++++++++++------ src/comm/QGCFlightGearLink.cc | 15 +++++++++-- src/comm/QGCHilLink.h | 3 +++ src/qgcunittest/MockUAS.h | 3 +++ src/uas/UAS.cc | 23 ++++++++++++++++- src/uas/UAS.h | 7 +++++- src/uas/UASInterface.h | 4 +++ 7 files changed, 68 insertions(+), 12 deletions(-) diff --git a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml index 0ecbf20d5..5ecb9966b 100644 --- a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml +++ b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml @@ -6,7 +6,7 @@ newline tab - + time (sec) float @@ -46,7 +46,7 @@ /orientation/roll-deg 0.01745329251994329576 - + pitch angle (rad) float @@ -54,7 +54,7 @@ /orientation/pitch-deg 0.01745329251994329576 - + yaw angle float @@ -109,7 +109,7 @@ - + Velocity North ("vn" mps) float @@ -176,7 +176,16 @@ /environment/pressure-inhg 33.86389 - + + + + Altitude AGL (m) + float + %.5f + /position/altitude-agl-ft + 0.3048 + + @@ -195,13 +204,13 @@ float /controls/flight/elevator - + rudder float /controls/flight/rudder - + running bool @@ -213,7 +222,7 @@ float /controls/engines/engine/throttle - + diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index f570adf5e..692dddc31 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -99,6 +99,8 @@ void QGCFlightGearLink::run() 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))); + connect(this, SIGNAL(sensorHilOpticalFlowChanged(quint64, qint16, qint16, float,float, quint8, float)), + mav, SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); // Start a new QProcess to run FlightGear in _fgProcess = new QProcess(this); @@ -298,7 +300,7 @@ void QGCFlightGearLink::readBytes() QStringList values = state.split("\t"); // Check length - const int nValues = 21; + const int nValues = 22; if (values.size() != nValues) { qDebug() << "RETURN LENGTH MISMATCHING EXPECTED" << nValues << "BUT GOT" << values.size(); @@ -316,6 +318,7 @@ void QGCFlightGearLink::readBytes() float temperature; float abs_pressure; float mag_variation, mag_dip, xmag_ned, ymag_ned, zmag_ned, xmag_body, ymag_body, zmag_body; + float alt_agl; lat = values.at(1).toDouble(); @@ -345,6 +348,8 @@ void QGCFlightGearLink::readBytes() abs_pressure = values.at(20).toFloat() * 1e2f; //convert to Pa from hPa abs_pressure += barometerOffsetkPa * 1e3f; //add offset, convert from kPa to Pa + alt_agl = values.at(21).toFloat(); + //calculate differential pressure const float air_gas_constant = 287.1f; // J/(kg * K) const float absolute_null_celsius = -273.15f; // °C @@ -424,8 +429,12 @@ void QGCFlightGearLink::readBytes() int satellites = 8; emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); - // qDebug() << "sensorHilGpsChanged " << lat << lon << alt << vel; + + // Send Optical Flow message. For now we set the flow quality to 0 and just write the ground_distance field + float distanceMeasurement = (float)(1.0/cosPhi * 1.0/cosThe * alt_agl); //asuming planar ground + emit sensorHilOpticalFlowChanged(QGC::groundTimeUsecs(), 0, 0, 0.0f, + 0.0f, 0.0f, distanceMeasurement); } else { emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, @@ -470,6 +479,8 @@ bool QGCFlightGearLink::disconnectSimulation() 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))); + disconnect(this, SIGNAL(sensorHilOpticalFlowChanged(quint64, qint16, qint16, float,float, quint8, float)), + mav, SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float))); if (_fgProcess) { diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h index 0c2a08f4f..ed83adafe 100644 --- a/src/comm/QGCHilLink.h +++ b/src/comm/QGCHilLink.h @@ -105,6 +105,9 @@ signals: float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_updated); + + void sensorHilOpticalFlowChanged(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance); /** @brief Remote host and port changed */ void remoteChanged(const QString& hostPort); diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index fc3b0cece..a153a234b 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -166,6 +166,9 @@ public slots: { Q_UNUSED(time_us); Q_UNUSED(xacc); Q_UNUSED(yacc); Q_UNUSED(zacc); Q_UNUSED(rollspeed); Q_UNUSED(pitchspeed); Q_UNUSED(yawspeed); Q_UNUSED(xmag); Q_UNUSED(ymag); Q_UNUSED(zmag); Q_UNUSED(abs_pressure); Q_UNUSED(diff_pressure); Q_UNUSED(pressure_alt); Q_UNUSED(temperature); Q_UNUSED(fields_changed); Q_ASSERT(false); }; 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) { Q_UNUSED(time_us); Q_UNUSED(lat); Q_UNUSED(lon); Q_UNUSED(alt); Q_UNUSED(fix_type); Q_UNUSED(eph); Q_UNUSED(epv); Q_UNUSED(vel); Q_UNUSED(vn); Q_UNUSED(ve); Q_UNUSED(vd); Q_UNUSED(cog); Q_UNUSED(satellites); Q_ASSERT(false); }; + virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance) + { Q_UNUSED(time_us); Q_UNUSED(flow_x); Q_UNUSED(flow_y); Q_UNUSED(flow_comp_m_x); Q_UNUSED(flow_comp_m_y); Q_UNUSED(quality); Q_UNUSED(ground_distance); Q_ASSERT(false);}; virtual bool isRotaryWing() { Q_ASSERT(false); return false; } virtual bool isFixedWing() { Q_ASSERT(false); return false; } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3fd754b09..647f6cde3 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -158,7 +158,8 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(), hilEnabled(false), sensorHil(false), lastSendTimeGPS(0), - lastSendTimeSensors(0) + lastSendTimeSensors(0), + lastSendTimeOpticalFlow(0) { moveToThread(thread); @@ -3147,6 +3148,26 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl } } +void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance) +{ + if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED) + { + mavlink_message_t msg; + mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, 0, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance); + sendMessage(msg); + lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); + } + else + { + // Attempt to set HIL mode + setMode(base_mode | MAV_MODE_FLAG_HIL_ENABLED, custom_mode); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } + +} + 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 diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 8124847a2..9d9681caa 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -813,6 +813,10 @@ public slots: 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); + /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ + void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance); + /** * @param time_us * @param lat @@ -1020,7 +1024,8 @@ 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; + quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent + quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent QList actions; ///< A list of actions that this UAS can perform. diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0064229d6..e15b98949 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -390,6 +390,10 @@ public slots: /** @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 vn, float ve, float vd, float cog, int satellites) = 0; + /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ + virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance) = 0; + protected: QColor color; -- 2.22.0