flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-04-27 11:05:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed interpolator (preliminary) to work with PQs and new classes, added stubs for unit tests in BlackCore

Browse Source
This commit is contained in:
Klaus Basan
2013-04-27 02:09:42 +02:00
parent 5eac9be7d5
commit c5b9c48cd6
31 changed files with 645 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/blackcore/blackcore.pro
View File

@@ -1,3 +1,4 @@
# GUI is required for the matrix classes
QT += network
TARGET = blackcore
@@ -5,25 +6,21 @@ TEMPLATE = lib
CONFIG += staticlib
INCLUDEPATH += ..
DEPENDPATH += . ..
linux-g++* {
QMAKE_CXXFLAGS += -std=c++0x
QMAKE_CXXFLAGS += -std=c++0x
}
#PRECOMPILED_HEADER = stdpch.h
precompile_header:!isEmpty(PRECOMPILED_HEADER) {
DEFINES += USING_PCH
}
DEFINES += USING_PCH
}
DEFINES += LOG_IN_FILE
HEADERS += *.h
SOURCES += *.cpp
DESTDIR = ../../lib

192
src/blackcore/interpolator.cpp
View File

@@ -1,155 +1,149 @@
#include <iostream>
#include "blackcore/matrix_3d.h"
#include "blackcore/vector_geo.h"
#include "blackcore/mathematics.h"
/* Copyright (C) 2013 VATSIM Community / contributors
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#include "blackcore/interpolator.h"
#include "blackcore/constants.h"
#include <iostream>
using namespace BlackMisc::Geo;
using namespace BlackMisc::Math;
using namespace BlackMisc::PhysicalQuantities;
using namespace BlackMisc::Aviation;
namespace BlackCore
{
CInterpolator::CInterpolator()
: m_state_begin(NULL), m_state_end(NULL)
/*
* Constructor
*/
CInterpolator::CInterpolator() : m_state_begin(nullptr), m_state_end(nullptr)
{
m_time.start();
}
/*
* Virtual destructor
*/
CInterpolator::~CInterpolator()
{
delete m_state_begin;
delete m_state_end;
delete m_state_begin;
delete m_state_end;
}
void CInterpolator::initialize()
/*
* Initialize
*/
void CInterpolator::initialize() {}
/*
* Push an update
*/
CCoordinateNed CInterpolator::pushUpdate(const CCoordinateGeodetic &pos, const CSpeed &groundSpeed, const CHeading &heading, const CAngle &pitch, const CAngle &bank)
{
}
void CInterpolator::pushUpdate(CVectorGeo pos, double groundVelocity, double heading, double pitch, double bank)
{
CNed vNED;
if ( m_state_begin == NULL )
CCoordinateNed velocityNED;
if (m_state_begin == nullptr)
{
m_state_begin = new TPlaneState();
m_state_begin = new TPlaneState();
m_state_begin->position = CCoordinateTransformation::toEcef(pos);
m_state_begin->orientation.heading = heading;
m_state_begin->orientation.pitch = pitch;
m_state_begin->orientation.bank = bank;
m_state_begin->groundspeed = groundSpeed;
m_state_begin->position = pos.toCartesian();
m_state_begin->orientation.heading = heading*Constants::DegToRad;
m_state_begin->orientation.pitch = pitch*Constants::DegToRad;
m_state_begin->orientation.bank = bank*Constants::DegToRad;
m_state_begin->groundspeed = groundVelocity * Constants::KnotsToMeterPerSecond;
velocityNED =
CCoordinateNed(pos,
cos(m_state_begin->orientation.heading.value(CAngleUnit::rad())) * m_state_begin->groundspeed.value(CSpeedUnit::m_s()),
sin(m_state_begin->orientation.heading.value(CAngleUnit::rad())) * m_state_begin->groundspeed.value(CSpeedUnit::m_s()), 0);
vNED.setNorth( cos(m_state_begin->orientation.heading)*m_state_begin->groundspeed );
vNED.setEast( sin(m_state_begin->orientation.heading)*m_state_begin->groundspeed );
vNED.setDown(0);
vNED.setPosition(pos);
m_state_begin->velocity = vNED.toECEF();
m_state_begin->timestamp = 0;
return;
}
else
{
stateNow(m_state_begin);
m_state_begin->velocity = CCoordinateTransformation::toEcef(velocityNED).toMathVector();
m_state_begin->timestamp = 0;
return velocityNED;
}
if ( m_state_end == NULL )
{
m_state_end = new TPlaneState();
}
stateNow(m_state_begin);
if (m_state_end == nullptr) m_state_end = new TPlaneState();
m_state_end->reset();
m_state_end->timestamp = m_time.elapsed();
m_state_end->position = CCoordinateTransformation::toEcef(pos);
m_state_end->orientation.heading = CHeading(normalizeRadians(heading), false);
m_state_end->orientation.pitch = normalizeRadians(pitch);
m_state_end->orientation.bank = normalizeRadians(bank);
m_state_end->groundspeed = groundSpeed;
m_state_end->position = pos.toCartesian();
m_state_end->orientation.heading = normalizeRadians(heading*Constants::DegToRad);
m_state_end->orientation.pitch = normalizeRadians(pitch*Constants::DegToRad);
m_state_end->orientation.bank = normalizeRadians(bank*Constants::DegToRad);
m_state_end->groundspeed = groundVelocity*Constants::KnotsToMeterPerSecond;
vNED.setNorth( cos(m_state_end->orientation.heading)*m_state_end->groundspeed );
vNED.setEast( sin(m_state_end->orientation.heading)*m_state_end->groundspeed );
vNED.setDown(0);
vNED.setPosition(pos);
m_state_end->velocity = vNED.toECEF();
std::cout << " Interpolator End velocity: " << std::endl;
vNED.print();
std::cout << std::endl;
velocityNED =
CCoordinateNed(pos,
cos(m_state_end->orientation.heading.value(CAngleUnit::rad())) * m_state_end->groundspeed.value(CSpeedUnit::m_s()),
sin(m_state_end->orientation.heading.value(CAngleUnit::rad())) * m_state_end->groundspeed.value(CSpeedUnit::m_s()), 0);
m_state_end->velocity = CCoordinateTransformation::toEcef(velocityNED).toMathVector();
m_timeEnd = 5;
double m_TFpow4 = CMath::cubic(m_timeEnd) * m_timeEnd;
double m_TFpow4 = CMath::cubic(m_timeEnd) * m_timeEnd;
m_a = m_state_begin->velocity * CMath::square(m_timeEnd);
m_a += m_state_end->velocity * CMath::square(m_timeEnd);
m_a += m_state_begin->position * m_timeEnd * 2;
m_a -= m_state_end->position * m_timeEnd * 2;
m_a += m_state_begin->position.toMathVector() * m_timeEnd * 2;
m_a -= m_state_end->position.toMathVector() * m_timeEnd * 2;
m_a *= 6;
m_a /= m_TFpow4;
m_b = m_state_begin->velocity * CMath::cubic(m_timeEnd) * (-2) - m_state_end->velocity * CMath::cubic(m_timeEnd);
m_b = m_b - m_state_begin->position * CMath::square(m_timeEnd) * 3 + m_state_end->position * CMath::square(m_timeEnd) * 3;
m_b = m_b * 2 / ( m_TFpow4 );
m_b = m_b - m_state_begin->position.toMathVector() * CMath::square(m_timeEnd) * 3 + m_state_end->position.toMathVector() * CMath::square(m_timeEnd) * 3;
m_b = m_b * 2 / (m_TFpow4);
return velocityNED;
}
bool CInterpolator::isValid()
/*
* Valid object?
*/
bool CInterpolator::isValid() const
{
return (m_state_begin && m_state_end);
}
/*
* Calculate current state
*/
bool CInterpolator::stateNow(TPlaneState *state)
{
if ( !isValid() )
return false;
if (!this->isValid()) return false;
double time = 5;
/*!
Plane Position
*/
// Plane Position
double timePow2 = CMath::square(time);
double timePow3 = CMath::cubic(time);
CEcef pos;
pos = m_b*3*timePow2*m_timeEnd + m_a * timePow3 * m_timeEnd - m_b * 3 * time * CMath::square(m_timeEnd) - m_a * time* CMath::cubic(m_timeEnd);
pos += m_state_begin->position*(-6)*time + m_state_begin->position*6*m_timeEnd + m_state_end->position*6*time;
pos /= 6*m_timeEnd;
CCoordinateEcef pos(m_b * 3 * timePow2 * m_timeEnd + m_a * timePow3 * m_timeEnd - m_b * 3 * time * CMath::square(m_timeEnd) - m_a * time * CMath::cubic(m_timeEnd));
pos += m_state_begin->position * (-6) * time + m_state_begin->position * 6 * m_timeEnd + m_state_end->position * 6 * time;
pos /= 6 * m_timeEnd;
state->position = pos;
CEcef vel;
vel.zeros();
vel = m_a * ( 3 * m_timeEnd * CMath::square(time) - CMath::cubic(m_timeEnd));
vel += m_b * ( 6 * m_timeEnd * time - 3 * CMath::square(m_timeEnd)) + (m_state_end->position - m_state_begin->position) * 6;
vel /= 6*m_timeEnd;
state->velocity = vel;
state->velNED = vel.toNED(pos.toGeodetic());
/*!
Plane Orientation
*/
double vEast = state->velNED.East();
double vNorth = state->velNED.North();
double fraction = vNorth / vEast;
double heading = atan2 (vNorth, vEast);
CVector3D vel(m_a * (3 * m_timeEnd * CMath::square(time) - CMath::cubic(m_timeEnd)));
vel += m_b * (6 * m_timeEnd * time - 3 * CMath::square(m_timeEnd)) + (m_state_end->position - m_state_begin->position).toMathVector() * 6;
vel /= 6 * m_timeEnd;
state->orientation.heading = heading * Constants::RadToDeg;
state->velocity = vel;
state->velNED = CCoordinateTransformation::toNed(CCoordinateEcef(vel), CCoordinateTransformation::toGeodetic(pos));
return true;
// Plane Orientation
double vEast = state->velNED.east();
double vNorth = state->velNED.north();
state->orientation.heading = CHeading(atan2(vNorth, vEast), false, CAngleUnit::rad());
return true;
}
double CInterpolator::normalizeRadians(double radian)
/*
* Normalize radians, clarify what happens here
*/
CAngle CInterpolator::normalizeRadians(const CAngle &angle) const
{
return radian - Constants::TwoPI * floor(0.5 + radian / Constants::TwoPI);
double radian = angle.value(CAngleUnit::rad());
radian = radian - BlackMisc::Math::TwoPI * floor(0.5 + radian / BlackMisc::Math::TwoPI);
return CAngle(radian, CAngleUnit::rad());
}
} // namespace BlackCore
} // namespace

127
src/blackcore/interpolator.h
View File

@@ -1,79 +1,116 @@
//! Copyright (C) 2013 Roland Winklmeier
//! This Source Code Form is subject to the terms of the Mozilla Public
//! License, v. 2.0. If a copy of the MPL was not distributed with this
//! file, You can obtain one at http://mozilla.org/MPL/2.0/
/* Copyright (C) 2013 VATSIM Community / contributors
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#ifndef INTERPOLATOR_H
#define INTERPOLATOR_H
#ifndef BLACKCORE_INTERPOLATOR_H
#define BLACKCORE_INTERPOLATOR_H
#include "blackmisc/coordinatetransformation.h"
#include "blackmisc/pqspeed.h"
#include "blackmisc/avheading.h"
#include <QElapsedTimer>
#include "blackcore/vector_geo.h"
#include <blackcore/ecef.h>
#include "blackcore/vector_3d.h"
#include "blackcore/ned.h"
#include "blackcore/ecef.h"
#include "blackcore/constants.h"
namespace BlackCore
{
/*!
* \typedef Plane's orientation
*/
typedef struct
{
double heading;
double pitch;
double bank;
BlackMisc::Aviation::CHeading heading; // honestly I think this is a track TODO
BlackMisc::PhysicalQuantities::CAngle pitch;
BlackMisc::PhysicalQuantities::CAngle bank;
} TOrientation;
/*!
* \typedef Plane's state
*/
typedef struct
{
void reset()
{
}
/*!
* \brief Reset data
*/
void reset() {}
qint64 timestamp;
CEcef position;
TOrientation orientation;
double groundspeed;
CVector3D velocity;
CNed velNED;
qint64 timestamp;
TOrientation orientation;
BlackMisc::PhysicalQuantities::CSpeed groundspeed;
BlackMisc::Math::CVector3D velocity;
BlackMisc::Geo::CCoordinateEcef position;
BlackMisc::Geo::CCoordinateNed velNED;
} TPlaneState;
/*!
* \brief Interpolator, calculation inbetween positions
*/
class CInterpolator
{
public:
/*!
* \brief Default constructor
*/
CInterpolator();
virtual ~CInterpolator();
/*!
* \brief Virtual destructor
*/
virtual ~CInterpolator();
/*!
* \brief Init object
*/
void initialize();
void pushUpdate(CVectorGeo pos, double groundVelocity, double heading, double pitch, double bank);
/*!
* \brief Push an update
* \param pos
* \param groundSpeed
* \param heading
* \param pitch
* \param bank
* \return
*/
BlackMisc::Geo::CCoordinateNed pushUpdate(const BlackMisc::Geo::CCoordinateGeodetic &pos,
const BlackMisc::PhysicalQuantities::CSpeed &groundSpeed,
const BlackMisc::Aviation::CHeading &heading,
const BlackMisc::PhysicalQuantities::CAngle &pitch,
const BlackMisc::PhysicalQuantities::CAngle &bank);
bool isValid();
/*!
* \brief Valid state?
* \return
*/
bool isValid() const;
/*!
* \brief Calculate current state
* \param state
* \return
*/
bool stateNow(TPlaneState *state);
bool stateNow(TPlaneState *state);
private:
double normalizeRadians(double radian);
QElapsedTimer m_time;
TPlaneState *m_state_begin;
TPlaneState *m_state_end;
bool m_valid;
CVector3D m_a;
CVector3D m_b;
double m_timeEnd;
double m_timeBegin;
BlackMisc::Math::CVector3D m_a;
BlackMisc::Math::CVector3D m_b;
QElapsedTimer m_time;
TPlaneState *m_state_begin;
TPlaneState *m_state_end;
bool m_valid;
double m_timeEnd;
double m_timeBegin;
/*!
* \brief Normalize radians
* \param angle
* \return
*/
BlackMisc::PhysicalQuantities::CAngle normalizeRadians(const BlackMisc::PhysicalQuantities::CAngle &angle) const;
};
} // namespace BlackCore
#endif // INTERPOLATOR_H
#endif // guard

92
src/blackcore/multiplayer.cpp
View File

@@ -1,48 +1,53 @@
#include <blackcore/simulator.h>
#include <blackcore/vector_geo.h>
/* Copyright (C) 2013 VATSIM Community / contributors
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#include "blackcore/simulator.h"
#include "blackmisc/coordinategeodetic.h"
#include "blackcore/plane.h"
#include "blackcore/multiplayer.h"
#include "blackmisc/avheading.h"
namespace BlackCore {
using namespace BlackMisc::Geo;
using namespace BlackMisc::PhysicalQuantities;
using namespace BlackMisc::Aviation;
CMultiPlayer::CMultiPlayer()
: m_isRunning(false)
namespace BlackCore
{
CMultiPlayer::CMultiPlayer() : m_isRunning(false)
{
registerMessageFunction(this, &CMultiPlayer::onPositionUpdate);
m_simulator = ISimulator::createDriver(ISimulator::FSX);
}
void CMultiPlayer::start()
{
if (m_isRunning)
return;
if (m_isRunning) return;
m_isRunning = true;
}
void CMultiPlayer::stop()
{
if (!m_isRunning)
return;
if (!m_isRunning) return;
m_isRunning = false;
}
void CMultiPlayer::run()
{
TPlaneManager::iterator it;
for (it = m_multiplayer_planes.begin(); it != m_multiplayer_planes.end(); ++it)
{
if (needsToRemoved(it.value()))
{
removePlane(it.value());
it = m_multiplayer_planes.erase(it);
}
{
TPlaneManager::iterator it;
for (it = m_multiplayer_planes.begin(); it != m_multiplayer_planes.end(); ++it)
{
if (needsToRemoved(it.value()))
{
removePlane(it.value());
it = m_multiplayer_planes.erase(it);
}
if (areAIPlanesEnabled())
it.value()->render();
}
if (areAIPlanesEnabled())
it.value()->render();
}
}
bool CMultiPlayer::isKnown(const QString &callsign) const
@@ -60,37 +65,38 @@ void CMultiPlayer::onPositionUpdate(const FSD::FSD_MSG_Plane_Position *plane_pos
QString callsign = plane_position->Callsign();
CPlane *plane;
plane = getPlane(callsign);
CVectorGeo position(plane_position->Latitude(), plane_position->Longitude(), plane_position->Altitude());
FS_PBH pitchBankHeading;
pitchBankHeading.pbh = plane_position->PBH();
//! TODO: Pitch Bank Heading and a timestamp
if (plane)
{
plane->addPosition(position, plane_position->Speed(), pitchBankHeading.hdg, pitchBankHeading.pitch, pitchBankHeading.bank);
}
else
plane = getPlane(callsign);
if (!plane)
{
plane = new CPlane(callsign, m_simulator);
addPlane(plane);
plane->addPosition(position, plane_position->Speed(), pitchBankHeading.hdg, pitchBankHeading.pitch, pitchBankHeading.bank);
}
CCoordinateGeodetic position(plane_position->Latitude(), plane_position->Longitude(), plane_position->Altitude());
FS_PBH pitchBankHeading;
pitchBankHeading.pbh = plane_position->PBH();
// TODO: Pitch Bank Heading and a timestamp
// TODO: Usage of physical quantities with FSD::FSD_MSG_Plane_Position
plane->addPosition(position,
CSpeed(plane_position->Speed(), CSpeedUnit::kts()),
CHeading((qint32)pitchBankHeading.hdg, false, CAngleUnit::deg()),
CAngle((qint32)pitchBankHeading.pitch, CAngleUnit::deg()),
CAngle((qint32)pitchBankHeading.bank, CAngleUnit::deg()));
}
void CMultiPlayer::addPlane(CPlane *plane)
{
m_multiplayer_planes.insert(plane->Callsign(), plane);
m_multiplayer_planes.insert(plane->callsign(), plane);
}
void CMultiPlayer::removePlane(CPlane *plane)
{
qint32 id;
m_simulator->removePlane(id);
qint32 id;
m_simulator->removePlane(id);
}
} //! namespace BlackCore
} // namespace

70
src/blackcore/plane.cpp
View File

@@ -1,36 +1,54 @@
/* Copyright (C) 2013 VATSIM Community / contributors
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#include "blackcore/plane.h"
#include "blackcore/interpolator.h"
#include "blackcore/simulator.h"
#include "blackmisc/debug.h"
namespace BlackCore {
using namespace BlackMisc::Geo;
using namespace BlackMisc::PhysicalQuantities;
CPlane::CPlane()
: m_interpolator(NULL), m_driver(NULL)
{
}
namespace BlackCore
{
CPlane::CPlane(const QString &callsign, ISimulator *driver)
: m_callsign(callsign), m_interpolator(NULL), m_driver(driver)
{
m_interpolator = new CInterpolator();
/*
* Default constructor
*/
CPlane::CPlane() : m_interpolator(nullptr), m_driver(nullptr) { }
Q_ASSERT(m_interpolator);
Q_ASSERT(driver);
}
/*
* Constructor
*/
CPlane::CPlane(const QString &callsign, ISimulator *driver) : m_callsign(callsign), m_interpolator(NULL), m_driver(driver)
{
m_interpolator = new CInterpolator();
Q_ASSERT(m_interpolator);
Q_ASSERT(driver);
}
void CPlane::addPosition(const CVectorGeo &position, double groundVelocity, double heading, double pitch, double bank)
{
Q_ASSERT(m_interpolator);
m_interpolator->pushUpdate(position, groundVelocity, heading, pitch, bank);
}
void CPlane::render()
{
//m_driver->updatePositionAndSpeed();
}
/*
* Add a position
*/
void CPlane::addPosition(const CCoordinateGeodetic &position, const CSpeed &groundVelocity, const BlackMisc::Aviation::CHeading &heading, const CAngle &pitch, const CAngle &bank)
{
Q_ASSERT(m_interpolator);
m_interpolator->pushUpdate(position, groundVelocity, heading, pitch, bank);
}
/*
* Render this object
*/
void CPlane::render()
{
// void
}
/*
* Last update timestamp
*/
double CPlane::getLastUpdateTime() const
{
return 0;
}
} // namespace BlackCore

103
src/blackcore/plane.h
View File

@@ -1,53 +1,80 @@
//! Copyright (C) 2013 Roland Winklmeier
//! This Source Code Form is subject to the terms of the Mozilla Public
//! License, v. 2.0. If a copy of the MPL was not distributed with this
//! file, You can obtain one at http://mozilla.org/MPL/2.0/
/* Copyright (C) 2013 VATSIM Community / contributors
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this
* file, You can obtain one at http://mozilla.org/MPL/2.0/. */
#ifndef PLANE_H
#define PLANE_H
#ifndef BLACKCORE_PLANE_H
#define BLACKCORE_PLANE_H
#include "blackcore/interpolator.h"
#include <QString>
namespace BlackCore
{
namespace BlackCore {
class ISimulator;
class ISimulator;
class CInterpolator;
class CVectorGeo;
/*!
* \brief Plane class
*/
class CPlane
{
public:
class CPlane
{
public:
enum ESquawkMode {Standby = 'S',
Charlie = 'N',
Ident = 'Y'
};
enum ESquawkMode {Standby = 'S',
Charlie = 'N',
Ident = 'Y'};
/*!
* \brief Default constructor
*/
CPlane();
CPlane();
CPlane(const QString &callsign, ISimulator *driver);
/*!
* \brief Constructor
* \param callsign
* \param driver
*/
CPlane(const QString &callsign, ISimulator *driver);
/*!
* \brief Add a position (for the interpolator)
* \param position
* \param groundVelocity
* \param heading
* \param pitch
* \param bank
*/
void addPosition(const BlackMisc::Geo::CCoordinateGeodetic &position,
const BlackMisc::PhysicalQuantities::CSpeed &groundVelocity,
const BlackMisc::Aviation::CHeading &heading,
const BlackMisc::PhysicalQuantities::CAngle &pitch,
const BlackMisc::PhysicalQuantities::CAngle &bank);
/*!
* \brief Returns the callsign of the multiplayer plane
* \return
*/
QString callsign() { return m_callsign; }
/*!
* \brief render
*/
void render();
void addPosition(const CVectorGeo &position, double groundVelocity, double heading, double pitch, double bank);
/*!
* \brief Last updated timestamp
* \return
*/
double getLastUpdateTime() const;
//! Returns the callsign of the multiplayer plane
/*!
\return callsign.
*/
inline QString& Callsign() { return m_callsign; }
void render();
double getLastUpdateTime();
private:
private:
QString m_callsign;
CInterpolator *m_interpolator;
ISimulator *m_driver;
};
QString m_callsign;
CInterpolator *m_interpolator;
ISimulator *m_driver;
};
} // namespace BlackCore
#endif // PLANE_H
#endif // guard