diff --git a/MessageBasic/MessageBasic.vcxproj b/MessageBasic/MessageBasic.vcxproj
index 2f830c0..34b5807 100644
--- a/MessageBasic/MessageBasic.vcxproj
+++ b/MessageBasic/MessageBasic.vcxproj
@@ -61,6 +61,7 @@
$(SolutionDir)SimsBasic;$(SolutionDir)StandardGlobe;$(IncludePath)
+ $(SolutionDir)$(Platform)\$(Configuration);$(LibraryPath)
@@ -68,6 +69,9 @@
Default
+
+ StandardGlobe.lib;%(AdditionalDependencies)
+
diff --git a/MessageBasic/motion_access.cpp b/MessageBasic/motion_access.cpp
index a49d412..71b7987 100644
--- a/MessageBasic/motion_access.cpp
+++ b/MessageBasic/motion_access.cpp
@@ -10,6 +10,9 @@ void NavigateWithRoute::recoveryFrom(const QJsonObject& obj)
AbstractMessage::recoveryFrom(obj);
STRING_PEAK(_route_name);
+ uint64_t _drive_mode;
+ UINT64_PEAK(_drive_mode);
+ this->_drive_mode = (NavigateDriveMode)_drive_mode;
}
void NavigateWithRoute::saveTo(QJsonObject& obj) const
@@ -17,48 +20,12 @@ void NavigateWithRoute::saveTo(QJsonObject& obj) const
AbstractMessage::saveTo(obj);
STRING_SAVE(_route_name);
-}
-
-PlatformMotionCommand::PlatformMotionCommand()
- :AbstractMessage(NAME(PlatformMotionCommand))
-{
-
+ uint64_t _drive_mode = (uint64_t)this->_drive_mode;
+ UINT64_SAVE(_drive_mode);
}
#include
#include
-void PlatformMotionCommand::recoveryFrom(const QJsonObject& obj)
-{
- AbstractMessage::recoveryFrom(obj);
- DOUBLE_PEAK(_arrived_time);
- DOUBLE_PEAK(_start_time);
-
- auto cmd_array = obj["array"].toArray();
- for (auto idx = 0; idx < cmd_array.size(); ++idx) {
- auto cmd_obj = cmd_array.at(idx).toObject();
-
- }
-}
-
-void PlatformMotionCommand::saveTo(QJsonObject& obj) const
-{
- AbstractMessage::saveTo(obj);
- DOUBLE_SAVE(_arrived_time);
- DOUBLE_SAVE(_start_time);
-
- QJsonArray cmd_array;
- for (auto k : this->_motion_set.keys()) {
- QJsonObject cmd;
- cmd["type"] = (int)k;
- QJsonObject objx;
- this->_motion_set[k]->saveTo(objx);
- cmd["content"] = objx;
-
- cmd_array.append(cmd);
- }
- obj["array"] = cmd_array;
-}
-
MotionSequencePreviewGet::MotionSequencePreviewGet()
:AbstractMessage(NAME(MotionSequencePreviewGet)) {
}
@@ -72,44 +39,174 @@ PlatformMotionSequence::PlatformMotionSequence()
void PlatformMotionSequence::recoveryFrom(const QJsonObject& obj)
{
- this->_cmd_sequence.clear();
-
- AbstractMessage::recoveryFrom(obj);
- auto cmd_array = obj["cmd_array"].toArray();
- for (auto idx = 0; idx < cmd_array.size(); idx++) {
- auto cmd_object = cmd_array.at(idx).toObject();
- auto ptr = std::make_shared();
- ptr->recoveryFrom(cmd_object);
- this->_cmd_sequence.append(ptr);
- }
+ throw -1;
}
void PlatformMotionSequence::saveTo(QJsonObject& obj) const
+{
+ throw -1;
+}
+
+#include
+
+#define PI 3.14159265354
+auto deg2a = [](double v) { return v / 180.0 * PI; };
+auto rad2d = [](double v) { return v / PI * 180.0; };
+#include
+#include
+StrightLineMotion::StrightLineMotion()
+ : PlatformMotionCommand(NAME(StrightLineMotion)) {
+}
+
+double StrightLineMotion::timeExpend() const
+{
+ auto sqrt_v = std::sqrt(_speed_motion * _speed_motion + 2 * _accerlate_rates * _length_total);
+ return (sqrt_v - _speed_motion) / _accerlate_rates;
+}
+
+#include
+QVector3D StrightLineMotion::posAtTime(double time) const
+{
+ auto length = _speed_motion * time +
+ 0.5 * _accerlate_rates * time * time;
+
+ return QVector3D(0, length, 0);
+}
+
+QVector3D StrightLineMotion::speedAtTime(double time) const
+{
+ auto vspeed = _speed_motion + _accerlate_rates * time;
+ return QVector3D(0, vspeed, 0);
+}
+
+void StrightLineMotion::recoveryFrom(const QJsonObject& obj)
+{
+ AbstractMessage::recoveryFrom(obj);
+
+ DOUBLE_PEAK(_speed_motion);
+ DOUBLE_PEAK(_accerlate_rates);
+ DOUBLE_PEAK(_length_total);
+}
+
+void StrightLineMotion::saveTo(QJsonObject& obj) const
{
AbstractMessage::saveTo(obj);
- QJsonArray cmd_array;
- for (auto info : _cmd_sequence) {
- QJsonObject cmd_object;
- info->saveTo(cmd_object);
- cmd_array.append(cmd_object);
- }
- obj["cmd_array"] = cmd_array;
+ DOUBLE_SAVE(_speed_motion);
+ DOUBLE_SAVE(_accerlate_rates);
+ DOUBLE_SAVE(_length_total);
}
-HorizontalLineMotionCmd::HorizontalLineMotionCmd(){}
+HorizontalArcMotion::HorizontalArcMotion()
+ : PlatformMotionCommand(NAME(HorizontalArcMotion)) {
+}
-MotionRotate HorizontalLineMotionCmd::rotateType() const
+double HorizontalArcMotion::timeExpend() const
{
- MotionRotate::Horizontal;
+ auto radius = std::abs(_center_point.x());
+ auto arc_length = radius * deg2a(_rotate_deg);
+ return arc_length / _speed_motion;
}
-MotionType HorizontalLineMotionCmd::motionType() const
+QVector3D HorizontalArcMotion::posAtTime(double time) const
{
- MotionType::ParabolaLine;
+ auto radius = std::abs(_center_point.x());
+ auto arc_length = _speed_motion * time;
+ auto _rotate_rads = arc_length / radius;
+
+ QVector3D vec(std::cos(_rotate_rads) * radius / _center_point.x(),
+ std::sin(_rotate_rads), 0);
+ return vec * radius + QVector3D(_center_point);
}
-LonLatAltPos HorizontalLineMotionCmd::posAtPercent(double percent)
+QVector3D HorizontalArcMotion::speedAtTime(double time) const
{
- throw std::logic_error("The method or operation is not implemented.");
+ auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
+ auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(0, 0, 1));
+ return speed_nv.normalized() * _speed_motion;
+}
+
+void HorizontalArcMotion::recoveryFrom(const QJsonObject& obj)
+{
+ AbstractMessage::recoveryFrom(obj);
+
+ double posx, posy, posz;
+ DOUBLE_PEAK(posx);
+ DOUBLE_PEAK(posy);
+ DOUBLE_PEAK(posz);
+ DOUBLE_PEAK(_rotate_deg);
+ DOUBLE_PEAK(_speed_motion);
+
+ _center_point = QVector3D(posx, posy, posz);
+}
+
+void HorizontalArcMotion::saveTo(QJsonObject& obj) const
+{
+ AbstractMessage::saveTo(obj);
+
+ double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
+ DOUBLE_SAVE(posx);
+ DOUBLE_SAVE(posy);
+ DOUBLE_SAVE(posz);
+ DOUBLE_SAVE(_rotate_deg);
+ DOUBLE_SAVE(_speed_motion);
+}
+
+PlatformMotionCommand::PlatformMotionCommand(const QString& nm)
+ : AbstractMessage(nm) {
+}
+
+VerticalArcMotion::VerticalArcMotion()
+ : PlatformMotionCommand(NAME(VerticalArcMotion)) {
+}
+
+double VerticalArcMotion::timeExpend() const
+{
+ auto radius = std::abs(_center_point.y());
+ auto arc_length = radius * deg2a(_rotate_deg);
+ return arc_length / _speed_motion;
+}
+
+QVector3D VerticalArcMotion::posAtTime(double time) const
+{
+ auto radius = std::abs(_center_point.z());
+ auto arc_length = _speed_motion * time;
+ auto rotate_rads = arc_length / radius;
+
+ QVector3D vec_r(0, std::sin(rotate_rads),
+ std::cos(rotate_rads) * radius / _center_point.z());
+ return _center_point + vec_r * radius;
+}
+
+QVector3D VerticalArcMotion::speedAtTime(double time) const
+{
+ auto dist_vec = (posAtTime(time) - QVector3D(_center_point)).normalized();
+ auto speed_nv = QVector3D::crossProduct(dist_vec, QVector3D(1, 0, 0));
+ return speed_nv.normalized() * _speed_motion;
+}
+
+void VerticalArcMotion::recoveryFrom(const QJsonObject& obj)
+{
+ AbstractMessage::recoveryFrom(obj);
+
+ double posx, posy, posz;
+ DOUBLE_PEAK(posx);
+ DOUBLE_PEAK(posy);
+ DOUBLE_PEAK(posz);
+ DOUBLE_PEAK(_rotate_deg);
+ DOUBLE_PEAK(_speed_motion);
+
+ _center_point = QVector3D(posx, posy, posz);
+}
+
+void VerticalArcMotion::saveTo(QJsonObject& obj) const
+{
+ AbstractMessage::saveTo(obj);
+
+ double posx = _center_point.x(), posy = _center_point.y(), posz = _center_point.z();
+ DOUBLE_SAVE(posx);
+ DOUBLE_SAVE(posy);
+ DOUBLE_SAVE(posz);
+ DOUBLE_SAVE(_rotate_deg);
+ DOUBLE_SAVE(_speed_motion);
}
diff --git a/MessageBasic/motion_access.h b/MessageBasic/motion_access.h
index d7eb60d..7903595 100644
--- a/MessageBasic/motion_access.h
+++ b/MessageBasic/motion_access.h
@@ -1,11 +1,20 @@
#pragma once
#include "messagebasic.h"
+///
+/// 指定航路机动模式
+///
+enum class NavigateDriveMode {
+ Cruise, // 巡航模式,长途奔袭模式
+ FullSpeed,// 全速模式,战斗机动
+ ReachStop, // 停驻模式
+};
///
/// 依照指定名称路径机动
///
struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
- QString _route_name;
+ QString _route_name; // 规划航线名称
+ NavigateDriveMode _drive_mode; // 驱动模式
NavigateWithRoute();
@@ -15,71 +24,28 @@ struct MESSAGEBASIC_EXPORT NavigateWithRoute : public AbstractMessage {
};
///
-/// 机动类型
+/// 平台机动命令,机体坐标系:主轴为Y,向右为X,向上为Z
///
-enum MotionType {
- None, // 没有
- StraightLine,// 直线机动
- StandardArc, // 标准圆弧
- ParabolaLine,// 抛物线
- Bezier2tArc, // 二次贝塞尔曲线
- Bezier3tArc, // 三次贝塞尔曲线
-};
+struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
+ PlatformMotionCommand(const QString &nm);
-///
-/// 机动角度方向
-///
-enum class MotionRotate {
- Horizontal, Vertical
-};
-
-///
-/// 正交机动命令抽象接口
-///
-struct OrthogonalMotionCommand : public Serializable {
///
- /// 机动平面方向
+ /// 消耗时间
///
///
- virtual MotionRotate rotateType() const = 0;
+ virtual double timeExpend() const = 0;
///
- /// 机动类型
+ /// 指定时间点的相对位置
///
- ///
- virtual MotionType motionType() const = 0;
+ ///
+ /// 机体坐标系
+ virtual QVector3D posAtTime(double time) const = 0;
///
- /// 计算指定百分比位置
+ /// 指定时间点的速度矢量
///
- ///
- ///
- virtual LonLatAltPos posAtPercent(double percent) = 0;
- ///
- /// 指定百分比位置的速度
- ///
- ///
- ///
- virtual double speedAtPercent(double percent) = 0;
- ///
- /// 指定百分比位置速度方向
- ///
- ///
- ///
- virtual QVector2D speedVectorAtPercent(double percent) = 0;
-};
-
-///
-/// 平台机动命令
-///
-struct PlatformMotionCommand : public AbstractMessage {
- QHash> _motion_set; //合成机动命令
- double _arrived_time = 0; // 抵达终点时间
- double _start_time = 0;
-
- PlatformMotionCommand();
-
- // Serializable
- void recoveryFrom(const QJsonObject& obj) override;
- void saveTo(QJsonObject& obj) const override;
+ ///
+ /// 机体坐标系
+ virtual QVector3D speedAtTime(double time) const = 0;
};
///
@@ -106,26 +72,83 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
// 正交机动命令实现 =======================================================
///
-/// 水平直线机动
+/// 直线机动
///
-struct MESSAGEBASIC_EXPORT HorizontalLineMotionCmd : public OrthogonalMotionCommand {
- LonLatPos _lonlat_anchor;// 起始点
- double _azimuth_deg = 0; // 起始方位角
- double _speed_value = 0;// 起始速度
- double _accerlate_rate = 0;// 加速度
+struct MESSAGEBASIC_EXPORT StrightLineMotion : public PlatformMotionCommand {
+ ///
+ /// 机动速度,m/s
+ ///
+ double _speed_motion = 0;
+ ///
+ /// 加速度,m/s^2
+ ///
+ double _accerlate_rates = 0;
+ ///
+ /// 路径长度
+ ///
+ double _length_total = 0;
- HorizontalLineMotionCmd();
+ StrightLineMotion();
+
+ double timeExpend() const override;
+ QVector3D posAtTime(double time) const override;
+ QVector3D speedAtTime(double time) const override;
- // Serializable
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
-
- MotionRotate rotateType() const override;
- MotionType motionType() const override;
- LonLatAltPos posAtPercent(double percent) override;
-
};
-struct MESSAGEBASIC_EXPORT VerticalLineMotionCmd : public OrthogonalMotionCommand {
+#include
+///
+/// 基于机体坐标系的水平标准圆弧机动
+///
+struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
+ ///
+ /// 转弯圆心,
+ ///
+ QVector3D _center_point;
+ ///
+ /// 弧线转角,°
+ ///
+ double _rotate_deg = 0;
+ ///
+ /// 机动速度,m/s
+ ///
+ double _speed_motion = 0;
+ HorizontalArcMotion();
+
+ double timeExpend() const override;
+ QVector3D posAtTime(double time) const override;
+ QVector3D speedAtTime(double time) const override;
+
+ void recoveryFrom(const QJsonObject& obj) override;
+ void saveTo(QJsonObject& obj) const override;
+};
+
+///
+/// 基于机体坐标系的垂直标准圆弧机动
+///
+struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
+ ///
+ /// 转弯圆心,<0, ypos, zpos>
+ ///
+ QVector3D _center_point;
+ ///
+ /// 弧线转角,°
+ ///
+ double _rotate_deg = 0;
+ ///
+ /// 机动速度,m/s
+ ///
+ double _speed_motion = 0;
+
+ VerticalArcMotion();
+
+ double timeExpend() const override;
+ QVector3D posAtTime(double time) const override;
+ QVector3D speedAtTime(double time) const override;
+
+ void recoveryFrom(const QJsonObject& obj) override;
+ void saveTo(QJsonObject& obj) const override;
};
\ No newline at end of file
diff --git a/StandardGlobe/standardglobe.cpp b/StandardGlobe/standardglobe.cpp
index 705641e..df6e5ee 100644
--- a/StandardGlobe/standardglobe.cpp
+++ b/StandardGlobe/standardglobe.cpp
@@ -11,7 +11,7 @@ StandardGlobe::StandardGlobe()
{
}
-ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
+ECEFv3d StandardGlobe::llaToEcef(const LonLatAltPos& v)
{
// 计算距离球心距离
auto axis_r_g = v._alt_m + _radius_m;
@@ -24,14 +24,14 @@ ECEFPos StandardGlobe::llaToEcef(const LonLatAltPos& v)
// ecef-y
auto axis_y = axis_r_lat * sin(deg2a(v._lon_deg));
- return ECEFPos{ axis_x, axis_y, axis_z };
+ return ECEFv3d{ axis_x, axis_y, axis_z };
}
-LonLatAltPos StandardGlobe::ecefToLLA(const ECEFPos& v)
+LonLatAltPos StandardGlobe::ecefToLLA(const ECEFv3d& v)
{
auto r2 = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos);
auto rad_lon = acos(v._x_pos / r2);
- if(v._y_pos < 0)
+ if (v._y_pos < 0)
rad_lon *= -1;
auto rx = std::sqrt(v._x_pos * v._x_pos + v._y_pos * v._y_pos + v._z_pos * v._z_pos);
auto rad_lat = asin(v._z_pos / rx);
@@ -89,7 +89,7 @@ void StandardGlobe::getTargetLLAWithDistance(const LonLatAltPos& base, double di
QQuaternion u1(rad_between, vec_bv);
auto ecef_t = u1.rotatedVector(llaToEcef(base).getVec3());
- ECEFPos sv;
+ ECEFv3d sv;
sv._x_pos = ecef_t.x();
sv._y_pos = ecef_t.y();
sv._z_pos = ecef_t.z();
@@ -139,7 +139,7 @@ LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const Pola
auto ecef_b = llaToEcef(base).getVec3();
auto u_azi = QQuaternion(-deg2a(target._azimuth_deg), ecef_b.normalized());
- auto vec_ts = u_azi.rotatedVector(QVector3D(0,0,1)).normalized();
+ auto vec_ts = u_azi.rotatedVector(QVector3D(0, 0, 1)).normalized();
auto axis_pitch = QVector3D::crossProduct(vec_ts, ecef_b.normalized());
auto u_pit = QQuaternion(deg2a(target._pitch_deg), axis_pitch);
@@ -147,19 +147,34 @@ LonLatAltPos StandardGlobe::getLLAWithPolar(const LonLatAltPos& base, const Pola
auto ts_appv = u_pit.rotatedVector(vec_ts).normalized() * r_shadow;
auto ecef_target = ecef_b + ts_appv;
- ECEFPos tt = ECEFPos::fromVec3(ecef_target);
+ ECEFv3d tt = ECEFv3d::fromVec3(ecef_target);
return ecefToLLA(tt);
}
-QVector3D ECEFPos::getVec3() const
+QVector3D ECEFv3d::getVec3() const
{
return QVector3D(_x_pos, _y_pos, _z_pos);
}
-ECEFPos ECEFPos::fromVec3(const QVector3D& p) {
- ECEFPos o;
+ECEFv3d ECEFv3d::fromVec3(const QVector3D& p) {
+ ECEFv3d o;
o._x_pos = p.x();
o._y_pos = p.y();
o._z_pos = p.z();
return o;
}
+
+LonLatAltPos::LonLatAltPos(const LonLatPos& plain_pos)
+ : _lon_deg(plain_pos._lon_deg),
+ _lat_deg(plain_pos._lat_deg),
+ _alt_m(0) {
+}
+
+LonLatAltPos::LonLatAltPos(double lon, double lat, double alt)
+ :_lon_deg(lon), _lat_deg(lat), _alt_m(alt) {
+}
+
+LonLatPos LonLatAltPos::toLonLatPos() const
+{
+ return LonLatPos{ _lon_deg, _lat_deg };
+}
diff --git a/StandardGlobe/standardglobe.h b/StandardGlobe/standardglobe.h
index 2c3bf9c..1fcb411 100644
--- a/StandardGlobe/standardglobe.h
+++ b/StandardGlobe/standardglobe.h
@@ -15,27 +15,32 @@ struct LonLatPos {
///
/// 经纬高坐标系坐标
///
-struct LonLatAltPos {
+struct STANDARDGLOBE_EXPORT LonLatAltPos {
double _lon_deg = 0;
double _lat_deg = 0;
double _alt_m = 0;
+
+ LonLatAltPos(double lon = 0, double lat = 0, double alt = 0);
+ LonLatAltPos(const LonLatPos& plain_pos);
+
+ LonLatPos toLonLatPos() const;
};
///
/// 地心坐标系
///
-struct ECEFPos {
+struct ECEFv3d {
double _x_pos = 0;// 指向0°经度圈与纬度0°交点
double _y_pos = 0;
double _z_pos = 0;
QVector3D getVec3() const;
- static ECEFPos fromVec3(const QVector3D& p);
+ static ECEFv3d fromVec3(const QVector3D& p);
};
///
/// 东天北坐标系
///
-struct ENUPos {
+struct ENUv3d {
double _e_pos = 0;
double _n_pos = 0;
double _t_pos = 0;
@@ -64,8 +69,8 @@ private:
public:
StandardGlobe();
- static ECEFPos llaToEcef(const LonLatAltPos &v);
- static LonLatAltPos ecefToLLA(const ECEFPos &v);
+ static ECEFv3d llaToEcef(const LonLatAltPos& v);
+ static LonLatAltPos ecefToLLA(const ECEFv3d& v);
///
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角,warning:本计算忽视LLA的高度值
///
@@ -73,7 +78,7 @@ public:
///
///
///
- static void getDistanceWithTargetLLA(const LonLatAltPos &base, const LonLatAltPos &target, double &dist, double &azi);
+ static void getDistanceWithTargetLLA(const LonLatAltPos& base, const LonLatAltPos& target, double& dist, double& azi);
///
/// 根据两点之间的大地线长度和b->t初始方位角,计算目标LLA定位坐标
///
@@ -88,12 +93,12 @@ public:
///
///
///
- static PolarPos getPolarWithLLA(const LonLatAltPos &base, const LonLatAltPos &target);
+ static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
///
/// 通过极坐标,获取目标LLA坐标
///
///
///
///
- static LonLatAltPos getLLAWithPolar(const LonLatAltPos &base, const PolarPos &target);
+ static LonLatAltPos getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target);
};