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); };