mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-05 01:05:34 +08:00
79 lines
1.4 KiB
C++
79 lines
1.4 KiB
C++
//! 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/
|
|
|
|
#ifndef INTERPOLATOR_H
|
|
#define INTERPOLATOR_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 struct
|
|
{
|
|
double heading;
|
|
double pitch;
|
|
double bank;
|
|
|
|
} TOrientation;
|
|
|
|
typedef struct
|
|
{
|
|
void reset()
|
|
{
|
|
}
|
|
|
|
qint64 timestamp;
|
|
CEcef position;
|
|
TOrientation orientation;
|
|
double groundspeed;
|
|
CVector3D velocity;
|
|
CNed velNED;
|
|
|
|
} TPlaneState;
|
|
|
|
class CInterpolator
|
|
{
|
|
public:
|
|
CInterpolator();
|
|
|
|
void initialize();
|
|
|
|
void pushUpdate(CVectorGeo pos, double groundVelocity, double heading, double pitch, double bank);
|
|
|
|
bool isValid();
|
|
|
|
|
|
|
|
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;
|
|
|
|
};
|
|
|
|
} // namespace BlackCore
|
|
|
|
#endif // INTERPOLATOR_H
|