diff --git a/DroneClientCpp/Drone.cpp b/DroneClientCpp/Drone.cpp index 7c24378..6774daa 100644 --- a/DroneClientCpp/Drone.cpp +++ b/DroneClientCpp/Drone.cpp @@ -5,7 +5,7 @@ namespace DroneClient { // Реализация статического метода GetBytes array^ Drone::GetBytes(Object^ data) { - int size = Marshal::SizeOf(data); + int size = Marshal::SizeOf(data); array^ arr = gcnew array(size); IntPtr ptr = IntPtr::Zero; @@ -45,12 +45,14 @@ namespace DroneClient { // Реализация приватного метода SendDataMotor4 array^ 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^ 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^ 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; + + + } } \ No newline at end of file diff --git a/DroneClientCpp/Drone.h b/DroneClientCpp/Drone.h index 4f1e8df..65aa718 100644 --- a/DroneClientCpp/Drone.h +++ b/DroneClientCpp/Drone.h @@ -3,10 +3,14 @@ #include "DroneData.h" #include #include +#include "Orientation.h" +#include "joypad.h" + #using #using + 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^ GetBytes(Object^ data); static Object^ FromBytes(array^ arr, Type^ type); @@ -37,6 +47,9 @@ namespace DroneClient { array^ RecvDataLocal(array^ data); array^ ClientRequestResponse(DroneData::DataHead head, array^ body); + void setMotors(); + + literal int DroneStreamCount = 512; array^ DroneStreamData; int DroneStreamIndex; @@ -47,5 +60,8 @@ namespace DroneClient { System::Collections::Generic::List^>^ DataStream(array^ data, int size); array^ SendRequest(); + void updateData(); + + }; } \ No newline at end of file diff --git a/DroneClientCpp/DroneClientCpp.vcxproj b/DroneClientCpp/DroneClientCpp.vcxproj index 4588522..28cad81 100644 --- a/DroneClientCpp/DroneClientCpp.vcxproj +++ b/DroneClientCpp/DroneClientCpp.vcxproj @@ -121,6 +121,7 @@ + @@ -128,7 +129,10 @@ CppForm + + + diff --git a/DroneClientCpp/DroneClientCpp.vcxproj.filters b/DroneClientCpp/DroneClientCpp.vcxproj.filters index 6c37b72..a328c1f 100644 --- a/DroneClientCpp/DroneClientCpp.vcxproj.filters +++ b/DroneClientCpp/DroneClientCpp.vcxproj.filters @@ -24,6 +24,9 @@ Исходные файлы + + Исходные файлы + @@ -38,5 +41,14 @@ Файлы заголовков + + Файлы заголовков + + + Файлы заголовков + + + Файлы заголовков + \ No newline at end of file diff --git a/DroneClientCpp/DroneData.h b/DroneClientCpp/DroneData.h index 5998a61..f2c736e 100644 --- a/DroneClientCpp/DroneData.h +++ b/DroneClientCpp/DroneData.h @@ -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); }; diff --git a/DroneClientCpp/FormMain.cpp b/DroneClientCpp/FormMain.cpp index dfbdfbc..7fa4bbe 100644 --- a/DroneClientCpp/FormMain.cpp +++ b/DroneClientCpp/FormMain.cpp @@ -9,4 +9,5 @@ int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) { Application::SetCompatibleTextRenderingDefault(false); Application::Run(gcnew FormMain); return 0; -} \ No newline at end of file +} + diff --git a/DroneClientCpp/FormMain.h b/DroneClientCpp/FormMain.h index 12ac3c2..f0a3974 100644 Binary files a/DroneClientCpp/FormMain.h and b/DroneClientCpp/FormMain.h differ diff --git a/DroneClientCpp/FormMain.resx b/DroneClientCpp/FormMain.resx index 27962ab..1b4562e 100644 --- a/DroneClientCpp/FormMain.resx +++ b/DroneClientCpp/FormMain.resx @@ -123,4 +123,7 @@ 310, 17 + + 25 + \ No newline at end of file diff --git a/DroneClientCpp/Orientation.cpp b/DroneClientCpp/Orientation.cpp new file mode 100644 index 0000000..140b6a8 --- /dev/null +++ b/DroneClientCpp/Orientation.cpp @@ -0,0 +1,254 @@ +#include "orientation.h" +#include + +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); + +} \ No newline at end of file diff --git a/DroneClientCpp/Orientation.h b/DroneClientCpp/Orientation.h new file mode 100644 index 0000000..fb5dbad --- /dev/null +++ b/DroneClientCpp/Orientation.h @@ -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); \ No newline at end of file diff --git a/DroneClientCpp/WsServer.h b/DroneClientCpp/WsServer.h new file mode 100644 index 0000000..a625b63 --- /dev/null +++ b/DroneClientCpp/WsServer.h @@ -0,0 +1,156 @@ +#pragma once +#include +#using +#using + +#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^ bytes = Encoding::UTF8->GetBytes(msg); + try + { + _ws->SendAsync(ArraySegment(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^ buf = gcnew array(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(buf), + System::Net::WebSockets::WebSocketMessageType::Binary, + true, System::Threading::CancellationToken::None)->Wait(); + } + + void TxLoop(System::Object^ param) + { + Drone^ d = safe_cast(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; // признак «живого» цикла +}; \ No newline at end of file diff --git a/DroneClientCpp/joypad.h b/DroneClientCpp/joypad.h new file mode 100644 index 0000000..a916550 --- /dev/null +++ b/DroneClientCpp/joypad.h @@ -0,0 +1,137 @@ +// joypad.h ─────────────────────────────────────────────────────────── +#pragma once +#include +#include +#include // <- для AllocConsole +#using + +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^ 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^ Channels { array^ get() { return _ch; } } + +private: + // ----------- поток приёма i-Bus --------------------------------- + void RxLoop() + { + array^ buf = gcnew array(64); + array^ pkt = gcnew array(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^ 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^ 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^ _ch = gcnew array(14); + array^ _fifo = gcnew array(128); + int _head = 0; +};