1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-26 11:52:10 +01:00
2013-12-11 22:22:28 +01:00

405 lines
9.2 KiB
C++

/**
******************************************************************************
*
* @file simulator.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @{
* @brief The Hardware In The Loop plugin
*****************************************************************************/
/*
* 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
*/
#ifndef ISIMULATOR_H
#define ISIMULATOR_H
#include "qscopedpointer.h"
#include "uavobjectmanager.h"
#include "accelstate.h"
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "barosensor.h"
#include "flightbatterystate.h"
#include "flightstatus.h"
#include "gcsreceiver.h"
#include "gcstelemetrystats.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "groundtruth.h"
#include "gyrostate.h"
#include "homelocation.h"
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "sonaraltitude.h"
#include "velocitystate.h"
#include "utils/coordinateconversions.h"
#include <QObject>
#include <QUdpSocket>
#include <QTime>
#include <QTimer>
#include <QProcess>
#include <qmath.h>
/**
* just imagine this was a class without methods and all public properties
*/
typedef struct _FLIGHT_PARAM {
// time
float T;
float dT;
unsigned int i;
// speeds
float cas; // Calibrated airspeed
float tas; // True airspeed
float groundspeed;
// position (absolute)
float X;
float Y;
float Z;
// speed (absolute)
float dX;
float dY;
float dZ;
// acceleration (absolute)
float ddX;
float ddY;
float ddZ;
// angle
float azimuth;
float pitch;
float roll;
// rotation speed
float dAzimuth;
float dPitch;
float dRoll;
} FLIGHT_PARAM;
struct AirParameters {
float groundDensity;
float groundTemp;
float seaLevelPress;
float tempLapseRate;
float univGasConstant;
float dryAirConstant;
float relativeHumidity; // [%]
float M; // Molar mass
};
typedef struct _CONNECTION {
QString simulatorId;
QString binPath;
QString dataPath;
QString hostAddress;
QString remoteAddress;
int outPort;
int inPort;
bool startSim;
bool addNoise;
QString latitude;
QString longitude;
// bool homeLocation;
bool attRawEnabled;
quint8 attRawRate;
bool attStateEnabled;
bool attActHW;
bool attActSim;
bool attActCalc;
bool baroSensorEnabled;
quint16 baroAltRate;
bool groundTruthEnabled;
quint16 groundTruthRate;
bool gpsPositionEnabled;
quint16 gpsPosRate;
bool inputCommand;
bool gcsReceiverEnabled;
bool manualControlEnabled;
quint16 minOutputPeriod;
bool airspeedStateEnabled;
quint16 airspeedStateRate;
} SimulatorSettings;
struct Output2Hardware {
float latitude;
float longitude;
float altitude;
float agl; // [m]
float heading;
float groundspeed; // [m/s]
float calibratedAirspeed; // [m/s]
float trueAirspeed; // [m/s]
float angleOfAttack;
float angleOfSlip;
float roll;
float pitch;
float pressure;
float temperature;
float velNorth; // [m/s]
float velEast; // [m/s]
float velDown; // [m/s]
float dstN; // [m]
float dstE; // [m]
float dstD; // [m]
float accX; // [m/s^2]
float accY; // [m/s^2]
float accZ; // [m/s^2]
float rollRate; // [deg/s]
float pitchRate; // [deg/s]
float yawRate; // [deg/s]
float delT; // [s]
float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; // Elements in rc_channel are between -1 and 1
float rollDesired;
float pitchDesired;
float yawDesired;
float throttleDesired;
float voltage;
float current;
float consumption;
};
// struct Output2Simulator{
// float roll;
// float pitch;
// float yaw;
// float throttle;
// float ailerons;
// float rudder;
// float elevator;
// float motor;
// };
class Simulator : public QObject {
Q_OBJECT
public:
Simulator(const SimulatorSettings & params);
virtual ~Simulator();
bool isAutopilotConnected() const
{
return autopilotConnectionStatus;
}
bool isSimulatorConnected() const
{
return simConnectionStatus;
}
QString Name() const
{
return name;
}
void setName(QString str)
{
name = str;
}
QString SimulatorId() const
{
return simulatorId;
}
void setSimulatorId(QString str)
{
simulatorId = str;
}
float airDensityFromAltitude(float alt, AirParameters air, float gravity);
float airPressureFromAltitude(float alt, AirParameters air, float gravity);
float cas2tas(float CAS, float alt, AirParameters air, float gravity);
float tas2cas(float TAS, float alt, AirParameters air, float gravity);
static bool IsStarted()
{
return isStarted;
}
static void setStarted(bool val)
{
isStarted = val;
}
static QStringList & Instances()
{
return Simulator::instances;
}
static void setInstance(const QString & str)
{
Simulator::instances.append(str);
}
virtual void stopProcess() {}
virtual void setupUdpPorts(const QString & host, int inPort, int outPort)
{
Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)
}
void resetInitialHomePosition();
void updateUAVOs(Output2Hardware out);
AirParameters getAirParameters();
void setAirParameters(AirParameters airParameters);
signals:
void autopilotConnected();
void autopilotDisconnected();
void simulatorConnected();
void simulatorDisconnected();
void processOutput(QString str);
void deleteSimProcess();
void myStart();
public slots:
Q_INVOKABLE virtual bool setupProcess()
{
return true;
}
private slots:
void onStart();
// void transmitUpdate();
void receiveUpdate();
void onAutopilotConnect();
void onAutopilotDisconnect();
void onSimulatorConnectionTimeout();
void telStatsUpdated(UAVObject *obj);
Q_INVOKABLE void onDeleteSimulator(void);
virtual void transmitUpdate() = 0;
virtual void processUpdate(const QByteArray & data) = 0;
protected:
static const float GEE;
static const float FT2M;
static const float KT2MPS;
static const float INHG2KPA;
static const float FPS2CMPS;
static const float DEG2RAD;
static const float RAD2DEG;
QProcess *simProcess;
QTime *time;
QUdpSocket *inSocket; // (new QUdpSocket());
QUdpSocket *outSocket;
ActuatorCommand *actCommand;
ActuatorDesired *actDesired;
ManualControlCommand *manCtrlCommand;
FlightStatus *flightStatus;
FlightBatteryState *flightBatt;
BaroSensor *baroAlt;
AirspeedState *airspeedState;
AttitudeState *attState;
AttitudeSettings *attSettings;
VelocityState *velState;
GPSPositionSensor *gpsPos;
GPSVelocitySensor *gpsVel;
PositionState *posState;
HomeLocation *posHome;
AccelState *accelState;
GyroState *gyroState;
GCSTelemetryStats *telStats;
GCSReceiver *gcsReceiver;
GroundTruth *groundTruth;
SimulatorSettings settings;
FLIGHT_PARAM current;
FLIGHT_PARAM old;
QMutex lock;
private:
bool once;
float initN;
float initE;
float initD;
int updatePeriod;
int simTimeout;
volatile bool autopilotConnectionStatus;
volatile bool simConnectionStatus;
QTimer *txTimer;
QTimer *simTimer;
QTime attRawTime;
QTime gpsPosTime;
QTime groundTruthTime;
QTime baroAltTime;
QTime battTime;
QTime gcsRcvrTime;
QTime airspeedStateTime;
QString name;
QString simulatorId;
volatile static bool isStarted;
static QStringList instances;
// QList<QScopedPointer<UAVDataObject> > requiredUAVObjects;
void setupOutputObject(UAVObject *obj, quint32 updatePeriod);
void setupInputObject(UAVObject *obj, quint32 updatePeriod);
void setupWatchedObject(UAVObject *obj, quint32 updatePeriod);
void setupObjects();
AirParameters airParameters;
};
class SimulatorCreator {
public:
SimulatorCreator(QString id, QString descr) :
classId(id),
description(descr)
{}
virtual ~SimulatorCreator() {}
QString ClassId() const
{
return classId;
}
QString Description() const
{
return description;
}
virtual Simulator *createSimulator(const SimulatorSettings & params) = 0;
private:
QString classId;
QString description;
};
#endif // ISIMULATOR_H