From 48fe4cecc31d916a3f332057f5d95ea945d1abea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Apr 2013 10:53:06 +0200 Subject: [PATCH] Cleanup on state HIL --- src/comm/QGCFlightGearLink.cc | 22 +++++++++++----------- src/comm/QGCHilLink.h | 6 +++--- src/comm/QGCJSBSimLink.cc | 4 ++-- src/comm/QGCXPlaneLink.cc | 4 ++-- src/uas/UAS.cc | 10 +++++++--- src/uas/UAS.h | 6 +++--- src/uas/UASInterface.h | 6 +++--- 7 files changed, 31 insertions(+), 27 deletions(-) diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index a493c2612..2c1ec4b49 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -244,9 +244,9 @@ void QGCFlightGearLink::readBytes() double lat, lon, alt; double vx, vy, vz, xacc, yacc, zacc; - lat = values.at(1).toDouble() * 1e7; - lon = values.at(2).toDouble() * 1e7; - alt = values.at(3).toDouble() * 1e3; + lat = values.at(1).toDouble(); + lon = values.at(2).toDouble(); + alt = values.at(3).toDouble(); roll = values.at(4).toDouble(); pitch = values.at(5).toDouble(); yaw = values.at(6).toDouble(); @@ -254,13 +254,13 @@ void QGCFlightGearLink::readBytes() pitchspeed = values.at(8).toDouble(); yawspeed = values.at(9).toDouble(); - xacc = values.at(10).toDouble()*1e3/9.8; // convert to mg's - yacc = values.at(11).toDouble()*1e3/9.8; - zacc = values.at(12).toDouble()*1e3/9.8; + xacc = values.at(10).toDouble(); + yacc = values.at(11).toDouble(); + zacc = values.at(12).toDouble(); - vx = values.at(13).toDouble() * 1e2; - vy = values.at(14).toDouble() * 1e2; - vz = values.at(15).toDouble() * 1e2; + vx = values.at(13).toDouble(); + vy = values.at(14).toDouble(); + vz = values.at(15).toDouble(); // Send updated state emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, @@ -299,7 +299,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(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(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))); if (process) { @@ -346,7 +346,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(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(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))); UAS* uas = dynamic_cast(mav); diff --git a/src/comm/QGCHilLink.h b/src/comm/QGCHilLink.h index b74bac37f..660a3c4ce 100644 --- a/src/comm/QGCHilLink.h +++ b/src/comm/QGCHilLink.h @@ -89,9 +89,9 @@ signals: void simulationConnected(bool connected); /** @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 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); void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites); diff --git a/src/comm/QGCJSBSimLink.cc b/src/comm/QGCJSBSimLink.cc index a1d9b6014..a0dcf1131 100644 --- a/src/comm/QGCJSBSimLink.cc +++ b/src/comm/QGCJSBSimLink.cc @@ -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(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(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))); 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(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(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))); UAS* uas = dynamic_cast(mav); diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 970398155..c04de3fa1 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -694,7 +694,7 @@ 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(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))); @@ -878,7 +878,7 @@ 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(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))); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index cf0575dc0..666109a82 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2728,8 +2728,8 @@ void UAS::enableHilXPlane(bool enable) * @param zacc Z acceleration (mg) */ void UAS::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) + float pitchspeed, float yawspeed, double lat, double lon, double alt, + float vx, float vy, float vz, float xacc, float yacc, float zacc) { if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { @@ -2743,11 +2743,15 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo 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, "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, yacc*1000, zacc*1000); + 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); } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 4bc794892..d77f2b5dd 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -541,9 +541,9 @@ public slots: 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, - 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 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); /** @brief RAW sensors for sensor HIL */ void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 4ff70077a..8105060fc 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -329,9 +329,9 @@ public slots: 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; + 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; /** @brief RAW sensors for sensor HIL */ virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, -- 2.22.0