update:添加机体机动路径

This commit is contained in:
codeboss 2025-06-21 12:54:39 +08:00
parent 300b31a578
commit a0bb1feb8c
5 changed files with 296 additions and 152 deletions

View File

@ -61,6 +61,7 @@
<PropertyGroup Label="UserMacros" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'">
<IncludePath>$(SolutionDir)SimsBasic;$(SolutionDir)StandardGlobe;$(IncludePath)</IncludePath>
<LibraryPath>$(SolutionDir)$(Platform)\$(Configuration);$(LibraryPath)</LibraryPath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x64'">
</PropertyGroup>
@ -68,6 +69,9 @@
<ClCompile>
<InlineFunctionExpansion>Default</InlineFunctionExpansion>
</ClCompile>
<Link>
<AdditionalDependencies>StandardGlobe.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x64'" Label="Configuration">
<ClCompile>

View File

@ -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 <QJsonArray>
#include <QJsonObject>
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<PlatformMotionCommand>();
ptr->recoveryFrom(cmd_object);
this->_cmd_sequence.append(ptr);
}
throw -1;
}
void PlatformMotionSequence::saveTo(QJsonObject& obj) const
{
throw -1;
}
#include <standardglobe.h>
#define PI 3.14159265354
auto deg2a = [](double v) { return v / 180.0 * PI; };
auto rad2d = [](double v) { return v / PI * 180.0; };
#include <QVector2D>
#include <cmath>
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>
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);
}

View File

@ -1,11 +1,20 @@
#pragma once
#include "messagebasic.h"
/// <summary>
/// 指定航路机动模式
/// </summary>
enum class NavigateDriveMode {
Cruise, // 巡航模式,长途奔袭模式
FullSpeed,// 全速模式,战斗机动
ReachStop, // 停驻模式
};
/// <summary>
/// 依照指定名称路径机动
/// </summary>
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 {
};
/// <summary>
/// 机动类型
/// 平台机动命令机体坐标系主轴为Y向右为X向上为Z
/// </summary>
enum MotionType {
None, // 没有
StraightLine,// 直线机动
StandardArc, // 标准圆弧
ParabolaLine,// 抛物线
Bezier2tArc, // 二次贝塞尔曲线
Bezier3tArc, // 三次贝塞尔曲线
};
struct MESSAGEBASIC_EXPORT PlatformMotionCommand : public AbstractMessage {
PlatformMotionCommand(const QString &nm);
/// <summary>
/// 机动角度方向
/// </summary>
enum class MotionRotate {
Horizontal, Vertical
};
/// <summary>
/// 正交机动命令抽象接口
/// </summary>
struct OrthogonalMotionCommand : public Serializable {
/// <summary>
/// 机动平面方向
/// 消耗时间
/// </summary>
/// <returns></returns>
virtual MotionRotate rotateType() const = 0;
virtual double timeExpend() const = 0;
/// <summary>
/// 机动类型
/// 指定时间点的相对位置
/// </summary>
/// <returns></returns>
virtual MotionType motionType() const = 0;
/// <param name="time"></param>
/// <returns>机体坐标系</returns>
virtual QVector3D posAtTime(double time) const = 0;
/// <summary>
/// 计算指定百分比位置
/// 指定时间点的速度矢量
/// </summary>
/// <param name="percent"></param>
/// <returns></returns>
virtual LonLatAltPos posAtPercent(double percent) = 0;
/// <summary>
/// 指定百分比位置的速度
/// </summary>
/// <param name="percent"></param>
/// <returns></returns>
virtual double speedAtPercent(double percent) = 0;
/// <summary>
/// 指定百分比位置速度方向
/// </summary>
/// <param name="percent"></param>
/// <returns></returns>
virtual QVector2D speedVectorAtPercent(double percent) = 0;
};
/// <summary>
/// 平台机动命令
/// </summary>
struct PlatformMotionCommand : public AbstractMessage {
QHash<MotionRotate, std::shared_ptr<OrthogonalMotionCommand>> _motion_set; //合成机动命令
double _arrived_time = 0; // 抵达终点时间
double _start_time = 0;
PlatformMotionCommand();
// Serializable
void recoveryFrom(const QJsonObject& obj) override;
void saveTo(QJsonObject& obj) const override;
/// <param name="time"></param>
/// <returns>机体坐标系</returns>
virtual QVector3D speedAtTime(double time) const = 0;
};
/// <summary>
@ -106,26 +72,83 @@ struct MESSAGEBASIC_EXPORT PlatformMotionSequence : public AbstractMessage {
// 正交机动命令实现 =======================================================
/// <summary>
/// 水平直线机动
/// 直线机动
/// </summary>
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 {
/// <summary>
/// 机动速度m/s
/// </summary>
double _speed_motion = 0;
/// <summary>
/// 加速度m/s^2
/// </summary>
double _accerlate_rates = 0;
/// <summary>
/// 路径长度
/// </summary>
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 <QVector3D>
/// <summary>
/// 基于机体坐标系的水平标准圆弧机动
/// </summary>
struct MESSAGEBASIC_EXPORT HorizontalArcMotion : public PlatformMotionCommand {
/// <summary>
/// 转弯圆心,<xpos, ypos, 0>
/// </summary>
QVector3D _center_point;
/// <summary>
/// 弧线转角,°
/// </summary>
double _rotate_deg = 0;
/// <summary>
/// 机动速度m/s
/// </summary>
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;
};
/// <summary>
/// 基于机体坐标系的垂直标准圆弧机动
/// </summary>
struct MESSAGEBASIC_EXPORT VerticalArcMotion : public PlatformMotionCommand {
/// <summary>
/// 转弯圆心,<0, ypos, zpos>
/// </summary>
QVector3D _center_point;
/// <summary>
/// 弧线转角,°
/// </summary>
double _rotate_deg = 0;
/// <summary>
/// 机动速度m/s
/// </summary>
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;
};

View File

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

View File

@ -15,27 +15,32 @@ struct LonLatPos {
/// <summary>
/// 经纬高坐标系坐标
/// </summary>
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;
};
/// <summary>
/// 地心坐标系
/// </summary>
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);
};
/// <summary>
/// 东天北坐标系
/// </summary>
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);
/// <summary>
/// 根据LLA坐标获取两点之间的大地线长度和b->t初始方位角warning本计算忽视LLA的高度值
/// </summary>
@ -73,7 +78,7 @@ public:
/// <param name="target"></param>
/// <param name="dist"></param>
/// <param name="azi"></param>
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);
/// <summary>
/// 根据两点之间的大地线长度和b->t初始方位角计算目标LLA定位坐标
/// </summary>
@ -88,12 +93,12 @@ public:
/// <param name="base"></param>
/// <param name="target"></param>
/// <returns></returns>
static PolarPos getPolarWithLLA(const LonLatAltPos &base, const LonLatAltPos &target);
static PolarPos getPolarWithLLA(const LonLatAltPos& base, const LonLatAltPos& target);
/// <summary>
/// 通过极坐标获取目标LLA坐标
/// </summary>
/// <param name="base"></param>
/// <param name="target"></param>
/// <returns></returns>
static LonLatAltPos getLLAWithPolar(const LonLatAltPos &base, const PolarPos &target);
static LonLatAltPos getLLAWithPolar(const LonLatAltPos& base, const PolarPos& target);
};