first commit
This commit is contained in:
Dana Markova
67
utils/filt.cpp
Normal file
67
utils/filt.cpp
Normal file
@ -0,0 +1,67 @@
|
||||
#include <math.h>
|
||||
|
||||
#include "filt.h"
|
||||
|
||||
float FilterGPS::Limit(float Value, float Min, float Max)
|
||||
{
|
||||
if (Value > Max) return Max;
|
||||
if (Value < Min) return Min;
|
||||
return Value;
|
||||
}
|
||||
|
||||
float FilterGPS::Minimum(float Value1, float Value2)
|
||||
{
|
||||
if (Value1 < Value2) return Value1;
|
||||
return Value2;
|
||||
}
|
||||
|
||||
void FilterGPS::Update(bool valid, float gps, float acc)
|
||||
{
|
||||
const float ga = 9.80665f;
|
||||
|
||||
Delay.Acc[Delay.Index] = acc;
|
||||
Delay.Pos[Delay.Index] = Position.Value;
|
||||
Delay.Spd[Delay.Index++] = Speed.Value;
|
||||
if (Delay.Index >= Delay.Size) Delay.Index = 0;
|
||||
|
||||
float prev = Delay.Acc[Delay.Index];
|
||||
float shift = gps - Delay.Pos[Delay.Index];
|
||||
|
||||
float coef_a = 1;
|
||||
if (Position.EnAcc) coef_a = Position.Acc / fabsf(prev);
|
||||
float coef_p = 1;
|
||||
if (Position.EnGPS) coef_p = powf(fabsf(shift * Position.Length), Position.Width);
|
||||
|
||||
float coef1 = Minimum(coef_a, coef_p);
|
||||
coef1 = Limit(coef1, Position.Min, Position.Max);
|
||||
|
||||
Speed.Count++;
|
||||
if (valid)
|
||||
{
|
||||
Speed.ValueGPS = (gps - Speed.Last_GPS) * Freq / Speed.Count;
|
||||
Speed.Last_GPS = gps;
|
||||
Speed.ValueAccGPS= (Speed.ValueGPS- Speed.Speed_GPS) * Freq / Speed.Count;
|
||||
Speed.Speed_GPS = Speed.ValueGPS;
|
||||
Speed.Count = 0;
|
||||
}
|
||||
|
||||
for (int a = 0; a < Delay.Size; a++) Delay.Pos[a] += shift * coef1;
|
||||
Position.Value += shift * coef1 + (Speed.Value / Freq) * (1 - coef1);
|
||||
|
||||
// SPEED
|
||||
|
||||
shift = Speed.ValueGPS - Delay.Spd[Delay.Index];
|
||||
|
||||
coef_a = 1;
|
||||
if (Speed.EnAcc) coef_a = Speed.Acc / fabsf(prev);
|
||||
float coef_g = 1;
|
||||
if (Speed.EnAcc) coef_g = Speed.AccGPS / fabsf(Speed.ValueAccGPS);
|
||||
float coef_s = 1;
|
||||
if (Speed.EnGPS) coef_s = powf(fabsf(shift * Speed.Length), Speed.Width);
|
||||
|
||||
float coef2 = Minimum(coef_a, Minimum(coef_s, coef_g));
|
||||
coef2 = Limit(coef2, Speed.Min, Speed.Max);
|
||||
|
||||
for (int a = 0; a < Delay.Size; a++) Delay.Spd[a] += shift * coef2;
|
||||
Speed.Value += shift * coef2 + (acc * ga / Freq) * (1 - coef2);
|
||||
}
|
||||
56
utils/filt.h
Normal file
56
utils/filt.h
Normal file
@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
class FilterGPS
|
||||
{
|
||||
public:
|
||||
|
||||
float Freq=100;
|
||||
|
||||
struct
|
||||
{
|
||||
float Value=0;
|
||||
//---
|
||||
float Min=0.0001, Max=0.1;
|
||||
//---
|
||||
bool EnAcc=true;
|
||||
float Acc=0.02;
|
||||
//---
|
||||
bool EnGPS=true;
|
||||
float Width=2, Length=1;
|
||||
//---
|
||||
}Position;
|
||||
|
||||
struct
|
||||
{
|
||||
float ValueAccGPS=0;
|
||||
float ValueGPS=0;
|
||||
float Value=0;
|
||||
//---
|
||||
float Min=0.0001, Max=0.1;
|
||||
//---
|
||||
bool EnAcc=true;
|
||||
float Acc=0.02;
|
||||
float AccGPS=0.02;
|
||||
//---
|
||||
bool EnGPS=true;
|
||||
float Width=2, Length=1;
|
||||
//---
|
||||
float Last_GPS=0, Speed_GPS=0;
|
||||
long Count=0;
|
||||
}Speed;
|
||||
|
||||
struct
|
||||
{
|
||||
float Pos[50];
|
||||
float Acc[50];
|
||||
float Spd[50];
|
||||
long Size=30, Index=0;
|
||||
}Delay;
|
||||
|
||||
void Update(bool valid, float gps, float acc);
|
||||
|
||||
private:
|
||||
|
||||
float Limit(float Value, float Min, float Max);
|
||||
float Minimum(float Value1, float Value2);
|
||||
|
||||
};
|
||||
72
utils/med.cpp
Normal file
72
utils/med.cpp
Normal file
@ -0,0 +1,72 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "med.h"
|
||||
|
||||
long MED_Update(long Value, MED_Data16& Data)
|
||||
{
|
||||
Data.Buf[Data.Index++] = Value;
|
||||
if (Data.Index >= Data.Size) Data.Index = 0;
|
||||
|
||||
memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(short));
|
||||
|
||||
short* t = Data.Temp;
|
||||
long s = Data.Size;
|
||||
|
||||
for (long a = 0; a < s; a++)
|
||||
for (long b = a; b < s; b++)
|
||||
{
|
||||
if (t[a] <= t[b]) continue;
|
||||
short v = t[a];
|
||||
t[a] = t[b];
|
||||
t[b] = v;
|
||||
}
|
||||
|
||||
return t[s / 2];
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
long MED_Update(long Value, MED_Data32& Data)
|
||||
{
|
||||
Data.Buf[Data.Index++] = Value;
|
||||
if (Data.Index >= Data.Size) Data.Index = 0;
|
||||
|
||||
memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(long));
|
||||
|
||||
long* t = Data.Temp;
|
||||
long s = Data.Size;
|
||||
|
||||
for (long a = 0; a < s; a++)
|
||||
for (long b = a; b < s; b++)
|
||||
{
|
||||
if (t[a] <= t[b]) continue;
|
||||
long v = t[a];
|
||||
t[a] = t[b];
|
||||
t[b] = v;
|
||||
}
|
||||
|
||||
return t[s / 2];
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
float MED_Update(float Value, MED_DataF32& Data)
|
||||
{
|
||||
Data.Buf[Data.Index++] = Value;
|
||||
if (Data.Index >= Data.Size) Data.Index = 0;
|
||||
|
||||
memcpy(Data.Temp, Data.Buf, Data.Size * sizeof(float));
|
||||
|
||||
float* t = Data.Temp;
|
||||
long s = Data.Size;
|
||||
|
||||
for (long a = 0; a < s; a++)
|
||||
for (long b = a; b < s; b++)
|
||||
{
|
||||
if (t[a] <= t[b]) continue;
|
||||
float v = t[a];
|
||||
t[a] = t[b];
|
||||
t[b] = v;
|
||||
}
|
||||
|
||||
return t[s / 2];
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
23
utils/med.h
Normal file
23
utils/med.h
Normal file
@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
|
||||
struct MED_Data16
|
||||
{
|
||||
short Size, Index;
|
||||
short *Buf, *Temp;
|
||||
};
|
||||
|
||||
struct MED_Data32
|
||||
{
|
||||
long Size, Index;
|
||||
long *Buf, *Temp;
|
||||
};
|
||||
|
||||
struct MED_DataF32
|
||||
{
|
||||
long Size, Index;
|
||||
float *Buf, *Temp;
|
||||
};
|
||||
|
||||
long MED_Update(long Value, MED_Data16& Data);
|
||||
long MED_Update(long Value, MED_Data32& Data);
|
||||
float MED_Update(float Value, MED_DataF32& Data);
|
||||
58
utils/med2.cpp
Normal file
58
utils/med2.cpp
Normal file
@ -0,0 +1,58 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "med2.h"
|
||||
|
||||
void MED2_Init(MED2_Data& Data)
|
||||
{
|
||||
for (unsigned char a = 0; a < Data.Size; a++) { Data.Index[a] = a; Data.Buff[a] = 0; }
|
||||
Data.Last = 0;
|
||||
}
|
||||
|
||||
long MED2_Update(long Value, MED2_Data& Data)
|
||||
{
|
||||
unsigned char* index=Data.Index;
|
||||
unsigned char size=Data.Size;
|
||||
unsigned char& last=Data.Last;
|
||||
long* buff=Data.Buff;
|
||||
//---
|
||||
buff[last] = Value;
|
||||
//---
|
||||
unsigned char id;
|
||||
for (id = 0; id < size; id++) if (index[id] == last) break;
|
||||
//---
|
||||
bool f = (!id || (Value > buff[index[id - 1]]));
|
||||
|
||||
if (f)
|
||||
{
|
||||
for (unsigned char a = id; a < size - 1; a++)
|
||||
{
|
||||
if (buff[index[a + 1]] >= Value) break;
|
||||
unsigned char tmp = index[a];
|
||||
index[a] = index[a + 1];
|
||||
index[a + 1] = tmp;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (unsigned char a = id; a > 0; a--)
|
||||
{
|
||||
if (buff[index[a - 1]] <= Value) break;
|
||||
unsigned char tmp = index[a - 1];
|
||||
index[a - 1] = index[a];
|
||||
index[a] = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
last++;
|
||||
if (last >= size) last = 0;
|
||||
|
||||
if(Data.Smooth>1)
|
||||
{
|
||||
long ret=0;
|
||||
long s = Data.Smooth/2;
|
||||
for(long a=-s; a<s; a++) ret+=buff[index[size/2+a]];
|
||||
return ret/Data.Smooth;
|
||||
}
|
||||
|
||||
return buff[index[size/2]];
|
||||
}
|
||||
12
utils/med2.h
Normal file
12
utils/med2.h
Normal file
@ -0,0 +1,12 @@
|
||||
#pragma once
|
||||
|
||||
struct MED2_Data
|
||||
{
|
||||
unsigned char Size, Last;
|
||||
unsigned char* Index;
|
||||
long* Buff;
|
||||
unsigned char Smooth;
|
||||
};
|
||||
|
||||
void MED2_Init(MED2_Data& Data);
|
||||
long MED2_Update(long Value, MED2_Data& Data);
|
||||
190
utils/mot.cpp
Normal file
190
utils/mot.cpp
Normal file
@ -0,0 +1,190 @@
|
||||
#include "mot.h"
|
||||
|
||||
#include "pwm.h"
|
||||
|
||||
void QUAD_Set(short throt, short x, short y, short z)
|
||||
{
|
||||
// 2 0
|
||||
// 1 3
|
||||
const unsigned char ESC_UL = 2; // CW // M3
|
||||
const unsigned char ESC_UR = 0; // CCW // M1
|
||||
const unsigned char ESC_DL = 1; // CCW // M2
|
||||
const unsigned char ESC_DR = 3; // CW // M4
|
||||
|
||||
|
||||
const unsigned long count = 4;
|
||||
|
||||
short pwm[count] {0,0,0,0};
|
||||
|
||||
pwm[ESC_UL] += x; pwm[ESC_UR] += x;
|
||||
pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
|
||||
|
||||
pwm[ESC_UL] += y; pwm[ESC_UR] -= y;
|
||||
pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
|
||||
|
||||
pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
|
||||
pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
|
||||
|
||||
short min=500, max=-500;
|
||||
for(int a=0; a<count; a++)
|
||||
{
|
||||
if(pwm[a]>500) pwm[a]=500;
|
||||
else if(pwm[a]<-500) pwm[a]=-500;
|
||||
//---
|
||||
if(min>pwm[a]) min=pwm[a];
|
||||
if(max<pwm[a]) max=pwm[a];
|
||||
}
|
||||
|
||||
short up=(throt+max)-1000;
|
||||
short down=(throt+min);
|
||||
if(up>0) throt-=up;
|
||||
if(down<0) throt-=down;
|
||||
|
||||
for(int a=0; a<count; a++) pwm[a] += throt+1000;
|
||||
|
||||
PWM_SetQuad(pwm, 900, 1500);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
//tree drone
|
||||
void QUAD_Set2(short throt, short x, short y, short z)
|
||||
{
|
||||
// 1 2
|
||||
// 3 0
|
||||
const unsigned char ESC_UL = 1; // CW // M3
|
||||
const unsigned char ESC_UR = 2; // CCW // M1
|
||||
const unsigned char ESC_DL = 3; // CCW // M2
|
||||
const unsigned char ESC_DR = 0; // CW // M4
|
||||
|
||||
const unsigned long count = 4;
|
||||
|
||||
short pwm[count] {0,0,0,0};
|
||||
|
||||
pwm[ESC_UL] += x; pwm[ESC_UR] += x;
|
||||
pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
|
||||
|
||||
pwm[ESC_UL] += y; pwm[ESC_UR] -= y;
|
||||
pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
|
||||
|
||||
pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
|
||||
pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
|
||||
|
||||
short min=500, max=-500;
|
||||
for(int a=0; a<count; a++)
|
||||
{
|
||||
if(pwm[a]>500) pwm[a]=500;
|
||||
else if(pwm[a]<-500) pwm[a]=-500;
|
||||
//---
|
||||
if(min>pwm[a]) min=pwm[a];
|
||||
if(max<pwm[a]) max=pwm[a];
|
||||
}
|
||||
|
||||
short up=(throt+max)-1000;
|
||||
short down=(throt+min);
|
||||
if(up>0) throt-=up;
|
||||
if(down<0) throt-=down;
|
||||
|
||||
for(int a=0; a<count; a++) pwm[a] += throt+1000;
|
||||
|
||||
PWM_SetQuad(pwm, 1100, 2000);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void QUAD_Set3(short throt, short x, short y, short z)
|
||||
{
|
||||
//const unsigned char ESC_UL = 0; // CW // M3
|
||||
// const unsigned char ESC_UR = 3; // CCW // M1
|
||||
//const unsigned char ESC_DL = 2; // CCW // M2
|
||||
// const unsigned char ESC_DR = 1; // CW // M4
|
||||
|
||||
// const unsigned char ESC_UL = 3; // CW // M3
|
||||
//const unsigned char ESC_UR = 1; // CCW // M1
|
||||
//const unsigned char ESC_DL = 0; // CCW // M2
|
||||
//const unsigned char ESC_DR = 2; // CW // M4
|
||||
|
||||
const unsigned char ESC_UL = 2; // CW // M3
|
||||
const unsigned char ESC_UR = 0; // CCW // M1
|
||||
const unsigned char ESC_DL = 1; // CCW // M2
|
||||
const unsigned char ESC_DR = 3; // CW // M4
|
||||
|
||||
|
||||
const unsigned long count = 4;
|
||||
short pwm[count] {0,0,0,0};
|
||||
|
||||
pwm[ESC_UL] += x; pwm[ESC_UR] += x;
|
||||
pwm[ESC_DL] -= x; pwm[ESC_DR] -= x;
|
||||
|
||||
pwm[ESC_UL] += y; pwm[ESC_UR] -= y;
|
||||
pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
|
||||
|
||||
pwm[ESC_UL] -= z; pwm[ESC_UR] += z;
|
||||
pwm[ESC_DL] += z; pwm[ESC_DR] -= z;
|
||||
|
||||
short min=500, max=-500;
|
||||
for(int a=0; a<count; a++)
|
||||
{
|
||||
if(pwm[a]>500) pwm[a]=500;
|
||||
else if(pwm[a]<-500) pwm[a]=-500;
|
||||
//---
|
||||
if(min>pwm[a]) min=pwm[a];
|
||||
if(max<pwm[a]) max=pwm[a];
|
||||
}
|
||||
|
||||
short up=(throt+max)-1000;
|
||||
short down=(throt+min);
|
||||
if(up>0) throt-=up;
|
||||
if(down<0) throt-=down;
|
||||
|
||||
for(int a=0; a<count; a++) pwm[a] += throt+1000;
|
||||
|
||||
PWM_SetQuad(pwm, 1100, 2000);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
void HEXA_Set(short throt, short x, short y, short z)
|
||||
{
|
||||
// 2
|
||||
// 1 4
|
||||
// 5 0
|
||||
// 3
|
||||
const unsigned char ESC_UM = 2; // CW // M1
|
||||
const unsigned char ESC_UL = 1; // CCW // M5
|
||||
const unsigned char ESC_UR = 4; // CCW // M4
|
||||
const unsigned char ESC_DL = 5; // CW // M3
|
||||
const unsigned char ESC_DR = 0; // CW // M6
|
||||
const unsigned char ESC_DM = 3; // CCW // M2
|
||||
|
||||
const unsigned long count = 6;
|
||||
|
||||
static short pwm[count];
|
||||
for(int a=0; a<count; a++) pwm[a] = 0;
|
||||
|
||||
pwm[ESC_UL] += x/2; pwm[ESC_UM] += x; pwm[ESC_UR] += x/2;
|
||||
pwm[ESC_DL] -= x/2; pwm[ESC_DM] -= x; pwm[ESC_DR] -= x/2;
|
||||
|
||||
pwm[ESC_UL] += y; pwm[ESC_UR] -= y;
|
||||
pwm[ESC_DL] += y; pwm[ESC_DR] -= y;
|
||||
|
||||
pwm[ESC_UL] += z; pwm[ESC_UM] -= z; pwm[ESC_UR] += z;
|
||||
pwm[ESC_DL] -= z; pwm[ESC_DM] += z; pwm[ESC_DR] -= z;
|
||||
|
||||
|
||||
short min=500, max=-500;
|
||||
for(int a=0; a<count; a++)
|
||||
{
|
||||
if(pwm[a]>500) pwm[a]=500;
|
||||
else if(pwm[a]<-500) pwm[a]=-500;
|
||||
//---
|
||||
if(min>pwm[a]) min=pwm[a];
|
||||
if(max<pwm[a]) max=pwm[a];
|
||||
}
|
||||
|
||||
short up=(throt+max)-1000;
|
||||
short down=(throt+min);
|
||||
if(up>0) throt-=up;
|
||||
if(down<0) throt-=down;
|
||||
|
||||
for(int a=0; a<count; a++) pwm[a] += throt+1000;
|
||||
|
||||
PWM_SetHexa(pwm, 1100, 1800);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
6
utils/mot.h
Normal file
6
utils/mot.h
Normal file
@ -0,0 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
void QUAD_Set(short throt, short x, short y, short z);
|
||||
void QUAD_Set2(short throt, short x, short y, short z);
|
||||
void QUAD_Set3(short throt, short x, short y, short z);
|
||||
void HEXA_Set(short throt, short x, short y, short z);
|
||||
102
utils/move.cpp
Normal file
102
utils/move.cpp
Normal file
@ -0,0 +1,102 @@
|
||||
#include "move.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "quat.h"
|
||||
|
||||
static const float PI = 3.14159265359f;
|
||||
static const float TO_DEG = 180.0f/PI;
|
||||
static const float TO_RAD = PI/180.0f;
|
||||
|
||||
Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time)
|
||||
{
|
||||
//static float move_x=0, move_y=0;
|
||||
|
||||
//static float move_tof_x=0, move_tof_y=0;
|
||||
//static float move_flo_x=0, move_flo_y=0;
|
||||
|
||||
/*static long timer=0;
|
||||
timer++;
|
||||
if(timer>1000)
|
||||
{
|
||||
move_flo_x=move_flo_y=move_tof_x=move_tof_y=0;
|
||||
move_x=0; move_y=0;
|
||||
timer=0;
|
||||
}*/
|
||||
|
||||
//if(!move) return {0,0,0};
|
||||
|
||||
static float len, high;
|
||||
high = range * vec.z; // cos(Z)
|
||||
len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
|
||||
|
||||
static float a_x=0, a_y=0;
|
||||
a_x=0, a_y=0;
|
||||
|
||||
static float v_x, v_y;
|
||||
|
||||
static float angle_shift=600;
|
||||
|
||||
if(move)
|
||||
{
|
||||
a_x = (v_x-vec.x)*angle_shift;
|
||||
a_y = (v_y-vec.y)*angle_shift;
|
||||
}
|
||||
|
||||
v_x=vec.x;
|
||||
v_y=vec.y;
|
||||
|
||||
static Vec3 v;
|
||||
|
||||
float x=(dx+a_x), y=(dy-a_y);
|
||||
|
||||
static float fx=0, fy=0;
|
||||
|
||||
//fx=(fx*3+x)/4;
|
||||
//fy=(fy*3+y)/4;
|
||||
|
||||
return {x, y, high};
|
||||
|
||||
//v = RotateToZ({dx-a_x, dy-a_y, 0});
|
||||
|
||||
//v = RotateToZ({(dx+a_x)*(range/1000.0f), (dy-a_y)*(range/1000.0f), 0}, true);
|
||||
|
||||
//v = RotateToZ({1000, 0, 0}, true);
|
||||
//v = RotateToZ({(dx+a_x)*(range/1000.0f), 0, 0}, true);
|
||||
|
||||
//v={dx+a_x, dy-a_y, 0};
|
||||
|
||||
//move_x-=v.x;
|
||||
//move_y-=v.y;
|
||||
|
||||
|
||||
//move_flo_x+=dx;
|
||||
//move_flo_y+=dy;
|
||||
|
||||
//move_tof_x+=(float)a_x;
|
||||
//move_tof_y+=(float)a_y;
|
||||
|
||||
return v;
|
||||
|
||||
}
|
||||
|
||||
/*void Motion(Vec3 vec, short dx, short dy, float range, unsigned long time)
|
||||
{
|
||||
static long move_x=0, move_y=0;
|
||||
|
||||
move_x+=dx;
|
||||
move_y+=dy;
|
||||
|
||||
static float len, high;
|
||||
high = range * vec.z; // cos(Z)
|
||||
len = range * sqrtf(1-vec.z*vec.z); // sin(Z)
|
||||
|
||||
static float a_x, a_y;
|
||||
|
||||
a_x = range * vec.x;
|
||||
a_y = -range * vec.y;
|
||||
|
||||
static float m_x, m_y;
|
||||
|
||||
m_x=move_x+a_x;
|
||||
m_y=move_y+a_y;
|
||||
}*/
|
||||
5
utils/move.h
Normal file
5
utils/move.h
Normal file
@ -0,0 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
#include "quat.h"
|
||||
|
||||
Vec3 Motion(Vec3 vec, bool move, float dx, float dy, float range, unsigned long time);
|
||||
28
utils/pid.cpp
Normal file
28
utils/pid.cpp
Normal file
@ -0,0 +1,28 @@
|
||||
#include "pid.h"
|
||||
|
||||
static inline float Saturation(float val, float min, float max)
|
||||
{
|
||||
if (val < min) val = min;
|
||||
if (val > max) val = max;
|
||||
return val;
|
||||
}
|
||||
|
||||
float PID_Update(float Value, float Current, PID_Data& Coef, float de)
|
||||
{
|
||||
float p, i, d;
|
||||
|
||||
float e = Value - Current;
|
||||
|
||||
p = e * Coef.P.C;
|
||||
p = Saturation(p, Coef.P.Min, Coef.P.Max);
|
||||
|
||||
i = Coef.i;
|
||||
i += e * Coef.I.C;
|
||||
i = Saturation(i, Coef.I.Min, Coef.I.Max);
|
||||
Coef.i = i;
|
||||
|
||||
d = de * Coef.D.C;
|
||||
d = Saturation(d, Coef.D.Min, Coef.D.Max);
|
||||
|
||||
return Saturation(p + i + d, Coef.Min, Coef.Max);
|
||||
}
|
||||
14
utils/pid.h
Normal file
14
utils/pid.h
Normal file
@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
struct PID_Data
|
||||
{
|
||||
float Min, Max;
|
||||
struct
|
||||
{
|
||||
float Min, Max;
|
||||
float C;
|
||||
}P,I,D;
|
||||
float i = 0;
|
||||
};
|
||||
|
||||
float PID_Update(float Value, float Current, PID_Data& Coef, float de);
|
||||
588
utils/prot.cpp
Normal file
588
utils/prot.cpp
Normal file
@ -0,0 +1,588 @@
|
||||
#include <string.h>
|
||||
|
||||
#include "tick.h"
|
||||
|
||||
#include "ori.h"
|
||||
|
||||
#include "uart.h"
|
||||
#include "gps.h"
|
||||
|
||||
#include "pwm.h"
|
||||
|
||||
#include "prot.h"
|
||||
#include "prot_head.h"
|
||||
|
||||
Point PointGPS{0,0,0};
|
||||
|
||||
MAIN_FLY_MODE MainFlyMode=MAIN_FLY_MODE::WAIT;
|
||||
|
||||
STATUS_MODE Main_StatusMode;
|
||||
SYS_MODE Main_SysMode;
|
||||
|
||||
const unsigned char SRC_ID = 1;
|
||||
const unsigned char DST_ID = 254;
|
||||
|
||||
bool HeaderBegin::CheckCRC()
|
||||
{
|
||||
unsigned char test = 0;
|
||||
for (unsigned short a = 0; a < len1; a++) test ^= data[a];
|
||||
return crc == test;
|
||||
}
|
||||
|
||||
void HeaderBegin::SetCRC()
|
||||
{
|
||||
unsigned char test = 0;
|
||||
for (unsigned short a = 0; a < len1; a++) test ^= data[a];
|
||||
crc = test;
|
||||
}
|
||||
|
||||
enum class DataMode : unsigned char { Begin, Head, Data };
|
||||
|
||||
static unsigned char ProtoData[MaxLength];
|
||||
static DataMode Mode = DataMode::Begin;
|
||||
static unsigned long Length, Index;
|
||||
|
||||
static const unsigned long Wait = 100; // ms
|
||||
static unsigned long Timer;
|
||||
|
||||
static void ProcessingMessage(HeaderMessages*);
|
||||
|
||||
bool MainUpdate(unsigned char byte)
|
||||
{
|
||||
const unsigned long tick = TICK_GetCount();
|
||||
|
||||
//if ((tick - Timer > 100)) // Момент долгой обработки
|
||||
//{
|
||||
// printf("------------> Warning: %d\n", tick - Timer);
|
||||
//}
|
||||
|
||||
if ((Mode != DataMode::Begin) && (tick - Timer > Wait))
|
||||
{
|
||||
Mode = DataMode::Begin;
|
||||
//printf("Error: timeout\n");
|
||||
}
|
||||
|
||||
Timer = tick;
|
||||
|
||||
switch (Mode)
|
||||
{
|
||||
case DataMode::Begin:
|
||||
{
|
||||
if (byte != Global_stx) return true;
|
||||
|
||||
Index = 0;
|
||||
ProtoData[Index++] = byte;
|
||||
Length = sizeof(HeaderBegin);
|
||||
Mode = DataMode::Head;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
case DataMode::Head:
|
||||
{
|
||||
ProtoData[Index++] = byte;
|
||||
if (Index < Length) return true;
|
||||
|
||||
HeaderBegin& begin = *(HeaderBegin*)ProtoData;
|
||||
|
||||
if ((begin.len1 != begin.len2) || (begin.len1 > MaxLength)
|
||||
|| (begin.len1 < sizeof(HeaderMessages)) || Index < sizeof(HeaderBegin))
|
||||
{
|
||||
//printf("Error: HeaderBegin -> size=%d\n", begin.len1);
|
||||
Mode = DataMode::Begin;
|
||||
return true;
|
||||
}
|
||||
|
||||
Mode = DataMode::Data;
|
||||
Length += begin.len1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
case DataMode::Data:
|
||||
{
|
||||
ProtoData[Index++] = byte;
|
||||
if (Index < Length) return true;
|
||||
|
||||
HeaderBegin& begin = *(HeaderBegin*)ProtoData;
|
||||
Mode = DataMode::Begin;
|
||||
if (!begin.CheckCRC())
|
||||
{
|
||||
//printf("Error: HeaderBegin -> CRC\n");
|
||||
return true;
|
||||
}
|
||||
ProcessingMessage((HeaderMessages*)(ProtoData + sizeof(HeaderBegin)));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void SendMes(HeaderBegin* begin)
|
||||
{
|
||||
begin->SetCRC();
|
||||
//SendData(begin, sizeof(HeaderBegin) + begin->len1);
|
||||
|
||||
LPUART1_Send(begin, sizeof(HeaderBegin) + begin->len1);
|
||||
}
|
||||
//Создает основные заголовки сообщения
|
||||
|
||||
static unsigned char MessageData[MaxLength];
|
||||
|
||||
void* CreateMessageHeaders(HeaderBegin*& begin, unsigned long size, MESSAGES_ID msg_id)
|
||||
{
|
||||
|
||||
begin = (HeaderBegin*)MessageData;
|
||||
begin->len1 = begin->len2 = sizeof(HeaderMessages) + size;
|
||||
begin->stx = Global_stx;
|
||||
HeaderMessages& message = *(HeaderMessages*)begin->data;
|
||||
message.msgId = msg_id;
|
||||
message.srcId = SRC_ID;
|
||||
message.dstId = DST_ID;
|
||||
message.len = size;
|
||||
message.timeusec = TICK_GetCount();
|
||||
|
||||
return message.data;
|
||||
}
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение BatteryInfo
|
||||
void BatteryInfo()
|
||||
{
|
||||
//Опрос датчиков и систем
|
||||
HeaderBegin* begin;
|
||||
PayloadBatteryInfo& info = *(PayloadBatteryInfo*) CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::BatteryInfo);
|
||||
|
||||
info.perCharge = 95;
|
||||
info.voltage = 14.5f;
|
||||
info.amperage = 20.8f;
|
||||
info.temp = 36.6f;
|
||||
info.totalPower = 100.0f;
|
||||
info.timeRemaining = 3600;
|
||||
//printf("MessageTelemetry: BatteryInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
NAV_SYS_MODE PROT_Nav=NAV_SYS_MODE::gps;
|
||||
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение SysInfo
|
||||
void SysInfo()
|
||||
{
|
||||
//Опрос датчиков и систем
|
||||
unsigned int count_engine = 4;
|
||||
HeaderBegin* begin;
|
||||
PayloadSysInfo& info = *(PayloadSysInfo*)CreateMessageHeaders(begin, sizeof(PayloadSysInfo) + count_engine * sizeof(EnginePowerInfo), MESSAGES_ID::SysInfo);
|
||||
|
||||
info.mode = Main_SysMode; // !!!!!!!!!!!!
|
||||
info.navSys = PROT_Nav;
|
||||
info.pressureBaro = DataORI.Bar.Bar;
|
||||
info.statusMode = Main_StatusMode; // !!!!!!!!!!
|
||||
info.engineStatus = PWM_Enable ? ENGINE_STATUS::enable : ENGINE_STATUS::disable; // !!!!!!!!!!!
|
||||
info.engineCount = count_engine;
|
||||
info.tempBaro = DataORI.Temp.Bar;
|
||||
|
||||
for (int i = 0; i < info.engineCount; i++)
|
||||
{
|
||||
EnginePowerInfo& pow = info.enginePower[i];
|
||||
pow.power = 50;
|
||||
pow.current = 1.0;
|
||||
pow.engineSpeed = 100;
|
||||
pow.voltage = 12.5;
|
||||
pow.temp = 34.5;
|
||||
}
|
||||
//printf("MessageTelemetry: SysInfo -> Send\n");
|
||||
|
||||
SendMes(begin);
|
||||
}
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение GyroInfo
|
||||
void GyroInfo()
|
||||
{
|
||||
//Опрос датчиков и систем
|
||||
HeaderBegin* begin;
|
||||
PayloadGyroInfo& info = *(PayloadGyroInfo*)CreateMessageHeaders(begin, sizeof(PayloadGyroInfo), MESSAGES_ID::GyroInfo);
|
||||
|
||||
info.pitchGyroVel = DataORI.Gyr.X;
|
||||
info.rollGyroVel = DataORI.Gyr.Y;
|
||||
info.yawGyroVel = DataORI.Gyr.Z;
|
||||
//printf("MessageTelemetry: GyroInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение AccelInfo
|
||||
void AccelInfo()
|
||||
{
|
||||
//Опрос датчиков и систем
|
||||
HeaderBegin* begin;
|
||||
PayloadAccelInfo& info = *(PayloadAccelInfo*)CreateMessageHeaders(begin, sizeof(PayloadAccelInfo), MESSAGES_ID::AccelInfo);
|
||||
|
||||
info.aX = DataORI.Acc.X;
|
||||
info.aY = DataORI.Acc.Y;
|
||||
info.aZ = DataORI.Acc.Z;
|
||||
info.pitchAccelVel = 0.0;
|
||||
info.rollAccelVel = 0.0;
|
||||
info.yawAccelVel = 0.0;
|
||||
info.tempAccel = DataORI.Temp.Acc;
|
||||
//printf("MessageTelemetry: AccelInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
static GPS_BaseInfo Main_GPS;
|
||||
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение GpsInfo
|
||||
void GpsInfo()
|
||||
{
|
||||
|
||||
GPS_GetBaseInfo(Main_GPS);
|
||||
|
||||
//Опрос датчиков и систем
|
||||
HeaderBegin* begin;
|
||||
PayloadGpsInfo& info = *(PayloadGpsInfo*)CreateMessageHeaders(begin, sizeof(PayloadGpsInfo), MESSAGES_ID::GpsInfo);
|
||||
|
||||
info.lat = Main_GPS.lat;
|
||||
info.lon = Main_GPS.lon;
|
||||
info.absAlt = Main_GPS.absAlt;
|
||||
info.realAlt = Main_GPS.realAlt;
|
||||
info.fixType = Main_GPS.fixType;
|
||||
info.hdop = Main_GPS.hdop;
|
||||
info.pdop = Main_GPS.pdop;
|
||||
info.vdop = Main_GPS.vdop;
|
||||
info.satUsed = Main_GPS.satUsed;
|
||||
info.satVisible = Main_GPS.satVisible;
|
||||
info.noise = Main_GPS.noise;
|
||||
info.jamming = 0.0;
|
||||
info.speed = Main_GPS.speed;
|
||||
info.timeUTC = Main_GPS.timeUTC;
|
||||
//printf("MessageTelemetry: GpsInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
//Обработчик запроса сообщения телеметрии. Опрашивает датчики и системы, формирует сообщение InertialInfo
|
||||
void InertialInfo()
|
||||
{
|
||||
//Опрос датчиков и систем
|
||||
HeaderBegin* begin;
|
||||
PayloadInertialInfo& info = *(PayloadInertialInfo*)CreateMessageHeaders(begin, sizeof(PayloadInertialInfo), MESSAGES_ID::InertialInfo);
|
||||
|
||||
info.x = Main_GPS.LocalXYZ[0];
|
||||
info.y = Main_GPS.LocalXYZ[1];
|
||||
info.z = Main_GPS.LocalXYZ[2];
|
||||
info.baroAlt = DataORI.Bar.Alt;
|
||||
info.pitch = DataORI.Pitch;
|
||||
info.yaw = DataORI.Yaw;
|
||||
info.roll = DataORI.Roll;
|
||||
info.headingDeg = DataORI.Yaw;
|
||||
info.speed = Main_GPS.speed;
|
||||
info.pitchVel = 1.0;
|
||||
info.yawVel = 1.0;
|
||||
info.rollVel = 1.0;
|
||||
//printf("MessageTelemetry: InertialInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
//Формирует ответ на команду по полезной нагрузке ответа
|
||||
void CommandAck(PayloadCommandAck& ack)
|
||||
{
|
||||
HeaderBegin* begin;
|
||||
PayloadCommandAck& info = *(PayloadCommandAck*)CreateMessageHeaders(begin, sizeof(PayloadCommandAck), MESSAGES_ID::Ack);
|
||||
info = ack;
|
||||
//printf("MessageCommandACK: Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
//Обработчик комманды приземления
|
||||
static void ProcessingCommandLand()
|
||||
{
|
||||
MainFlyMode=MAIN_FLY_MODE::STOP;
|
||||
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::Land;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды остановки полета (зависание)
|
||||
static void ProcessingCommandPause()
|
||||
{
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::Pause;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды возврата в точку старта
|
||||
static void ProcessingCommandGoHome()
|
||||
{
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::GoHome;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды продолжения полета
|
||||
static void ProcessingCommandContinue()
|
||||
{
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::Continue;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды запуска моторов
|
||||
static void ProcessingCommandStartEngine()
|
||||
{
|
||||
if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::START;
|
||||
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::StartEngine;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды остановки моторов
|
||||
static void ProcessingCommandStopEngine()
|
||||
{
|
||||
MainFlyMode=MAIN_FLY_MODE::WAIT;
|
||||
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::StopEngine;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды изменения скорости полета
|
||||
static void ProcessingCommandChangeSpeed(PayloadCommandChangeSpeed* payload)
|
||||
{
|
||||
// Выполняет команду...
|
||||
//payload->speed;
|
||||
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::ChangeSpeed;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды смены навигационной системы
|
||||
static void ProcessingCommandChangeNav(PayloadCommandChangeNav* payload)
|
||||
{
|
||||
PROT_Nav=(NAV_SYS_MODE)payload->nav_id;
|
||||
// Выполняет команду...
|
||||
//payload->nav_id;
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::ChangeNav;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
}
|
||||
//Обработчик комманды перемещения в точку по глобальным координатам
|
||||
static void ProcessingCommandGoToGlobal(PayloadCommandGoToGlobal* payload)
|
||||
{
|
||||
PointGPS.Latitude=payload->lat;
|
||||
PointGPS.Longitude=payload->lon;
|
||||
PointGPS.Altitude=payload->abs_alt;
|
||||
|
||||
if(MainFlyMode!=MAIN_FLY_MODE::STOP) MainFlyMode=MAIN_FLY_MODE::FLY;
|
||||
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::GoToGlobal;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
//Обработчик комманды перемещения в точку по локальным координатам
|
||||
static void ProcessingCommandGoToLocal(PayloadCommandGoToLocal* payload)
|
||||
{
|
||||
// Выполняет команду...
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::GoToLocal;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
CommandAck(info);
|
||||
|
||||
}
|
||||
|
||||
//
|
||||
static void ProcessingCommandSetParameter(PayloadCommandSetParameter* payload)
|
||||
{
|
||||
unsigned char cat_id = payload->cat_id; // ID категории
|
||||
unsigned char param_id = payload->param_id; // ID параметра
|
||||
DATA_TYPES param_type = payload->param_type_code; // Тип данных
|
||||
unsigned char param_len = payload->param_len; // Размер массива данных
|
||||
//payload->param_data; // Массив байт параметра
|
||||
|
||||
PayloadCommandAck info;
|
||||
info.commandId = COMMANDS_NAME::SetParameter;
|
||||
info.status = FEASIBILITY_CODE_COMMAND::accepted;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
CommandAck(info);
|
||||
}
|
||||
|
||||
//Общий обработчик сообщения с командой
|
||||
static void ProcessingCommand(CommandObj* commandObject) {
|
||||
switch (commandObject->commandId)
|
||||
{
|
||||
case COMMANDS_NAME::Land: ProcessingCommandLand(); return;
|
||||
case COMMANDS_NAME::Pause: ProcessingCommandPause(); return;
|
||||
case COMMANDS_NAME::GoHome: ProcessingCommandGoHome(); return;
|
||||
case COMMANDS_NAME::Continue: ProcessingCommandContinue(); return;
|
||||
case COMMANDS_NAME::StartEngine: ProcessingCommandStartEngine(); return;
|
||||
case COMMANDS_NAME::StopEngine: ProcessingCommandStopEngine(); return;
|
||||
case COMMANDS_NAME::ChangeSpeed: ProcessingCommandChangeSpeed((PayloadCommandChangeSpeed*)commandObject->data); return;
|
||||
case COMMANDS_NAME::ChangeNav: ProcessingCommandChangeNav((PayloadCommandChangeNav*)commandObject->data); return;
|
||||
case COMMANDS_NAME::GoToGlobal: ProcessingCommandGoToGlobal((PayloadCommandGoToGlobal*)commandObject->data); return;
|
||||
case COMMANDS_NAME::GoToLocal: ProcessingCommandGoToLocal((PayloadCommandGoToLocal*)commandObject->data); return;
|
||||
case COMMANDS_NAME::SetParameter: ProcessingCommandSetParameter((PayloadCommandSetParameter*)commandObject->data); return;
|
||||
}
|
||||
}
|
||||
//Обработчик сообщения тестирования связи
|
||||
static void ConnectionTest()
|
||||
{
|
||||
HeaderBegin* begin;
|
||||
CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::ConnectionTest);
|
||||
|
||||
begin->SetCRC();
|
||||
|
||||
LPUART1_Send(begin, sizeof(HeaderBegin) + begin->len1);
|
||||
}
|
||||
//Обработчик сообщения запроса статуса команды
|
||||
static void StatusCommand(PayloadStatusCommand* payload)
|
||||
{
|
||||
// payload->commandId; Тут должна быть какая-то проверка текущего состояния выполнения команды по ее ID
|
||||
HeaderBegin* begin;
|
||||
PayloadStatusCommand& info = *(PayloadStatusCommand*)CreateMessageHeaders(begin, sizeof(PayloadStatusCommand), MESSAGES_ID::StatusCommand);
|
||||
info.commandId = payload->commandId;
|
||||
info.executionCode = EXEC_CODE_COMMAND::completed;
|
||||
info.errorCode = ERROR_CODE_COMMAND::No_error;
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
static void CountMenuCategories()
|
||||
{
|
||||
//
|
||||
unsigned char count_categories = 4;
|
||||
unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 };
|
||||
HeaderBegin* begin;
|
||||
PayloadCountMenuCategories& info = *(PayloadCountMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadCountMenuCategories) + count_categories * sizeof(unsigned char), MESSAGES_ID::CountMenuCategories);
|
||||
|
||||
info.countCategories = count_categories;
|
||||
for (int i = 0; i < info.countCategories; i++) {
|
||||
info.data[i] = categoriesNamesLen[i];
|
||||
}
|
||||
//printf("MessageTelemetry: SysInfo -> Send\n");
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
static void MenuCategories(PayloadMenuCategories* payload)
|
||||
{
|
||||
const char* names[4] = { "Main1","PID_S1","MOTOR", "ACCELER"}; // Имена категорий
|
||||
unsigned char categoriesNamesLen[4] = { 5, 6, 5, 7 }; // Длины имен категорий (имена в кодировке utf-8)
|
||||
unsigned char req_count = payload->countCategories;
|
||||
unsigned char total_len_names = 0;
|
||||
for (int i = 0; i < req_count; i++)
|
||||
{
|
||||
unsigned char cat_id = (payload->categories)[i];
|
||||
unsigned char cat_name_len = categoriesNamesLen[(payload->categories)[i]];
|
||||
total_len_names += cat_name_len;
|
||||
}
|
||||
HeaderBegin* begin;
|
||||
PayloadMenuCategories& info = *(PayloadMenuCategories*)CreateMessageHeaders(begin, sizeof(PayloadMenuCategories) + req_count * sizeof(MenuCategori) + total_len_names * sizeof(unsigned char), MESSAGES_ID::CategoriesMenu);
|
||||
info.countCategories = req_count;
|
||||
MenuCategori* menu = (MenuCategori*)info.categories;
|
||||
for (int i = 0; i < req_count; i++)
|
||||
{
|
||||
menu->id = (payload->categories)[i];
|
||||
menu->count_param = 1;
|
||||
menu->len_name = categoriesNamesLen[menu->id];
|
||||
memcpy(menu->name, names[menu->id], menu->len_name);
|
||||
menu = (MenuCategori*)(((char*)menu) + sizeof(MenuCategori) + menu->len_name);
|
||||
}
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
static void CategoriesParameters(PayloadCategoriesParameters* payload)
|
||||
{
|
||||
unsigned char cat_id = payload->cat_id; // id категории для поиска параметра
|
||||
unsigned char param_id = payload->param_id; // id параметра для поиска параметра
|
||||
// Получение параметра
|
||||
const char* name = "PARAMETER1";
|
||||
unsigned char len_name = 10;
|
||||
float param1 = 0.231;
|
||||
DATA_TYPES param_type = DATA_TYPES::dt_float_32;
|
||||
//
|
||||
|
||||
HeaderBegin* begin;
|
||||
PayloadCategoriesParameters& info = *(PayloadCategoriesParameters*)CreateMessageHeaders(begin, sizeof(PayloadCategoriesParameters) + len_name + sizeof(param1), MESSAGES_ID::CategoriesParameters);
|
||||
info.cat_id = cat_id;
|
||||
info.param_id = param_id;
|
||||
info.param_name_len = len_name;
|
||||
info.param_len = sizeof(param1);
|
||||
info.param_type_code = param_type;
|
||||
memcpy(info.param_data, name, len_name);
|
||||
memcpy(&info.param_data[len_name], ¶m1, sizeof(param1));
|
||||
SendMes(begin);
|
||||
}
|
||||
|
||||
//void Ping(HeaderMessages* message)
|
||||
//{
|
||||
// HeaderBegin* begin;
|
||||
// CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Ping);
|
||||
// //---
|
||||
// SendMes(begin);
|
||||
// //---
|
||||
// printf("Ping %d Pong %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
|
||||
//}
|
||||
//
|
||||
//void Pong(HeaderMessages* message)
|
||||
//{
|
||||
// HeaderBegin* begin;
|
||||
// CreateMessageHeaders(begin, sizeof(PayloadBatteryInfo), MESSAGES_ID::Pong);
|
||||
// //---
|
||||
// SendMes(begin);
|
||||
// //---
|
||||
// printf("Pong %d Ping %d \n", message->timeusec, ((HeaderMessages*)begin->data)->timeusec);
|
||||
//}
|
||||
|
||||
static void ProcessingMessage(HeaderMessages* message)
|
||||
{
|
||||
switch (message->msgId)
|
||||
{
|
||||
/*case MESSAGES_ID::Ping: Pong(message); return;
|
||||
case MESSAGES_ID::Pong: Ping(message); return;*/
|
||||
case MESSAGES_ID::BatteryInfo: BatteryInfo(); return;
|
||||
case MESSAGES_ID::GyroInfo: GyroInfo(); return;
|
||||
case MESSAGES_ID::AccelInfo: AccelInfo(); return;
|
||||
case MESSAGES_ID::GpsInfo: GpsInfo(); return;
|
||||
case MESSAGES_ID::InertialInfo: InertialInfo(); return;
|
||||
case MESSAGES_ID::SysInfo: SysInfo(); return;
|
||||
case MESSAGES_ID::Command: ProcessingCommand((CommandObj*)message->data); return;
|
||||
case MESSAGES_ID::ConnectionTest: ConnectionTest(); return;
|
||||
case MESSAGES_ID::StatusCommand: StatusCommand((PayloadStatusCommand*)message->data); return;
|
||||
|
||||
case MESSAGES_ID::CountMenuCategories: CountMenuCategories(); return;
|
||||
case MESSAGES_ID::CategoriesMenu: MenuCategories((PayloadMenuCategories*)message->data); return;
|
||||
case MESSAGES_ID::CategoriesParameters: CategoriesParameters((PayloadCategoriesParameters*)message->data); return;
|
||||
}
|
||||
}
|
||||
|
||||
//void TestPing()
|
||||
//{
|
||||
// HeaderMessages message;
|
||||
// message.timeusec = GetTickCount();
|
||||
// Pong(&message);
|
||||
//}
|
||||
|
||||
void PROT_Update()
|
||||
{
|
||||
char byte;
|
||||
unsigned char size = LPUART1_Recv(&byte, 1);
|
||||
if (size) MainUpdate(byte);
|
||||
}
|
||||
14
utils/prot.h
Normal file
14
utils/prot.h
Normal file
@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#include "prot_head.h"
|
||||
|
||||
extern Point PointGPS;
|
||||
|
||||
enum class MAIN_FLY_MODE { WAIT, START, FLY, STOP };
|
||||
|
||||
extern MAIN_FLY_MODE MainFlyMode;
|
||||
|
||||
extern STATUS_MODE Main_StatusMode;
|
||||
extern SYS_MODE Main_SysMode;
|
||||
|
||||
void PROT_Update();
|
||||
318
utils/prot_head.h
Normal file
318
utils/prot_head.h
Normal file
@ -0,0 +1,318 @@
|
||||
#pragma once
|
||||
|
||||
enum class MESSAGES_ID : unsigned char
|
||||
{
|
||||
SysInfo = 1,
|
||||
GyroInfo = 2,
|
||||
AccelInfo = 3,
|
||||
GpsInfo = 4,
|
||||
InertialInfo = 5,
|
||||
BatteryInfo = 6,
|
||||
Ack = 7,
|
||||
StatusCommand = 8,
|
||||
StatusAllCommands = 9,
|
||||
Command = 10,
|
||||
RequestReg = 11,
|
||||
ResponseReg = 12,
|
||||
Auth = 13,
|
||||
OpenKey = 14,
|
||||
MissionCount = 15,
|
||||
MissionItem = 16,
|
||||
MissionItemAck = 17,
|
||||
MissionRequestItem = 18,
|
||||
RequestLastMessage = 19,
|
||||
ConnectionTest = 20,
|
||||
|
||||
CountMenuCategories = 32,
|
||||
CategoriesMenu = 33,
|
||||
CategoriesParameters = 34,
|
||||
|
||||
//Ping = 200, // :)
|
||||
//Pong = 201, // :]
|
||||
};
|
||||
enum class COMMANDS_NAME : unsigned char
|
||||
{
|
||||
ChangeNav = 1,
|
||||
ChangeSpeed = 2,
|
||||
Land = 3,
|
||||
GoHome = 4,
|
||||
StopEngine = 5,
|
||||
StartEngine = 6,
|
||||
Pause = 7,
|
||||
Continue = 8,
|
||||
GoToGlobal = 9,
|
||||
GoToLocal = 10,
|
||||
|
||||
SetParameter = 15,
|
||||
};
|
||||
enum class ERROR_CODE_COMMAND : unsigned char
|
||||
{
|
||||
No_error = 0,
|
||||
Speed_has_already_been_set = 1,
|
||||
Engines_are_already_running = 2,
|
||||
Engines_have_already_been_stopped = 3,
|
||||
UAV_is_already_on_the_ground = 4,
|
||||
UAV_is_already_landing = 5,
|
||||
UAV_is_already_moving = 6,
|
||||
Mission_has_not_been_launched = 7,
|
||||
Mission_is_already_on_pause = 8,
|
||||
Mission_was_not_on_pause = 9,
|
||||
Mission_was_not_launched = 10,
|
||||
This_NavSys_has_already_been_applied = 11,
|
||||
Lost_connection_with_controller = 12,
|
||||
The_command_is_canceled_when_a_new_one_is_received = 13,
|
||||
Error_reading_the_mission_file = 14,
|
||||
Error_validating_the_mission_file = 15
|
||||
};
|
||||
enum class ENGINE_STATUS : unsigned char
|
||||
{
|
||||
disable = 0,
|
||||
enable = 1
|
||||
};
|
||||
|
||||
enum class STATUS_MODE : unsigned char
|
||||
{
|
||||
on_ground = 1,
|
||||
taking_off = 2,
|
||||
fly = 3,
|
||||
landing = 4
|
||||
};
|
||||
|
||||
enum class SYS_MODE : unsigned char
|
||||
{
|
||||
take_off = 1,
|
||||
land = 2,
|
||||
go_home = 3,
|
||||
auto_mode = 4,
|
||||
hold = 5
|
||||
};
|
||||
|
||||
enum class NAV_SYS_MODE : unsigned char
|
||||
{
|
||||
inertial = 1,
|
||||
gps = 2,
|
||||
Optical_flow = 3
|
||||
};
|
||||
|
||||
enum class EXEC_CODE_COMMAND : unsigned char
|
||||
{
|
||||
error = 0,
|
||||
in_progress = 1,
|
||||
completed = 2
|
||||
};
|
||||
|
||||
enum class FEASIBILITY_CODE_COMMAND : unsigned char
|
||||
{
|
||||
cannot_be_performed = 0,
|
||||
accepted = 1
|
||||
};
|
||||
|
||||
enum class DATA_TYPES : unsigned char
|
||||
{
|
||||
dt_char = 0,
|
||||
dt_unsigned_char = 1,
|
||||
dt_string = 2,
|
||||
dt_short = 3,
|
||||
dt_bool = 4,
|
||||
dt_unsigned_short = 5,
|
||||
dt_int = 6,
|
||||
dt_unsigned_int = 7,
|
||||
dt_long = 8,
|
||||
dt_unsigned_long = 9,
|
||||
dt_long_long = 10,
|
||||
dt_unsigned_long_long = 11,
|
||||
dt_float_32 = 12,
|
||||
dt_float_64 = 13,
|
||||
dt_unknown = 255
|
||||
};
|
||||
|
||||
#pragma pack(push,1)
|
||||
|
||||
const unsigned short MaxLength = 256;
|
||||
const unsigned char Global_stx = 0xAA;
|
||||
//Начало заголовка
|
||||
struct HeaderBegin
|
||||
{
|
||||
unsigned char stx;
|
||||
unsigned short len1; //Len header + payload (message)
|
||||
unsigned short len2; //Len header + payload (message)
|
||||
unsigned char crc; // CRC MESSAGE
|
||||
unsigned char data[0]; // Message
|
||||
//---
|
||||
bool CheckCRC();
|
||||
void SetCRC();
|
||||
};
|
||||
// Заголовок сообщения
|
||||
struct HeaderMessages
|
||||
{
|
||||
MESSAGES_ID msgId;
|
||||
unsigned char srcId;
|
||||
unsigned char dstId;
|
||||
unsigned char len;
|
||||
unsigned long long timeusec;
|
||||
unsigned char data[0];
|
||||
};
|
||||
struct CommandObj
|
||||
{
|
||||
COMMANDS_NAME commandId;
|
||||
unsigned char data[0];
|
||||
};
|
||||
//Полезная нагрузка сообщения с данными по АКБ
|
||||
struct PayloadBatteryInfo
|
||||
{
|
||||
unsigned char perCharge;
|
||||
float voltage;
|
||||
float amperage;
|
||||
float temp;
|
||||
float totalPower;
|
||||
unsigned long long timeRemaining;
|
||||
};
|
||||
// Структура с данными по каждому мотору
|
||||
struct EnginePowerInfo
|
||||
{
|
||||
unsigned char power; //Текущая нагрузка на мотор от 0 до 100%
|
||||
unsigned int engineSpeed;//Текущие оборты в секунду мотора
|
||||
float current; // Ток потребляемый мотором
|
||||
float voltage; // Напряжение на моторе
|
||||
float temp; // Температура мотора
|
||||
|
||||
};
|
||||
struct PayloadSysInfo // Полезная нагрузка с данными о остоянии БПЛА
|
||||
{
|
||||
SYS_MODE mode; //Текущий режим полета (взлет - 1, посадка - 2, режим автоматического возврата домой - 3, режим автономного полета - 4, ожидание - 5)
|
||||
STATUS_MODE statusMode; //Текущий статус режим (На земле (on ground) - 1, набор высоты (TakingOff) - 2, полет (In air) - 3, приземление (Landing) - 4)
|
||||
NAV_SYS_MODE navSys; // Текущая используемая система навигации (GPS\ИНС\Optical Flow) ИНС - 1, GPS - 2, Optical Flow - 3
|
||||
ENGINE_STATUS engineStatus; // Текущее состояние моторов. Выкл - 0, Вкл - 1.
|
||||
unsigned char engineCount; // Количество двигателей на БПЛА
|
||||
float pressureBaro; //Атмосферное давление
|
||||
float tempBaro; // Температура Окр. среды
|
||||
EnginePowerInfo enginePower[0];
|
||||
};
|
||||
struct PayloadGyroInfo // Полезная нагрузка с данными от гироскопа
|
||||
{
|
||||
float yawGyroVel; // Текущая скорость изменения угла рысканья (в чем будет измеряться? рад\с?)
|
||||
float pitchGyroVel; // Текущая скорость изменения угла тангажа (в чем будет измеряться? рад\с?)
|
||||
float rollGyroVel; // Текущая скорость изменения угла крена (в чем будет измеряться? рад\с?)
|
||||
};
|
||||
struct PayloadAccelInfo // Полезная нагрузка сообщения с данными от акселерометра
|
||||
{
|
||||
float yawAccelVel; // текущее ускорение изменения угла рысканья (Z)
|
||||
float pitchAccelVel; // текущее ускорение изменения угла тангажа (Y)
|
||||
float rollAccelVel; // текущее ускорение изменения угла крена (X)
|
||||
float aX; // ускорение по X (м\с^2)
|
||||
float aY; // ускорение по У (м\с^2)
|
||||
float aZ; // ускорение по Z (м\с^2)
|
||||
float tempAccel; //Температура акселерометра
|
||||
};
|
||||
struct PayloadGpsInfo // Полезная нагрузка сообщения с данными от GPS датчика
|
||||
{
|
||||
float lat; //
|
||||
float lon; //
|
||||
float absAlt; //
|
||||
float realAlt; //
|
||||
float hdop; //
|
||||
float vdop; //
|
||||
float pdop; //
|
||||
float noise; //
|
||||
float jamming; //
|
||||
unsigned char satVisible;
|
||||
unsigned char satUsed; //
|
||||
float speed; //
|
||||
unsigned char fixType; // NO_GPS - 0, NO_FIX - 1, 2D_FIX - 2, 3D_FIX - 3, DGPS - 4, RTK_FLOAT - 5, RTK_FIXED - 6, STATIC - 7, PPP - 8
|
||||
unsigned long long timeUTC; // UTC - GPS
|
||||
};
|
||||
struct PayloadInertialInfo // Полезная нагрузка с данными от инерциальной системы координат
|
||||
{
|
||||
float x; // Координата X (по локальной системе координат)
|
||||
float y; // Координата Y (по локальной системе координат)
|
||||
float z; // Координата высоты (по локальной системе координат)
|
||||
float headingDeg; // Текущее направление движения БПЛА относительно севера
|
||||
float speed; // скорость БПЛА (полученная не по GPS)
|
||||
float roll; // текущий угол крена в градусах
|
||||
float pitch; // текущий угол тангажа в градусах
|
||||
float yaw; // текущий угол рысканья в градусах
|
||||
float rollVel; // Скорость по направлению крена м\с (в направлении оси Y)
|
||||
float pitchVel; // Скорость по направлению тангажа м\с (в направлении оси Х)
|
||||
float yawVel; // Скорость по направлению рысканья м\с (в направлении оси Z)
|
||||
float baroAlt; // Высота в метрах по барометру
|
||||
};
|
||||
//Полезная нагрузка сообщения с ответом о получении команды
|
||||
struct PayloadCommandAck
|
||||
{
|
||||
COMMANDS_NAME commandId;
|
||||
FEASIBILITY_CODE_COMMAND status; // 0 - не может быть выполнена (указывается код ошибки), 1 - принята
|
||||
ERROR_CODE_COMMAND errorCode;
|
||||
};
|
||||
struct PayloadStatusCommand
|
||||
{
|
||||
COMMANDS_NAME commandId;
|
||||
EXEC_CODE_COMMAND executionCode; // 0 - ошибка при выполнении ,1 - выполняется ,2 - выполнена успешно.
|
||||
ERROR_CODE_COMMAND errorCode;
|
||||
};
|
||||
// Полезная нагрузка команды для смены скорости полета
|
||||
struct PayloadCommandChangeSpeed
|
||||
{
|
||||
float speed;
|
||||
};
|
||||
// Полезная нагрузка команды для смены текущей исп. нав. системы.
|
||||
struct PayloadCommandChangeNav
|
||||
{
|
||||
unsigned char nav_id; // Inertial - 1, GPS = 2, Oprical_Flow = 3
|
||||
};
|
||||
// Полезная нагрузка команды для перемещения в точку по глобальным координатам
|
||||
struct PayloadCommandGoToGlobal
|
||||
{
|
||||
float lat;
|
||||
float lon;
|
||||
float abs_alt;
|
||||
float speed;
|
||||
};
|
||||
// Полезная нагрузка команды для перемещения в точку по локальным координатам
|
||||
struct PayloadCommandGoToLocal
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
float speed;
|
||||
};
|
||||
|
||||
struct PayloadCountMenuCategories
|
||||
{
|
||||
unsigned char countCategories;
|
||||
unsigned char data[0];
|
||||
};
|
||||
|
||||
struct MenuCategori
|
||||
{
|
||||
unsigned char id;
|
||||
unsigned char len_name;
|
||||
unsigned char count_param;
|
||||
char name[0];
|
||||
};
|
||||
|
||||
struct PayloadMenuCategories
|
||||
{
|
||||
unsigned char countCategories;
|
||||
unsigned char categories[0];
|
||||
};
|
||||
|
||||
struct PayloadCategoriesParameters
|
||||
{
|
||||
unsigned char cat_id;
|
||||
unsigned char param_id;
|
||||
unsigned char param_name_len;
|
||||
unsigned char param_len;
|
||||
DATA_TYPES param_type_code;
|
||||
char param_data[0];
|
||||
};
|
||||
|
||||
struct PayloadCommandSetParameter
|
||||
{
|
||||
unsigned char cat_id;
|
||||
unsigned char param_id;
|
||||
DATA_TYPES param_type_code;
|
||||
unsigned char param_len;
|
||||
char param_data[0];
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
274
utils/quat.cpp
Normal file
274
utils/quat.cpp
Normal file
@ -0,0 +1,274 @@
|
||||
#include "quat.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;
|
||||
|
||||
struct Quaternion
|
||||
{
|
||||
float w, x, y, z;
|
||||
};
|
||||
|
||||
static Quaternion qCurrent = { 1, 0, 0, 0 };
|
||||
|
||||
static const float period = 0.01f; // 100Hz
|
||||
|
||||
static bool isFirst = true;
|
||||
|
||||
inline 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;
|
||||
}
|
||||
}
|
||||
|
||||
inline 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
|
||||
};
|
||||
}
|
||||
|
||||
inline 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, Vec3 xyz, float& error)
|
||||
{
|
||||
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);
|
||||
|
||||
float x=a.x-xyz.x, y=a.y-xyz.y, z=a.z-xyz.z;
|
||||
|
||||
error=sqrtf(x*x+y*y+z*z)/10.0f;
|
||||
|
||||
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)
|
||||
{
|
||||
if(a1>0.0f) return 90.0f;
|
||||
if(a1<0.0f) return -90.0f;
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
return atanf(a1/sqrtf(a2*a2+az*az)) * TO_DEG;
|
||||
}
|
||||
|
||||
static Vec3 quaternionToPitchRollYaw(const Quaternion& q, Vec3& cosXYZ)
|
||||
{
|
||||
Quaternion pry=rotateVectorByQuaternion(q, {0, 0, 1});
|
||||
|
||||
float yaw = 2.0f * atan2f(q.z, q.w) * TO_DEG;
|
||||
if(yaw<0.0f) yaw=360.0f+yaw;
|
||||
|
||||
cosXYZ = {pry.x, pry.y, pry.z};
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
ORI WorkAccGyroMag(const Vec3 acc, const Vec3 gyr, const Vec3 mag, const float mag_shift, const float alpha)
|
||||
{
|
||||
float wx = gyr.x * 500.0f / 30000.0f * TO_RAD * period;
|
||||
float wy = gyr.y * 500.0f / 30000.0f * TO_RAD * period;
|
||||
float wz = gyr.z * 500.0f / 30000.0f * TO_RAD * period;
|
||||
|
||||
Vec3 aB = acc;
|
||||
|
||||
Vec3 cosXYZ;
|
||||
Vec3 pry=quaternionToPitchRollYaw(qCurrent, cosXYZ);
|
||||
|
||||
static float error=0;
|
||||
// Quaternion qAcc = AccQuaternion(qCurrent, aB, 0.05f, cosXYZ, error); // Tolerance for gravity deviation 5%
|
||||
|
||||
qCurrent = GyroQuaternion(qCurrent, wx, wy, wz);
|
||||
|
||||
if(error>0.01f) error=0.01f;
|
||||
if(error<alpha) error=alpha;
|
||||
/*
|
||||
|
||||
Quaternion qFused = nlerp(qCurrent, qAcc, 0.0);
|
||||
|
||||
if (isFirst)
|
||||
{
|
||||
qFused = qAcc;
|
||||
isFirst = false;
|
||||
}
|
||||
|
||||
qCurrent = qFused;
|
||||
*/
|
||||
//addMagneto(qCurrent, mag, alpha, mag_shift);
|
||||
|
||||
Vec3 ine=backRotateVectorByQuaternion(qCurrent, aB);
|
||||
|
||||
return
|
||||
{
|
||||
cosXYZ.x, cosXYZ.y, cosXYZ.z,
|
||||
pry.x, pry.y, pry.z,
|
||||
ine.x, ine.y, ine.z,
|
||||
};
|
||||
}
|
||||
|
||||
Vec3 RotateToZ(const Vec3 vec, bool Rev)
|
||||
{
|
||||
Quaternion v { 0, vec.x, vec.y, vec.z };
|
||||
Quaternion q = { qCurrent.w, 0, 0, Rev ? -qCurrent.z : qCurrent.z };
|
||||
normalizeQuaternion(q);
|
||||
|
||||
q = quaternionMultiply(quaternionMultiply(v, q), q); // Востановить оборот по Z
|
||||
|
||||
return { q.x, q.y, q.z };
|
||||
}
|
||||
|
||||
void QuatGoToZero()
|
||||
{
|
||||
qCurrent = {1.0f,0.0f,0.0f,0.0f};
|
||||
}
|
||||
15
utils/quat.h
Normal file
15
utils/quat.h
Normal file
@ -0,0 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
struct Vec3 { float x, y, z; };
|
||||
|
||||
struct ORI
|
||||
{
|
||||
float sinX, sinY, cosZ; // 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);
|
||||
|
||||
Vec3 RotateToZ(const Vec3 vec, bool Rev=false);
|
||||
void QuatGoToZero();
|
||||
Reference in New Issue
Block a user