Skip to content

Q
qgroundcontrol
Commit 0ab36f36 authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Working on sensor HIL

parent d9360a15
Hide whitespace changes
Inline Side-by-side
Showing with 139 additions and 12 deletions
src/comm/QGCHilLink.h
View file @ 0ab36f36
......@@ -80,10 +80,19 @@ signals:
**/
void simulationConnected(bool connected);
/** @brief State update from FlightGear */
/** @brief State update from simulation */
void hilStateChanged(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t 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 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);
/** @brief Remote host and port changed */
void remoteChanged(const QString& hostPort);
......
src/comm/QGCXPlaneLink.cc
View file @ 0ab36f36
......@@ -53,7 +53,8 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
xPlaneVersion(0),
simUpdateLast(QGC::groundTimeMilliseconds()),
simUpdateLastText(QGC::groundTimeMilliseconds()),
simUpdateHz(0)
simUpdateHz(0),
sensorHilEnabled(true)
{
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
......@@ -390,6 +391,7 @@ void QGCXPlaneLink::readBytes()
{
// Only emit updates on attitude message
bool emitUpdate = false;
quint16 fields_changed = 0;
const qint64 maxLength = 1000;
char data[maxLength];
......@@ -440,6 +442,12 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
}
else if (p.index == 6 && xPlaneVersion == 10)
{
// inHg to kPa
abs_pressure = p.f[0] * 3.3863886666718317f;
temperature = p.f[1];
}
// Forward controls from X-Plane to MAV, not very useful
// better: Connect Joystick to QGroundControl
// else if (p.index == 8)
......@@ -457,6 +465,7 @@ void QGCXPlaneLink::readBytes()
rollspeed = p.f[2];
pitchspeed = p.f[1];
yawspeed = p.f[0];
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
}
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
{
......@@ -471,6 +480,20 @@ void QGCXPlaneLink::readBytes()
if (yaw < -M_PI) {
yaw += 2.0 * M_PI;
}
float yawmag = p.f[3];
if (yawmag > M_PI) {
yawmag -= 2.0 * M_PI;
}
if (yawmag < -M_PI) {
yawmag += 2.0 * M_PI;
}
xmag = cos(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
ymag = sin(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
zmag = 0.0f + 0.0f + 1.0f * 0.4f;
emitUpdate = true;
}
else if ((xPlaneVersion == 9 && p.index == 17))
......@@ -478,6 +501,7 @@ void QGCXPlaneLink::readBytes()
rollspeed = p.f[2];
pitchspeed = p.f[1];
yawspeed = p.f[0];
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
}
// else if (p.index == 19)
......@@ -491,6 +515,12 @@ void QGCXPlaneLink::readBytes()
lon = p.f[1];
alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
}
else if (p.index == 21 && xPlaneVersion == 10)
{
vx = p.f[3];
vy = p.f[4];
vz = p.f[5];
}
else if (p.index == 12)
{
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
......@@ -545,9 +575,27 @@ void QGCXPlaneLink::readBytes()
}
simUpdateLast = QGC::groundTimeMilliseconds();
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
if (sensorHilEnabled)
{
diff_pressure = 0.0f;
pressure_alt = alt;
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 = yaw;
int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
} else {
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
}
}
if (!oldConnectionState && xPlaneConnected)
......@@ -594,7 +642,11 @@ 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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, 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)));
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
{
......@@ -774,7 +826,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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, 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)));
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
......
src/comm/QGCXPlaneLink.h
View file @ 0ab36f36
......@@ -184,6 +184,7 @@ protected:
float vx, vy, vz, xacc, yacc, zacc;
float airspeed;
float groundspeed;
float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature;
float man_roll, man_pitch, man_yaw;
QString airframeName;
......@@ -193,6 +194,7 @@ protected:
quint64 simUpdateLast;
quint64 simUpdateLastText;
float simUpdateHz;
bool sensorHilEnabled;
void setName(QString name);
......
src/uas/UAS.cc
View file @ 0ab36f36
......@@ -2706,6 +2706,48 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
}
}
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)
{
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
{
mavlink_message_t msg;
mavlink_msg_highres_imu_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);
}
else
{
// Attempt to set HIL mode
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
sendMessage(msg);
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 cog, int satellites)
{
if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
{
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, cog*1e2, satellites);
sendMessage(msg);
}
else
{
// Attempt to set HIL mode
mavlink_message_t msg;
mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
}
/**
* Connect flight gear link.
**/
......
src/uas/UAS.h
View file @ 0ab36f36
......@@ -534,13 +534,19 @@ public slots:
void enableHilFlightGear(bool enable, QString options);
void enableHilXPlane(bool enable);
/** @brief Send the full HIL state to the MAV */
void sendHilState( uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
void sendHilState(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc);
/** @brief RAW sensors for sensor HIL */
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);
/** @brief Send raw GPS for sensor HIL */
void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);
/** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
void startHil();
......
src/uas/UASInterface.h
View file @ 0ab36f36
......@@ -328,6 +328,19 @@ public slots:
/** @brief Get the current battery type and specs */
virtual QString getBatterySpecs() = 0;
/** @brief Send the full HIL state to the MAV */
virtual void sendHilState(uint64_t time_us, float roll, float pitch, float yaw, float rollspeed,
float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt,
int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t 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;
/** @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;
protected:
QColor color;
......
src/ui/QGCVehicleConfig.cc
View file @ 0ab36f36
......@@ -475,7 +475,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (minTpl.exactMatch(parameterName)) {
bool ok;
unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
int index = parameterName.mid(2, 1).toInt(&ok) - 1;
//qDebug() << "PARAM:" << parameterName << "index:" << index;
if (ok && (index >= 0) && (index < chanMax))
{
......@@ -485,7 +485,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (maxTpl.exactMatch(parameterName)) {
bool ok;
unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
int index = parameterName.mid(2, 1).toInt(&ok) - 1;
if (ok && (index >= 0) && (index < chanMax))
{
rcMax[index] = value.toInt();
......@@ -494,7 +494,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (trimTpl.exactMatch(parameterName)) {
bool ok;
unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
int index = parameterName.mid(2, 1).toInt(&ok) - 1;
if (ok && (index >= 0) && (index < chanMax))
{
rcTrim[index] = value.toInt();
......@@ -503,7 +503,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
if (revTpl.exactMatch(parameterName)) {
bool ok;
unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1;
int index = parameterName.mid(2, 1).toInt(&ok) - 1;
if (ok && (index >= 0) && (index < chanMax))
{
rcRev[index] = (value.toInt() == -1) ? true : false;
......
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