dmarkova/Simulator
1
0
Fork 0
forked from CPL/Simulator
Code Pull Requests Activity

edit proshivka

Browse Source
This commit is contained in:
Dana Markova 2025-04-30 12:16:18 +03:00
parent 1f97891439
commit 27a120fc7f
12 changed files with 707 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
DroneClientCpp/Drone.cpp
View File

@ -5,7 +5,7 @@ namespace DroneClient {
// Реализация статического метода GetBytes
array<Byte>^ Drone::GetBytes(Object^ data)
{
int size = Marshal::SizeOf(data);
int size = Marshal::SizeOf(data);
array<Byte>^ arr = gcnew array<Byte>(size);
IntPtr ptr = IntPtr::Zero;
@ -45,12 +45,14 @@ namespace DroneClient {
// Реализация приватного метода SendDataMotor4
array<Byte>^ Drone::SendDataMotor4()
{
DroneData::DataMotor4 mot4;
mot4.Head.Size = Marshal::SizeOf(DroneData::DataMotor4::typeid);
mot4.Head.Mode = DroneData::DataMode::Response;
mot4.Head.Type = DroneData::DataType::DataMotor4;
updateData();
setMotors();
mot4.UL = MotorUL;
mot4.UR = MotorUR;
mot4.DL = MotorDL;
@ -158,6 +160,92 @@ namespace DroneClient {
return zero;
}
//void Drone::setMotors()
//{
// array<UInt16>^ ch = joy->Channels; // ссылка на тот же массив
// UInt16 pow = (ch[2] - 1000) / 10;
// float fpow = float(pow)/ 100.0f;
// const float maxAngle = 20.0f;
//
// desPitch = (ch[1] - 1500) * maxAngle / 500;
// desRoll = (ch[0] - 1499) * maxAngle / 500;
// float errorPitch, errorRoll, forceRoll, forcePitch;
// errorPitch = desPitch -pitch;
// errorRoll = desRoll - roll;
// static float PRegulator = 0.001f;
// forcePitch = PRegulator * errorPitch;
// forceRoll = PRegulator * errorRoll;
// MotorUL = fpow-forcePitch + forceRoll;
// MotorUR = fpow - forcePitch - forceRoll;
// MotorDL = fpow + forcePitch + forceRoll;
// MotorDR = fpow + forcePitch - forceRoll;
//}
/* ───────────── вспомогательная функция ───────────── */
static float Clamp01(float v)
{
if (v < 0.0f) return 0.0f;
if (v > 1.0f) return 1.0f;
return v;
}
/* ───────────── PD-регулятор и микширование ───────── */
void Drone::setMotors()
{
/* ---------- входные каналы --------------------- */
array<UInt16>^ ch = joy->Channels;
const float fpow = (ch[2] - 1000) * 0.001f; // 0…1
/* ---------- желаемые углы ---------------------- */
const float maxAngle = 20.0f;
desPitch = (ch[1] - 1500) * maxAngle / 500.0f;
desRoll = (ch[0] - 1499) * maxAngle / 500.0f;
/* ---------- PD-регулятор ----------------------- */
static float prevErrPitch = 0.0f, prevErrRoll = 0.0f;
const float errPitch = desPitch - pitch;
const float errRoll = desRoll - roll;
const float dt = 0.01f; // период 10 мс (100 Гц)
const float dPitch = (errPitch - prevErrPitch) / dt;
const float dRoll = (errRoll - prevErrRoll) / dt;
const float Kp = 0.115f;
const float Kd = 0.0f;
const float forcePitch = Kp * errPitch + Kd * dPitch;
const float forceRoll = Kp * errRoll + Kd * dRoll;
prevErrPitch = errPitch;
prevErrRoll = errRoll;
/* ---------- распределение на моторы ------------ */
MotorUL = fpow - forcePitch + forceRoll;
MotorUR = fpow - forcePitch - forceRoll;
MotorDL = fpow + forcePitch + forceRoll;
MotorDR = fpow + forcePitch - forceRoll;
//MotorUL = Clamp01(MotorUL);
//MotorUR = Clamp01(MotorUR);
//MotorDL = Clamp01(MotorDL);
//MotorDR = Clamp01(MotorDR);
}
// Реализация конструктора
Drone::Drone()
{
@ -166,6 +254,11 @@ namespace DroneClient {
DroneStreamHead.Mode = DroneData::DataMode::None;
DroneStreamHead.Size = 0;
DroneStreamHead.Type = DroneData::DataType::None;
this->joy = gcnew Joypad();
this->joy->Start("COM7", 115200);
}
// Реализация метода DataStream
@ -252,4 +345,16 @@ namespace DroneClient {
return send;
}
void Drone::updateData(){
Vec3 acc{ this->AccX,this->AccY, this->AccZ };
Vec3 gyr{ this->GyrX,this->GyrY, this->GyrZ };
Vec3 mag{ 1,0, 0};
ORI result = WorkAccGyroMag(acc, gyr, mag, 0, 0.01);
this->pitch = result.Pitch;
this->roll = -result.Roll;
this->yaw = result.Yaw;
}
}

16
DroneClientCpp/Drone.h
View File

@ -3,10 +3,14 @@
#include "DroneData.h"
#include <Windows.h>
#include <vcclr.h>
#include "Orientation.h"
#include "joypad.h"
#using <System.dll>
#using <mscorlib.dll>
using namespace System;
using namespace System::Collections::Generic;
using namespace System::Runtime::InteropServices;
@ -20,12 +24,18 @@ namespace DroneClient {
float GyrX, GyrY, GyrZ;
unsigned int TimeAcc, TimeGyr;
float PosX, PosY;
float LaserRange;
unsigned int TimeRange;
float MotorUL, MotorUR, MotorDL, MotorDR;
float pitch, roll, yaw;
float desPitch, desRoll, desYaw;
Joypad^ joy;
static array<Byte>^ GetBytes(Object^ data);
static Object^ FromBytes(array<Byte>^ arr, Type^ type);
@ -37,6 +47,9 @@ namespace DroneClient {
array<Byte>^ RecvDataLocal(array<Byte>^ data);
array<Byte>^ ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body);
void setMotors();
literal int DroneStreamCount = 512;
array<Byte>^ DroneStreamData;
int DroneStreamIndex;
@ -47,5 +60,8 @@ namespace DroneClient {
System::Collections::Generic::List<array<Byte>^>^ DataStream(array<Byte>^ data, int size);
array<Byte>^ SendRequest();
void updateData();
};
}

4
DroneClientCpp/DroneClientCpp.vcxproj
View File

@ -121,6 +121,7 @@
<ClCompile Include="Drone.cpp" />
<ClCompile Include="FormMain.cpp" />
<ClCompile Include="NetClient.cpp" />
<ClCompile Include="Orientation.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="Drone.h" />
@ -128,7 +129,10 @@
<ClInclude Include="FormMain.h">
<FileType>CppForm</FileType>
</ClInclude>
<ClInclude Include="joypad.h" />
<ClInclude Include="NetClient.h" />
<ClInclude Include="Orientation.h" />
<ClInclude Include="WsServer.h" />
</ItemGroup>
<ItemGroup>
<EmbeddedResource Include="FormMain.resx">

12
DroneClientCpp/DroneClientCpp.vcxproj.filters
View File

@ -24,6 +24,9 @@
<ClCompile Include="Drone.cpp">
<Filter>Исходные файлы</Filter>
</ClCompile>
<ClCompile Include="Orientation.cpp">
<Filter>Исходные файлы</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="FormMain.h">
@ -38,5 +41,14 @@
<ClInclude Include="Drone.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="WsServer.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="Orientation.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
<ClInclude Include="joypad.h">
<Filter>Файлы заголовков</Filter>
</ClInclude>
</ItemGroup>
</Project>

2
DroneClientCpp/DroneData.h
View File

@ -110,7 +110,7 @@ namespace DroneData {
{
public:
DataHead Head;
float UL, UR, LL, RR, DL, DR;
float UL, UR, LL, RR, DL, DR;
static const int StrLen = sizeof(DataMotor6);
};

3
DroneClientCpp/FormMain.cpp
View File

@ -9,4 +9,5 @@ int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) {
Application::SetCompatibleTextRenderingDefault(false);
Application::Run(gcnew FormMain);
return 0;
}
}

BIN
DroneClientCpp/FormMain.h
View File

Binary file not shown.

3
DroneClientCpp/FormMain.resx
View File

@ -123,4 +123,7 @@
<metadata name="timer1.TrayLocation" type="System.Drawing.Point, System.Drawing, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">
<value>310, 17</value>
</metadata>
<metadata name="$this.TrayHeight" type="System.Int32, mscorlib, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089">
<value>25</value>
</metadata>
</root>

254
DroneClientCpp/Orientation.cpp Normal file
View File

@ -0,0 +1,254 @@
#include "orientation.h"
#include <math.h>
static const float PI = 3.14159265359f;
static const float TO_DEG = 180.0f / PI;
static const float TO_RAD = PI / 180.0f;
static const float R = 8.314f;
static const float M = 0.029f;
static const float g = 9.80665f;
static const float ro = 1.189f;
struct Quaternion
{
float w, x, y, z;
};
static Quaternion qCurrent = { 1, 0, 0, 0 };
static const float period = 0.005f;
static bool isFirst = true;
static void vecNormalize(Vec3& v)
{
float n = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
if (n > 1e-9f)
{
v.x /= n;
v.y /= n;
v.z /= n;
}
}
static Vec3 vecCross(const Vec3& a, const Vec3& b)
{
return
{
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
};
}
static void normalizeQuaternion(Quaternion& q)
{
float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
if (norm > 1e-12f)
{
q.w /= norm;
q.x /= norm;
q.y /= norm;
q.z /= norm;
}
}
static Quaternion quaternionMultiply(const Quaternion& q1, const Quaternion& q2)
{
Quaternion r;
r.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
r.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
r.y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
r.z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
return r;
}
static Quaternion rotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
{
Quaternion r = quaternionMultiply(quaternionMultiply(q, Quaternion{ 0, In.x, In.y, In.z }), Quaternion{ q.w, -q.x, -q.y, -q.z });
return r;
}
static Vec3 backRotateVectorByQuaternion(const Quaternion& q, const Vec3& In)
{
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, Quaternion{ 0, In.x, In.y, In.z }), q);
return { r.x, r.y, r.z };
}
static Quaternion backRotateVectorByQuaternion2(const Quaternion& q, const Quaternion& In)
{
Quaternion r = quaternionMultiply(quaternionMultiply(Quaternion{ q.w, -q.x, -q.y, -q.z }, In), q);
return r;
}
static Quaternion createYawQuaternion(float angle)
{
Quaternion q;
q.w = cosf(angle);
q.x = 0.0f;
q.y = 0.0f;
q.z = sinf(angle);
return q;
}
static Quaternion AccQuaternion(Quaternion& current, Vec3 a, float gravity)
{
float acc = sqrtf(a.x * a.x + a.y * a.y + a.z * a.z);
if (acc > (1.0f + gravity) || acc < (1.0f - gravity)) return current;
vecNormalize(a);
Vec3 g{ 0, 0, 1 };
Vec3 axis = vecCross(g, a);
float w = 1 + (g.x * a.x + g.y * a.y + g.z * a.z);
Quaternion q = { w, axis.x, axis.y, axis.z };
normalizeQuaternion(q);
Quaternion qYaw{ current.w, 0, 0, current.z };
return quaternionMultiply(q, qYaw); // Âîñòàíîâèòü îáîðîò ïî Z
}
static Quaternion GyroQuaternion(Quaternion& current, float wx, float wy, float wz)
{
Quaternion Mapp = current;
Quaternion Spd{ 0, wx, wy, wz };
Quaternion aq = quaternionMultiply(Spd, Mapp);
Mapp.w -= 0.5f * aq.w;
Mapp.x -= 0.5f * aq.x;
Mapp.y -= 0.5f * aq.y;
Mapp.z -= 0.5f * aq.z;
normalizeQuaternion(Mapp);
return Mapp;
}
static Quaternion nlerp(const Quaternion& q1, const Quaternion& q2, float alpha)
{
float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
Quaternion q2_ = q2;
if (dot < 0)
{
q2_.w = -q2.w;
q2_.x = -q2.x;
q2_.y = -q2.y;
q2_.z = -q2.z;
}
Quaternion r;
r.w = (1.0f - alpha) * q1.w + alpha * q2_.w;
r.x = (1.0f - alpha) * q1.x + alpha * q2_.x;
r.y = (1.0f - alpha) * q1.y + alpha * q2_.y;
r.z = (1.0f - alpha) * q1.z + alpha * q2_.z;
normalizeQuaternion(r);
return r;
}
inline float GetAngle(float a1, float a2, float az)
{
if (a2 == 0.0f && az == 0.0f) return a1 > 0.0f ? 90.0f : -90.0f;
return atanf(a1 / sqrtf(a2 * a2 + az * az)) * TO_DEG;
}
static Vec3 quaternionToPitchRollYaw(const Quaternion& q, float& Upside)
{
Quaternion pry = rotateVectorByQuaternion(q, { 0, 0, 1 });
Upside = (pry.z > 0.0f) ? 1.0f : -1.0f;
float yaw = 2 * atan2f(q.z, q.w) * TO_DEG;
if (yaw < 0.0f) yaw = 360.0f + yaw;
return // Sovereign orientation
{
GetAngle(pry.y, pry.x, pry.z), // Pitch
GetAngle(-pry.x, pry.y, pry.z), // Roll
yaw // Yaw
};
}
static void addMagneto(Quaternion& q, Vec3 mag, float alpha, const float shift)
{
static Quaternion yq = createYawQuaternion(shift * TO_RAD);
vecNormalize(mag);
Quaternion mQ = { 0, mag.x, mag.y, mag.z };
Quaternion mW = backRotateVectorByQuaternion2(q, mQ);
mW = quaternionMultiply(mW, yq); // Shifting the axes to the true north
float gamma = mW.x * mW.x + mW.y * mW.y;
float beta = sqrtf(gamma + mW.x * sqrtf(gamma));
Quaternion mD
{
beta / (sqrtf(2.0f * gamma)),
0.0f,
0.0f,
mW.y / (sqrtf(2.0f) * beta),
};
mD.w = (1.0f - alpha) + alpha * mD.w;
mD.z = alpha * mD.z;
if (mD.w != mD.w || mD.x != mD.x || mD.y != mD.y || mD.z != mD.z) return;
q = quaternionMultiply(q, mD);
normalizeQuaternion(q);
}
// WorkAccGyro({DataIMU.Acc.X, DataIMU.Acc.Y, DataIMU.Acc.Z}, {DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z}, {-DataIMU.Mag.X, DataIMU.Mag.Y, DataIMU.Mag.Z}, 0.01);
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
{
float wx = gyr.x * 500 / 32768 * 1.21f * (PI / 180) * period;
float wy = gyr.y * 500 / 32768 * 1.21f * (PI / 180) * period;
float wz = gyr.z * 500 / 32768 * 1.21f * (PI / 180) * period;
Vec3 aB = acc;
Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f); // Tolerance for gravity deviation 5%
qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
Quaternion qFused = nlerp(qCurrent, qAcc, alpha);
if (isFirst)
{
qFused = qAcc;
isFirst = false;
}
qCurrent = qFused;
addMagneto(qCurrent, mag, alpha, mag_shift);
float up;
Vec3 pry = quaternionToPitchRollYaw(qCurrent, up);
Vec3 ine = backRotateVectorByQuaternion(qCurrent, aB);
return
{
sqrtf(pry.x * pry.x + pry.y * pry.y) * up,
pry.x, pry.y, pry.z,
ine.x, ine.y, ine.z
};
}
float calculateHeight(float bar, float temp)
{
static double firstBar = bar;
return (R * (temp + 273) / (M * g)) * log(firstBar / bar);
}

15
DroneClientCpp/Orientation.h Normal file
View File

@ -0,0 +1,15 @@
#pragma once
struct Vec3 {
float x, y, z;
};
struct ORI
{
float Tilt; // Earth's plane tilt
float Pitch, Roll, Yaw; // Sovereign orientation (not Euler)
float IneX, IneY, IneZ; // Inertial accelerations
};
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha);
float calculateHeight(float bar, float temp);

156
DroneClientCpp/WsServer.h Normal file
View File

@ -0,0 +1,156 @@
#pragma once
#include <Windows.h>
#using <System.dll>
#using <System.Net.Http.dll>
#pragma pack(push, 1)
struct angles_native { float pitch, roll, yaw; };
#pragma pack(pop)
using namespace System;
using namespace System::Collections::Generic;
using namespace System::Net;
using namespace System::Net::WebSockets;
using namespace System::Threading;
using namespace System::Text;
using namespace DroneClient; // ← поправьте, если у вас другое
public ref class WsServer // имя можно оставить тем же
{
public:
WsServer() : _connected(false) {}
/* ---------- подключение --------------------------------------- */
bool Connect(String^ uri)
{
if (_connected) return true; // уже подключены
_ws = gcnew ClientWebSocket();
_cts = gcnew CancellationTokenSource();
try
{
// 1) запускаем асинхронное соединение
System::Threading::Tasks::Task^ t =
_ws->ConnectAsync(gcnew Uri(uri), _cts->Token);
// 2) ждём завершения
t->Wait(); // здесь AggregateException «обёртывает»
// все реальные ошибки
_connected = true;
}
catch (AggregateException^ ag)
{
// сообщений может быть несколько – выводим главное
Exception^ ex = ag->InnerExceptions->Count
? ag->InnerExceptions[0] : ag;
System::String^ msg = ex->Message;
// типовые причины:
// • WebSocketException (ошибка DNS / connection refused / timeout)
// • NotSupportedException (Windows 7: платформа без WebSocket‑клиента)
// • InvalidOperationException (неверный URI)
// → выводим в Debug и просто возвращаем false
System::Diagnostics::Debug::WriteLine("Connect error: " + msg);
_connected = false;
}
catch (Exception^ ex) // на всякий случай – «прочие»
{
System::Diagnostics::Debug::WriteLine("Connect error: " + ex->Message);
_connected = false;
}
return _connected;
}
/* ---------- отключение ---------------------------------------- */
void Disconnect()
{
if (!_connected) return;
try
{
_ws->CloseAsync(WebSocketCloseStatus::NormalClosure,
"bye", CancellationToken::None)->Wait();
}
catch (Exception^) { /* игнор */ }
_ws = nullptr;
_cts = nullptr;
_connected = false;
}
/* ---------- быстрая отправка строки --------------------------- */
bool SendString(String^ msg)
{
if (!_connected) return false;
array<Byte>^ bytes = Encoding::UTF8->GetBytes(msg);
try
{
_ws->SendAsync(ArraySegment<Byte>(bytes),
WebSocketMessageType::Text,
true, CancellationToken::None)->Wait();
return true;
}
catch (Exception^) { return false; }
}
/* ---------- состояние ---------------------------------------- */
property bool IsConnected
{
bool get() { return _connected; }
}
void SendAnglesBinary(float p, float r, float y)
{
angles_native a = { p, r, y };
// безопасно копируем struct → byte[]
array<System::Byte>^ buf = gcnew array<System::Byte>(sizeof(a));
System::Runtime::InteropServices::GCHandle h =
System::Runtime::InteropServices::GCHandle::Alloc(buf,
System::Runtime::InteropServices::GCHandleType::Pinned);
memcpy(h.AddrOfPinnedObject().ToPointer(), &a, sizeof(a));
h.Free();
// шлём как Binary
_ws->SendAsync(System::ArraySegment<Byte>(buf),
System::Net::WebSockets::WebSocketMessageType::Binary,
true, System::Threading::CancellationToken::None)->Wait();
}
void TxLoop(System::Object^ param)
{
Drone^ d = safe_cast<Drone^>(param);
// 20 мс = 50 Гц
const int PERIOD_MS = 20;
// примерные значения, чтобы «шевелились»
float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
while (_runTx && _connected)
{
float p = System::Threading::Volatile::Read(d->pitch);
float r = System::Threading::Volatile::Read(d->roll);
float y = System::Threading::Volatile::Read(d->yaw);
SendAnglesBinary(p,r, y);
System::Threading::Thread::Sleep(PERIOD_MS);
}
}
private:
ClientWebSocket^ _ws;
CancellationTokenSource^ _cts;
bool _connected;
public:
// --- рядом с тем, где уже лежит wsClient ---
System::Threading::Thread^ _txThread = nullptr; // поток передатчик
bool _runTx = false; // признак «живого» цикла
};

137
DroneClientCpp/joypad.h Normal file
View File

@ -0,0 +1,137 @@
// joypad.h ───────────────────────────────────────────────────────────
#pragma once
#include <iostream>
#include <iomanip>
#include <Windows.h> // <- для AllocConsole
#using <System.dll>
using namespace System;
using namespace System::IO::Ports;
using namespace System::Threading;
/*---------------------------------------------------------------------
Joypad (i-Bus → COM-порт)
---------------------------------------------------------------------*/
public ref class Joypad
{
public:
/* событие – _не подписывайтесь_ ➜ ничего в форме не вызовется */
delegate void TickHandler(array<UInt16>^ ch);
event TickHandler^ TickEvent;
Joypad()
{
// -------- открываем консоль один раз ----------------------
static bool consoleReady = false;
if (!consoleReady && ::AllocConsole())
{
FILE* fp;
freopen_s(&fp, "CONOUT$", "w", stdout);
std::cout << " CH1 CH2 CH3 CH4 CH5 CH6\n";
consoleReady = true;
}
// ----------------------------------------------------------
_sp = gcnew SerialPort();
_sp->ReadTimeout = 200;
_sp->ReadBufferSize = 256;
}
/* ---------- запуск ------------------------------------------- */
bool Start(String^ port, int baud )
{
if (_run) return true;
try
{
_sp->PortName = port;
_sp->BaudRate = baud;
_sp->Parity = Parity::None;
_sp->DataBits = 8;
_sp->StopBits = StopBits::One;
_sp->Open();
}
catch (Exception^ ex)
{
System::Diagnostics::Debug::WriteLine("Serial error: " + ex->Message);
return false;
}
_run = true;
_thr = gcnew Thread(gcnew ThreadStart(this, &Joypad::RxLoop));
_thr->IsBackground = true;
_thr->Start();
return true;
}
/* ---------- остановка ---------------------------------------- */
void Stop()
{
_run = false;
if (_thr && _thr->IsAlive) _thr->Join();
if (_sp->IsOpen) _sp->Close();
}
property array<UInt16>^ Channels { array<UInt16>^ get() { return _ch; } }
private:
// ----------- поток приёма i-Bus ---------------------------------
void RxLoop()
{
array<Byte>^ buf = gcnew array<Byte>(64);
array<Byte>^ pkt = gcnew array<Byte>(32);
while (_run)
{
int read = 0;
try { read = _sp->Read(buf, 0, buf->Length); }
catch (TimeoutException^) { continue; }
for (int i = 0; i < read; ++i)
{
_fifo[_head++] = buf[i];
if (_head >= _fifo->Length) _head = 0;
if (_fifo[_head] == 0x20 && _fifo[(_head + 1) & 0x7F] == 0x40)
{
for (int j = 0; j < 32; ++j)
pkt[j] = _fifo[(_head + j) & 0x7F];
if (!CheckPkt(pkt)) continue;
ParseChannels(pkt);
// -------- 1) печать в консоль ----------------
std::cout << '\r';
for (int k = 0; k < 6; ++k)
std::cout << std::setw(6) << _ch[k] << ' ';
std::cout << std::flush;
// ---------------------------------------------
// -------- 2) необязательный вызов события ----
TickEvent(_ch); // raise
}
}
}
}
bool CheckPkt(array<Byte>^ p)
{
UInt16 sum = 0; for (int i = 0; i < 30; ++i) sum += p[i];
sum = 0xFFFF - sum;
return sum == (p[30] | p[31] << 8);
}
void ParseChannels(array<Byte>^ p)
{
for (int i = 0; i < 14; ++i)
_ch[i] = (UInt16)(p[2 + i * 2] | p[3 + i * 2] << 8);
}
/* ---------- поля --------------------------------------------- */
SerialPort^ _sp;
Thread^ _thr;
bool _run = false;
array<UInt16>^ _ch = gcnew array<UInt16>(14);
array<Byte>^ _fifo = gcnew array<Byte>(128);
int _head = 0;
};