mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
314 lines
9.4 KiB
C++
314 lines
9.4 KiB
C++
/**
|
|
******************************************************************************
|
|
*
|
|
* @file aerosimrc.cpp
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
|
|
* @addtogroup GCSPlugins GCS Plugins
|
|
* @{
|
|
* @addtogroup HITLPlugin HITLv2 Plugin
|
|
* @{
|
|
* @brief The Hardware In The Loop plugin version 2
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "aerosimrcsimulator.h"
|
|
#include <extensionsystem/pluginmanager.h>
|
|
#include <coreplugin/icore.h>
|
|
#include <coreplugin/threadmanager.h>
|
|
|
|
AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings ¶ms)
|
|
: Simulator(params)
|
|
{
|
|
udpCounterASrecv = 0;
|
|
}
|
|
|
|
AeroSimRCSimulator::~AeroSimRCSimulator()
|
|
{}
|
|
|
|
bool AeroSimRCSimulator::setupProcess()
|
|
{
|
|
QMutexLocker locker(&lock);
|
|
|
|
return true;
|
|
}
|
|
|
|
void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
|
|
{
|
|
Q_UNUSED(outPort)
|
|
if (inSocket->bind(QHostAddress(host), inPort)) {
|
|
emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
|
|
} else {
|
|
emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
|
|
}
|
|
}
|
|
|
|
void AeroSimRCSimulator::transmitUpdate()
|
|
{
|
|
// read actuator output
|
|
ActuatorCommand::DataFields actCmdData;
|
|
|
|
actCmdData = actCommand->getData();
|
|
float channels[10];
|
|
for (int i = 0; i < 10; ++i) {
|
|
qint16 ch = actCmdData.Channel[i];
|
|
float out = -1.0;
|
|
if (ch >= 1000 && ch <= 2000) {
|
|
ch -= 1000;
|
|
out = ((float)ch / 500.0) - 1.0;
|
|
}
|
|
channels[i] = out;
|
|
}
|
|
|
|
ActuatorDesired::DataFields actData;
|
|
FlightStatus::DataFields flightStatusData = flightStatus->getData();
|
|
ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
|
|
float roll = -1;
|
|
float pitch = -1;
|
|
float yaw = -1;
|
|
float throttle = -1;
|
|
|
|
if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
|
|
// Read joystick input
|
|
if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
|
|
// Note: Pitch sign is reversed in FG ?
|
|
roll = manCtrlData.Roll;
|
|
pitch = -manCtrlData.Pitch;
|
|
yaw = manCtrlData.Yaw;
|
|
throttle = manCtrlData.Throttle;
|
|
}
|
|
} else {
|
|
// Read ActuatorDesired from autopilot
|
|
actData = actDesired->getData();
|
|
|
|
roll = actData.Roll;
|
|
pitch = -actData.Pitch;
|
|
yaw = actData.Yaw;
|
|
throttle = (actData.Thrust * 2.0) - 1.0;
|
|
}
|
|
channels[0] = roll;
|
|
channels[1] = pitch;
|
|
if (throttle < -1) {
|
|
throttle = -1;
|
|
}
|
|
channels[2] = throttle;
|
|
channels[3] = yaw;
|
|
|
|
// read flight status
|
|
quint8 armed;
|
|
quint8 mode;
|
|
armed = flightStatusData.Armed;
|
|
mode = flightStatusData.FlightMode;
|
|
|
|
QByteArray data;
|
|
// 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32)
|
|
data.resize(50);
|
|
QDataStream stream(&data, QIODevice::WriteOnly);
|
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
|
stream << quint32(0x52434D44); // magic header, "RCMD"
|
|
for (int i = 0; i < 10; ++i) {
|
|
stream << channels[i]; // channels
|
|
}
|
|
stream << armed << mode; // flight status
|
|
stream << udpCounterASrecv; // packet counter
|
|
|
|
if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
|
|
qDebug() << "write failed: " << outSocket->errorString();
|
|
}
|
|
|
|
#ifdef DBG_TIMERS
|
|
static int cntTX = 0;
|
|
if (cntTX >= 100) {
|
|
qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart();
|
|
cntTX = 0;
|
|
} else {
|
|
++cntTX;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
|
{
|
|
// check size
|
|
if (data.size() > 192) {
|
|
qDebug() << "!!! big datagram: " << data.size();
|
|
return;
|
|
}
|
|
|
|
QByteArray buf = data;
|
|
QDataStream stream(&buf, QIODevice::ReadOnly);
|
|
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
|
|
|
// check magic header
|
|
quint32 magic;
|
|
stream >> magic;
|
|
if (magic != 0x4153494D) { // "AERO"
|
|
qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D);
|
|
return;
|
|
}
|
|
|
|
#define AEROSIM_RCCHANNEL_NUMELEM 8
|
|
float delT,
|
|
homeX, homeY, homeZ,
|
|
WpHX, WpHY, WpLat, WpLon,
|
|
posX, posY, posZ, // world
|
|
velX, velY, velZ, // world
|
|
angX, angY, angZ, // model
|
|
accX, accY, accZ; // model
|
|
qreal lat, lon;
|
|
float agl, // world
|
|
yaw, pitch, roll, // model
|
|
volt, curr, cons,
|
|
rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
|
|
ch[AEROSIM_RCCHANNEL_NUMELEM];
|
|
|
|
stream >> delT;
|
|
stream >> homeX >> homeY >> homeZ;
|
|
stream >> WpHX >> WpHY >> WpLat >> WpLon;
|
|
stream >> posX >> posY >> posZ;
|
|
stream >> velX >> velY >> velZ;
|
|
stream >> angX >> angY >> angZ;
|
|
stream >> accX >> accY >> accZ;
|
|
stream >> lat >> lon >> agl;
|
|
stream >> yaw >> pitch >> roll;
|
|
stream >> volt >> curr >> cons;
|
|
stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
|
|
stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
|
|
stream >> udpCounterASrecv;
|
|
|
|
Output2Hardware out;
|
|
memset(&out, 0, sizeof(Output2Hardware));
|
|
|
|
|
|
out.delT = delT;
|
|
|
|
/*************************************************************************************/
|
|
for (int i = 0; i < AEROSIM_RCCHANNEL_NUMELEM; i++) {
|
|
out.rc_channel[i] = ch[i]; // Elements in rc_channel are between -1 and 1
|
|
}
|
|
|
|
/**********************************************************************************************/
|
|
QMatrix4x4 mat;
|
|
mat = QMatrix4x4(fy, fx, -fz, 0.0, // model matrix
|
|
ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z)
|
|
-uy, -ux, uz, 0.0,
|
|
0.0, 0.0, 0.0, 1.0);
|
|
mat.optimize();
|
|
|
|
QQuaternion quat; // model quat
|
|
asMatrix2Quat(mat, quat);
|
|
|
|
/*************************************************************************************/
|
|
// rotate gravity
|
|
QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z)
|
|
QVector3D gee = QVector3D(0.0, 0.0, -GEE);
|
|
QQuaternion qWorld = quat.conjugate();
|
|
gee = qWorld.rotatedVector(gee);
|
|
acc += gee;
|
|
|
|
out.rollRate = angY * RAD2DEG; // gyros (X,Y,Z) -> (+Y,+X,-Z)
|
|
out.pitchRate = angX * RAD2DEG;
|
|
out.yawRate = angZ * -RAD2DEG;
|
|
|
|
out.accX = acc.x();
|
|
out.accY = acc.y();
|
|
out.accZ = acc.z();
|
|
|
|
/*************************************************************************************/
|
|
QVector3D rpy; // model roll, pitch, yaw
|
|
asMatrix2RPY(mat, rpy);
|
|
|
|
out.roll = rpy.x();
|
|
out.pitch = rpy.y();
|
|
out.heading = rpy.z();
|
|
|
|
|
|
/**********************************************************************************************/
|
|
out.altitude = posZ;
|
|
out.agl = posZ;
|
|
out.heading = yaw * RAD2DEG;
|
|
out.latitude = lat * 10e6;
|
|
out.longitude = lon * 10e6;
|
|
out.groundspeed = qSqrt(velX * velX + velY * velY);
|
|
|
|
/**********************************************************************************************/
|
|
out.dstN = posY;
|
|
out.dstE = posX;
|
|
out.dstD = -posZ;
|
|
|
|
out.velNorth = velY;
|
|
out.velEast = velX;
|
|
out.velDown = -velZ;
|
|
|
|
out.voltage = volt;
|
|
out.current = curr;
|
|
out.consumption = cons * 1000.0;
|
|
|
|
updateUAVOs(out);
|
|
|
|
#ifdef DBG_TIMERS
|
|
static int cntRX = 0;
|
|
if (cntRX >= 100) {
|
|
qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart();
|
|
cntRX = 0;
|
|
} else {
|
|
++cntRX;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// transfomations
|
|
|
|
void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
|
|
{
|
|
qreal w, x, y, z;
|
|
|
|
// w always >= 0
|
|
w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
|
|
x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
|
|
y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
|
|
z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;
|
|
|
|
x = copysign(x, (m(1, 2) - m(2, 1)));
|
|
y = copysign(y, (m(2, 0) - m(0, 2)));
|
|
z = copysign(z, (m(0, 1) - m(1, 0)));
|
|
|
|
q.setScalar(w);
|
|
q.setX(x);
|
|
q.setY(y);
|
|
q.setZ(z);
|
|
}
|
|
|
|
void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
|
|
{
|
|
qreal roll, pitch, yaw;
|
|
|
|
if (qFabs(m(0, 2)) > 0.998) {
|
|
// ~86.3°, gimbal lock
|
|
roll = 0.0;
|
|
pitch = copysign(M_PI_2, -m(0, 2));
|
|
yaw = qAtan2(-m(1, 0), m(1, 1));
|
|
} else {
|
|
roll = qAtan2(m(1, 2), m(2, 2));
|
|
pitch = qAsin(-m(0, 2));
|
|
yaw = qAtan2(m(0, 1), m(0, 0));
|
|
}
|
|
|
|
rpy.setX(roll * RAD2DEG);
|
|
rpy.setY(pitch * RAD2DEG);
|
|
rpy.setZ(yaw * RAD2DEG);
|
|
}
|