forked from CPL/Simulator
360 lines
11 KiB
C++
360 lines
11 KiB
C++
#include "Drone.h"
|
||
|
||
namespace DroneClient {
|
||
|
||
// Реализация статического метода GetBytes
|
||
array<Byte>^ Drone::GetBytes(Object^ data)
|
||
{
|
||
int size = Marshal::SizeOf(data);
|
||
array<Byte>^ arr = gcnew array<Byte>(size);
|
||
|
||
IntPtr ptr = IntPtr::Zero;
|
||
try
|
||
{
|
||
ptr = Marshal::AllocHGlobal(size);
|
||
Marshal::StructureToPtr(data, ptr, true);
|
||
Marshal::Copy(ptr, arr, 0, size);
|
||
}
|
||
finally
|
||
{
|
||
Marshal::FreeHGlobal(ptr);
|
||
}
|
||
return arr;
|
||
}
|
||
|
||
// Реализация статического метода FromBytes
|
||
Object^ Drone::FromBytes(array<Byte>^ arr, Type^ type)
|
||
{
|
||
Object^ mem = gcnew Object();
|
||
|
||
int size = Marshal::SizeOf(type);
|
||
IntPtr ptr = IntPtr::Zero;
|
||
try
|
||
{
|
||
ptr = Marshal::AllocHGlobal(size);
|
||
Marshal::Copy(arr, 0, ptr, size);
|
||
mem = Marshal::PtrToStructure(ptr, type);
|
||
}
|
||
finally
|
||
{
|
||
Marshal::FreeHGlobal(ptr);
|
||
}
|
||
return mem;
|
||
}
|
||
|
||
// Реализация приватного метода 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;
|
||
mot4.DR = MotorDR;
|
||
|
||
return GetBytes(mot4);
|
||
}
|
||
|
||
array<Byte>^ Drone::RecvDataAcc(array<Byte>^ data)
|
||
{
|
||
DroneData::DataAcc imu = (DroneData::DataAcc)FromBytes(data, DroneData::DataAcc::typeid);
|
||
|
||
AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
|
||
TimeAcc = imu.Time;
|
||
|
||
return gcnew array<Byte>(0);
|
||
}
|
||
|
||
array<Byte>^ Drone::RecvDataGyr(array<Byte>^ data)
|
||
{
|
||
DroneData::DataGyr imu = (DroneData::DataGyr)FromBytes(data, DroneData::DataGyr::typeid);
|
||
|
||
GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;
|
||
TimeGyr = imu.Time;
|
||
|
||
return gcnew array<Byte>(0);
|
||
}
|
||
|
||
array<Byte>^ Drone::RecvDataRange(array<Byte>^ data)
|
||
{
|
||
DroneData::DataRange pos = (DroneData::DataRange)FromBytes(data, DroneData::DataRange::typeid);
|
||
|
||
LaserRange = pos.LiDAR;
|
||
TimeRange = pos.Time;
|
||
|
||
return gcnew array<Byte>(0);
|
||
}
|
||
|
||
array<Byte>^ Drone::RecvDataLocal(array<Byte>^ data)
|
||
{
|
||
DroneData::DataLocal pos = (DroneData::DataLocal)FromBytes(data, DroneData::DataLocal::typeid);
|
||
|
||
PosX = pos.Local.X; PosY = pos.Local.Y;
|
||
|
||
return gcnew array<Byte>(0);
|
||
}
|
||
|
||
array<Byte>^ Drone::ClientRequestResponse(DroneData::DataHead head, array<Byte>^ body)
|
||
{
|
||
array<Byte>^ zero = gcnew array<Byte>(0);
|
||
|
||
switch (head.Type)
|
||
{
|
||
case DroneData::DataType::DataAcc:
|
||
if (head.Mode == DroneData::DataMode::Request)
|
||
{
|
||
return zero; // Запрос данных (не реализовано)
|
||
}
|
||
else
|
||
{
|
||
return RecvDataAcc(body); // Пришли данные
|
||
}
|
||
|
||
case DroneData::DataType::DataGyr:
|
||
if (head.Mode == DroneData::DataMode::Request)
|
||
{
|
||
return zero; // Запрос данных (не реализовано)
|
||
}
|
||
else
|
||
{
|
||
return RecvDataGyr(body); // Пришли данные
|
||
}
|
||
|
||
case DroneData::DataType::DataRange:
|
||
if (head.Mode == DroneData::DataMode::Request)
|
||
{
|
||
return zero; // Запрос данных (не реализовано)
|
||
}
|
||
else
|
||
{
|
||
return RecvDataRange(body); // Пришли данные
|
||
}
|
||
|
||
case DroneData::DataType::DataLocal:
|
||
if (head.Mode == DroneData::DataMode::Request)
|
||
{
|
||
return zero; // Запрос данных (не реализовано)
|
||
}
|
||
else
|
||
{
|
||
return RecvDataLocal(body); // Пришли данные
|
||
}
|
||
|
||
case DroneData::DataType::DataMotor4:
|
||
if (head.Mode == DroneData::DataMode::Request)
|
||
{
|
||
return SendDataMotor4(); // Запрос данных
|
||
}
|
||
else
|
||
{
|
||
return zero; // Пришли данные (не реализовано)
|
||
}
|
||
}
|
||
|
||
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()
|
||
{
|
||
DroneStreamData = gcnew array<Byte>(DroneStreamCount); // Инициализация массива
|
||
DroneStreamIndex = 0;
|
||
DroneStreamHead.Mode = DroneData::DataMode::None;
|
||
DroneStreamHead.Size = 0;
|
||
DroneStreamHead.Type = DroneData::DataType::None;
|
||
|
||
this->joy = gcnew Joypad();
|
||
|
||
this->joy->Start("COM7", 115200);
|
||
|
||
}
|
||
|
||
// Реализация метода DataStream
|
||
List<array<Byte>^>^ Drone::DataStream(array<Byte>^ data, int size)
|
||
{
|
||
System::Collections::Generic::List<array<Byte>^>^ ret = gcnew System::Collections::Generic::List<array<Byte>^>();
|
||
|
||
if (data == nullptr) return ret; // Последовательность не сформирована
|
||
|
||
if (size + DroneStreamIndex > DroneStreamCount) return nullptr; // Ошибка переполнения
|
||
|
||
Array::Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
|
||
DroneStreamIndex += size;
|
||
|
||
while (true)
|
||
{
|
||
if (DroneStreamHead.Size == 0) // Заголовок еще не получен
|
||
{
|
||
if (DroneStreamIndex < DroneData::DataHead::StrLen) return ret;
|
||
DroneStreamHead = (DroneData::DataHead)FromBytes(DroneStreamData, DroneData::DataHead::typeid);
|
||
}
|
||
|
||
if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет еще не полный
|
||
|
||
array<Byte>^ body = gcnew array<Byte>(DroneStreamHead.Size);
|
||
Array::Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);
|
||
|
||
int shift = DroneStreamHead.Size;
|
||
|
||
DroneStreamIndex -= shift;
|
||
Array::Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвиг массива
|
||
|
||
DroneStreamHead.Size = 0; // Сброс заголовка
|
||
|
||
ret->Add(ClientRequestResponse(DroneStreamHead, body));
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
|
||
// Реализация метода SendRequest
|
||
array<Byte>^ Drone::SendRequest()
|
||
{
|
||
DroneData::DataHead^ acc = gcnew DroneData::DataHead();
|
||
acc->Size = DroneData::DataHead::StrLen;
|
||
acc->Mode = DroneData::DataMode::Request;
|
||
acc->Type = DroneData::DataType::DataAcc;
|
||
|
||
DroneData::DataHead^ gyr = gcnew DroneData::DataHead();
|
||
gyr->Size = DroneData::DataHead::StrLen;
|
||
gyr->Mode = DroneData::DataMode::Request;
|
||
gyr->Type = DroneData::DataType::DataGyr;
|
||
|
||
DroneData::DataHead^ range = gcnew DroneData::DataHead();
|
||
range->Size = DroneData::DataHead::StrLen;
|
||
range->Mode = DroneData::DataMode::Request;
|
||
range->Type = DroneData::DataType::DataRange;
|
||
|
||
DroneData::DataHead^ local = gcnew DroneData::DataHead();
|
||
local->Size = DroneData::DataHead::StrLen;
|
||
local->Mode = DroneData::DataMode::Request;
|
||
local->Type = DroneData::DataType::DataLocal;
|
||
|
||
List<array<byte>^>^ list = gcnew List<array<byte>^>();
|
||
|
||
list->Add(GetBytes(acc));
|
||
list->Add(GetBytes(gyr));
|
||
list->Add(GetBytes(range));
|
||
list->Add(GetBytes(local));
|
||
list->Add(SendDataMotor4());
|
||
|
||
int count = 0;
|
||
|
||
for each(array<byte>^ d in list) count += d->Length;
|
||
|
||
array<byte>^ send = gcnew array<byte>(count);
|
||
|
||
count = 0;
|
||
for each (array<byte> ^ d in list)
|
||
{
|
||
d->CopyTo(send, count);
|
||
count += d->Length;
|
||
}
|
||
|
||
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;
|
||
|
||
|
||
}
|
||
} |