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
Expand all 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
This diff is collapsed.
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