1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-13 20:48:42 +01:00
LibrePilot/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp

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 &params)
: 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);
}