Skip to content

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

Updated MAVLink, fixed up HIL

parent 478b476a
Hide whitespace changes
Inline Side-by-side
Showing with 250 additions and 89 deletions
src/comm/MAVLinkSimulationMAV.cc
View file @ f9318458
......@@ -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;
......
src/comm/QGCFlightGearLink.cc
View file @ f9318458
......@@ -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<UAS*>(mav);
......
src/comm/QGCHilLink.h
View file @ f9318458
......@@ -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);
......
src/comm/QGCJSBSimLink.cc
View file @ f9318458
......@@ -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<UAS*>(mav);
......
src/comm/QGCXPlaneLink.cc
View file @ f9318458
......@@ -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<UAS*>(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<UAS*>(mav);
if (uas)
......
src/comm/QGCXPlaneLink.h
View file @ f9318458
......@@ -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;
......
src/uas/UAS.cc
View file @ f9318458
......@@ -26,6 +26,7 @@
#include "QGCMAVLink.h"
#include "LinkManager.h"
#include "SerialLink.h"
#include <Eigen/Geometry>
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
......@@ -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);
}
......
src/uas/UAS.h
View file @ f9318458
......@@ -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 **/
......
src/uas/UASInterface.h
View file @ f9318458
......@@ -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:
......
src/ui/QGCHilXPlaneConfiguration.cc
View file @ f9318458
......@@ -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)));
}
......
src/ui/WaypointViewOnlyView.cc
View file @ f9318458
......@@ -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)
......
src/ui/map/QGCMapWidget.cc
View file @ f9318458
......@@ -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;
......
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