From 27a120fc7f88da782340b0c6a9ffffc822afb353 Mon Sep 17 00:00:00 2001 From: Dana Markova Date: Wed, 30 Apr 2025 12:16:18 +0300 Subject: [PATCH] edit proshivka --- DroneClientCpp/Drone.cpp | 109 +++++++- DroneClientCpp/Drone.h | 16 ++ DroneClientCpp/DroneClientCpp.vcxproj | 4 + DroneClientCpp/DroneClientCpp.vcxproj.filters | 12 + DroneClientCpp/DroneData.h | 2 +- DroneClientCpp/FormMain.cpp | 3 +- DroneClientCpp/FormMain.h | Bin 80010 -> 93404 bytes DroneClientCpp/FormMain.resx | 3 + DroneClientCpp/Orientation.cpp | 254 ++++++++++++++++++ DroneClientCpp/Orientation.h | 15 ++ DroneClientCpp/WsServer.h | 156 +++++++++++ DroneClientCpp/joypad.h | 137 ++++++++++ 12 files changed, 707 insertions(+), 4 deletions(-) create mode 100644 DroneClientCpp/Orientation.cpp create mode 100644 DroneClientCpp/Orientation.h create mode 100644 DroneClientCpp/WsServer.h create mode 100644 DroneClientCpp/joypad.h 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 12ac3c2ab9fc658db9c834a56fb7916fb46a31f2..f0a397457421800a87950c72205a9236d8a7334c 100644 GIT binary patch delta 6148 zcma)A3s98T75*>#vnu3HyYFN3zq> zN`)5uG$Ns1*e&e9uSwV{ECiN68+?n^c?MxsfKnhB5Uv%$>T!01upzkSNcOa(HMnpk z4z>ypQ!B@?r&KV;%*m?|OIl)*05wAfmqR;U&d1g-q2SQZ7Nc^cIo zVK;mnJw1C8&%!KWwlJTj(pMTzVbdku9Hx{1s6I6n4rF@ZTHFj~f#%FQC@qVJF})7| zU5d_zLs^RvSUqzx;Chr8ZYER&l+c7Hnh(aEnTFfWBqSS3#QCC2bchQ?k4PJv;Cix) z*>JyY%*tEZdmAg}t%J9A+COF^%A#3lgyFoYNgHs-W|YW|fVA1N>B##!=A!vY&d>qr zkkFqs4a_^^<)N%aTF~9q1n-l3ZwCdI0oqt(y4%_0p0AhhW1GS-M5AKyq^t z&6K;VYoeg#hFWD6is8uef3SJ5YrqYLgCbeHWo)KXtb#MRPF}leh6(`{0s*?e(*W1j)J6b1*;fR*`-)}X z+R>=kc>x}*5F8%6uOuoqh=k(nE&}iR_B-Hs*Uy0$zet69Hk^&KB4ZAPF%E__+3WqB zkr+_g$<6b7E{M>*Sp&XxDKPS~4*MdsY&w7j+kbfp{?p3ko2#SaTpWa-6D->+VPsn* znVjr%L(BG|h>W?RW5=Rsi(p2L4FAgsgN-?IR$~DJZ>COO_l=&#IVbjT!EnJmG`UoG zWP6Hqm@z&0{&N^Sk9nZw=7=<8WmcIS54zxV30mJ@dXr5$9-u4VA??{F9$6d0weSm@C$7)q@Gw%W2#1ij;csP8* z1>KLX!jM2qyB|#l-wS#$Hx|ep9hdn|p!S-<|M-l+GGAa9vn3UV9v5+X4Fvk~(4p&M zk_C-Vk%pqC@h2*ezRB*SCi9b8@OQp`2UQVu@CPNTd1$qyEdTei75J7WV}RTwg)(I^%#bw;r_q(M6>?fXBRq%^m~4Wr#^=zf~?;DPeP ziE?N6)mxS2c2t1BM+7e@lXN|a^15fPhLof`mH}o-A&wqRl;<4#GNdfp!tmS`MSQaP zcvjSeV%&~p;Oh`k!ufFJ1SzE}ppfg&PbdUxefi)&u9L6%x0YBngJ`MDt^V=`&C%MF8mHN5q5 zAN=BSl|1Ml2~8j)rU~6=)o@%^CUl=22~)s81~i@{LN#1GuZDtS)jbz9;61OAu2~pf zOy?ERaql6dkyjLV7=2ZP{dnkor3>uOdU`G>Vn-LE?$DtW`Npq0L!LxShc1Se4qfD) z)HA3!QY9w_9qIaDm?IfpQygjZRmG8(g7eU@P7 z(a+3VJ6L{F1}(oojRIXvGvuv9jSP*+Dg*;@G3tyisE$90aH{v-2)xh*Mbz_ucmYG} z#I_TwZoaLSPrv<%Y9h81WZj?dhL?IHT~U^+ABV%p#&_q62_cT~V37Rb0(kcQwUK%0 zd0zt$Up4f+FT%Ye?cq?gPR{sXOckpfPJuca83u+v(!i%5`X&KX$X7r5n+ju~IdD=N z_FQoA+MSM5|My#uQ}-zzhkP`sGUXmOk7?k|u}FqM;w7s0vs-x~rQo<+bNz-20m%v| zF>s-9hYJdOzBG`wBb*KIdL+u8FSO0NR*6FO-9cXc2eVq2V;UX3qW zfboXT`LQ2kr6!3JLLTlhu-$G>tcurFj-A|B8LOBfii0Ol zfedVU8XR6qx!A$x!T{!_&)jjLRLdMT`;tEs2H!3S%=@hR0bKW&?lkDyP6*?^mB@eH z&ehoV?_7f|?utlgf?Qfoo~=+yh)%NHi0#2>Xy=G_j%c5RsN8@~ zoI{9qj)*D!1fm^|oUOz0AflZk+BhQSS$KY1;QW-Yk+p=*Z{wIYj%nkVm_+0GZBT!{ z4qL>G-iJ@QJ+EBN6UjS{=i&Y1%GVOqmQ020Sfka21b;} zTQfc+D*_`8a8P3eUA+~rMUtqMbBl>7muQgNphT^lS1ae$O1xqg<-+`?4A+si)*mm% z9&#MhN)H`;>hy133W?8axn+~KEgYZQAZjd#ACxa9jwoNUKPR??Zwuh#Kg7rEiCY7= zwAdDWIhkVqOrBYsuPzu~bRf=i%tCt9jTCzl(iooH2K7kO3v-R+iF6 z9ft})`G<#@qP$_7P-L5ML;hA&K)|?q4+8#l^?qzE_pYX+V%AN9bn=6un)fY@3&tF) zfTrC&4{3BvCqJe$%#5&)dmk8bAuVW1cvYcsx;UnbW4Z#EN-U+P4q;-7hL{)-QFNI7 zPM9Y4{h2CqbY^lOC5Y=n+)Ak_hMmR1T>}njt%iLP_O2>cLW5+AW3yoJK`nDhERL<_ zT_<%L;PZVZBA4KGsXccwmf53jk9z0EO35(IZE5HIh}&@}b3;BUPob63M4$20m#}&surs%ru6XNVJCN z8~MoTG1it~!;loD1U*R^$6y**p~9FT)n&3w z$z)~uWNraM(q^>N-B?WtIES(jO(#H4Q0OZZ2PLY53Ng|JC%Y__n^-w&s9_4*1{-Il zLwSs5w(+*w2!)?YUN52APS7tJpg!73Q1?t1mtt+ij5U%{(Jd|)vBo1V6zw8aPHMy@ zSUvHG)d5NgQfrVG;wstT#x>=lmA4gQPtFV~0!hYHW-QF*Y?NSitOz3u6`AVscPsXG zqf?gP{fqekev5=$VK&qo(?~Zk7L!jb-Cc+=GyG!^+?>U7Y!&uA$YKdHZMbD#_?ki( zj>#s}>642^B#;eQ%y|ffu}atrClBJSo2Qu>d0rb<`N9foFdjVxeB-*jurZ}56ezZZ z3UjkK2fBQngr*)(iZqF7ifs@$-7%SEx~%er zH7Mk;vSEO+y;!lMidGBM7v|A{3n#Bu1T*Qt{Q5%sfelGh;ncwbT6x>PcU~FBwbY-< zrZ}h?Xy6~R4TDh1HAq?|V{)M3nZ-IZMT4*i4@)N7Agt$-^wuZdW*N6={LZ#;D}@*6 zNG})*==$k5PMt<|)ZJ*N;Xb0=9S56@bmrjXq}*)ysw!71Psij2$1!~5#5ACB?-m-6 z%?cPzHcJ?-W)x-PX$(RIKhb=&S2w!Rot-Nevmhga>FFB=I2FAj=l|L;TUJEw-|+2J zN=A~8vj29og8@XzP_Pos{T1cs{PydsP_R^}=GEX}IoVi@vQc#HM6D}<5>!#cijo`z^;#Ouz3^=$HW;zVx)}+r7L8Ad z^!n2-5vJ1ps$3x#ua~Zoe@9D?%fDM}?egyx+gd%73x|bv;itl026-d4kNPNe6b<)WkFJd%e*#0#}By1V7IiHex=1d({* zyYzs6SVfQQD9}hdRU{uBXY!HUVZhB@R(Sk2JbKHecM26kv0xRdglgQ`UMI^#&1(AK zM~yiv%4Ly5x}zth2)$T<=DDVo4tdk2h_Gj}4#C|Qa+O`XP=B|KLLs5NJsx2c94DV~k*o~6( z9~}+;WHLupDcH~(b}GBaI#d`+8UpOla)P zZwCiqJH-g?F{Aj6V%pOlBPp+)_(y9gl3r6}ry5Oj&5!x*Qogf7$xS@8Dobt{dysfVi?+~EZrOsF2P?faaD0mA zN5Ls$p*mRUd!nO^6t5W(er+j$kDfd+NmL8~_B}qterd>Jf$5|Xhhv4Ptr9VL^3@D0 zMta$~lWIErtJ!qaQ~o4QJ)H%O`{RGz6#0E24&<3(>wYs=p`t=PFBhU`#6pYTctBEj zrJW1*7_*|9(ec*U5-1-H_T=Zl(OD0FBpdaagqvb?nX$Sz!xTEwJD|~5rzp)iaA$i1 zViG5z2i7JLyU!>bur?VmG2>b&AcX;^oy0&*Z#NIozm?pY?=!Kk&EOTFy?`J`X3^RaBi-(cc zW^|FPYe@x?wH8JLH4P5;5%fp5B7WL{9Lc=|kQg(%n@dhm_##hzk?e%i01ZSD()$3G*j% zEej69e=~EC;ZMxCeJ!J)S!b6|EpR37levQ6a2v~ljz<5!o}ekd75)#hbH)c#;(7)- zcxV-UEy5$)5ZGj9d^;TZRHTL1zgFv&5#kVRtUc%5I7`QFtR}33G@|x17aK_B?8Aq* zL#TrH2j0l1MojFt;Mz2sX#cd*ikn9WyG&{O4tQtn2z_qB;ADSB6J`^OhClBl-{QQq zrzZq!Sb4X8&Bruso7M2&Qe*B`ROMtBv6&h9!b0!f{#uo?kHwhlmu*D+D;=y~$>W`^ z2h;jAIz6wFXN__}8MBr>DDGMC;4WAGGOZ}@?K^J4)Vs`udlou*Kdf4zL_s+eo3U_G zvqQCr&u9^tS+EM>hXvICa9X9WVgXa);#6+>#)4C`idmsq9cszs@$s7 z5LWK-%)KOgZ~C7O^K*;po;eG>|KB+k>+Qd;o0x~wQZ_CCU4Evd%ZkDg-^F-zMC z9bw}CO_IiA>zB_nTMrH3o+V~p_S)rs}0krmI1LRLgF1`59a!_lZ0axFhV4!A< z9StCH(Fp&aW2q++$9+DFksOn2@49$tiJ{*|$4BE*>jv`2#R6s2c~;t1Or{y7)9=x8 z&iIy6siTy*SUHUTtAv#2@Ys%Svy*PE^mY{~#!DIlJWuKPOI#|dCM8n+7UDu+r4j$x zYmmNdA-kmT4x-i{6;=u&s}uH-W@<>Ow97@dO2aPlJ!#%W0+PW^TBV7N#LZWyr6D(2 zce5ETImZvsU3ajAr%@VL=#RNHA@}!WZh`%X0oaPZ5kc($Ov0ezBC;mF=Lmo KQ8vCy{{BB)b_}op 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; +};