Merge pull request #858 from mavlink/distancehil

Flightgear HIL: simulate a range finder

files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml

@@ -6,7 +6,7 @@
   <output>
    <line_separator>newline</line_separator>
    <var_separator>tab</var_separator>
-   
+
    <chunk>
     <name>time (sec)</name>
     <type>float</type>
@@ -46,7 +46,7 @@
     <node>/orientation/roll-deg</node>
     <factor>0.01745329251994329576</factor>  <!-- degrees to radians -->
    </chunk>
-   
+
    <chunk>
     <name>pitch angle (rad)</name>
     <type>float</type>
@@ -54,7 +54,7 @@
     <node>/orientation/pitch-deg</node>
     <factor>0.01745329251994329576</factor>  <!-- degrees to radians -->
    </chunk>
-   
+
    <chunk>
     <name>yaw angle</name>
     <type>float</type>
@@ -109,7 +109,7 @@
    </chunk>
 
    <!-- Velocities -->
-   
+
    <chunk>
     <name>Velocity North ("vn" mps)</name>
     <type>float</type>
@@ -176,7 +176,16 @@
     <node>/environment/pressure-inhg</node>
     <factor>33.86389</factor>                  <!-- inhg to hpa -->
    </chunk>
-   
+
+   <!-- Altitude AGL -->
+   <chunk>
+    <name>Altitude AGL (m)</name>
+    <type>float</type>
+    <format>%.5f</format>
+    <node>/position/altitude-agl-ft</node>
+    <factor>0.3048</factor>  <!-- feet to meter -->
+   </chunk>
+
   </output>
 
   <input>
@@ -195,13 +204,13 @@
     <type>float</type>
     <node>/controls/flight/elevator</node>
    </chunk>
-   
+
    <chunk>
      <name>rudder</name>
      <type>float</type>
      <node>/controls/flight/rudder</node>
    </chunk>
-   
+
    <chunk>
     <name>running</name>
     <type>bool</type>
@@ -213,7 +222,7 @@
     <type>float</type>
     <node>/controls/engines/engine/throttle</node>
    </chunk>
-   
+
 
  </input>
 

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,16 @@ 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 = -1.0; // -1 means invalid value
+        if (fabsf(roll) < 0.87 && fabsf(pitch) < 0.87) // return a valid value only for decent angles
+        {
+            distanceMeasurement = fabsf((float)(1.0/cosPhi * 1.0/cosThe * alt_agl)); // assuming 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 +483,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)
     {

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);

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; }

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

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<QAction*> actions; ///< A list of actions that this UAS can perform.
 
 

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;