zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Cleanup on state HIL

QGC4.4
Lorenz Meier 12 years ago
parent
d4db727b38
commit
48fe4cecc3
7 changed files with 31 additions and 27 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 22
      src/comm/QGCFlightGearLink.cc
  2. 6
      src/comm/QGCHilLink.h
  3. 4
      src/comm/QGCJSBSimLink.cc
  4. 4
      src/comm/QGCXPlaneLink.cc
  5. 10
      src/uas/UAS.cc
  6. 6
      src/uas/UAS.h
  7. 6
      src/uas/UASInterface.h

22
src/comm/QGCFlightGearLink.cc
Unescape View File

@ -244,9 +244,9 @@ void QGCFlightGearLink::readBytes() @@ -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() @@ -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() @@ -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() @@ -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<UAS*>(mav);

6
src/comm/QGCHilLink.h
Unescape View File

@ -89,9 +89,9 @@ signals: @@ -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);

4
src/comm/QGCJSBSimLink.cc
Unescape View File

@ -271,7 +271,7 @@ bool QGCJSBSimLink::disconnectSimulation() @@ -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() @@ -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<UAS*>(mav);

4
src/comm/QGCXPlaneLink.cc
Unescape View File

@ -694,7 +694,7 @@ bool QGCXPlaneLink::disconnectSimulation() @@ -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() @@ -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)));

10
src/uas/UAS.cc
Unescape View File

@ -2728,8 +2728,8 @@ void UAS::enableHilXPlane(bool enable) @@ -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 @@ -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);
}
}

6
src/uas/UAS.h
Unescape View File

@ -541,9 +541,9 @@ public slots: @@ -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,

6
src/uas/UASInterface.h
Unescape View File

@ -329,9 +329,9 @@ public slots: @@ -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,

Write Preview
Loading…
Cancel
Save