diff --git a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
index 0ecbf20d5dde9d6a742eef9a2d37f9bf318d8fc5..5ecb9966bc1e3283c91b3c1d961ba1a57dda120c 100644
--- a/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
+++ b/files/flightgear/Protocol/qgroundcontrol-fixed-wing.xml
@@ -6,7 +6,7 @@
@@ -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 f570adf5ef99fb0472f66900d15db368302c9cad..9bc8a7aa2ac4c1c01117eda863413f35145a295c 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,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)
{
diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h
index 0c2a08f4f518a59a0ffbeac7c677dea9b4d2429a..ed83adafe11ff428249968bba952eceba9cdfe43 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 fc3b0cece36dcc25143448e008ca5c1534cd981b..a153a234b2cde6f3beb76ebefed0c192afdc5694 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 3fd754b098acdd2d9241a4264fb8c0f0faf7f2b0..647f6cde32bef6aa809394791c11a384f4dc190c 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 8124847a2671a0cd2701eced5c507f8d42d2587e..9d9681caa6b3429b3bffb3b02cbf9a7f13ed2477 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 0064229d6bb971fe2975512dda82602415fd9209..e15b98949a491993a48fa600f7e4ec1fb4631415 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;