282 lines
6.4 KiB
C++
282 lines
6.4 KiB
C++
#include <math.h>
|
|
|
|
#include "imu.h"
|
|
#include "bar.h"
|
|
|
|
#include "med2.h"
|
|
|
|
#include "ori.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;
|
|
|
|
static float FK_PR = 0.01f;
|
|
static float FK_Y = 0.01f;
|
|
|
|
static float MEGA_BAR;
|
|
static float MEGA_BAR_MIN=0.00001f;
|
|
static float MEGA_BAR_MAX=0.1f;
|
|
static float MEGA_BAR_MUL = 100.0f;
|
|
static float MEGA_BAR_POW = 2.0f;
|
|
static float MEGA_BAR_CNT = 7.0f;
|
|
|
|
ORI_Data DataORI;
|
|
|
|
const int BAR_MedCount=50;
|
|
long BAR_MedData[BAR_MedCount];
|
|
unsigned char BAR_MedIndex[BAR_MedCount];
|
|
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 20};
|
|
|
|
static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
|
|
{
|
|
static float falt=0;
|
|
falt=falt*(1.0f-MEGA_BAR)+(Alt)*MEGA_BAR;
|
|
|
|
static float alt_speed=0, alt_acc=0;
|
|
static int count=0;
|
|
if(count>=MEGA_BAR_CNT)
|
|
{
|
|
static float last_b=0, last_s=0;
|
|
alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
|
|
alt_acc=(alt_speed-last_s);
|
|
last_b=falt;
|
|
last_s=alt_speed;
|
|
count=0;
|
|
}
|
|
else count++;
|
|
|
|
float coef_w, coef_s;
|
|
|
|
coef_w=MEGA_BAR_MIN;
|
|
float sub=fabsf(Alt-falt);
|
|
coef_w*=powf(sub*MEGA_BAR_MUL, MEGA_BAR_POW);
|
|
if(coef_w>MEGA_BAR_MAX) coef_w=MEGA_BAR_MAX;
|
|
if(coef_w<MEGA_BAR_MIN) coef_w=MEGA_BAR_MIN;
|
|
|
|
|
|
MEGA_BAR=coef_w;
|
|
|
|
//static float test_speed=0;
|
|
//test_speed=(test_speed*15.0f+alt_speed)/16.0f;
|
|
|
|
Speed=MED2_Update(alt_speed*1000, BAR_Med)/1000.0f;
|
|
|
|
Acc=alt_acc;
|
|
|
|
return falt;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
/*const int BAR_MedCount=200;
|
|
long BAR_MedData[BAR_MedCount];
|
|
unsigned char BAR_MedIndex[BAR_MedCount];
|
|
MED2_Data BAR_Med = {BAR_MedCount, 0, BAR_MedIndex, BAR_MedData, 100};
|
|
|
|
const int BAR_MedCount2=50;
|
|
long BAR_MedData2[BAR_MedCount2];
|
|
unsigned char BAR_MedIndex2[BAR_MedCount2];
|
|
MED2_Data BAR_Med2 = {BAR_MedCount2, 0, BAR_MedIndex2, BAR_MedData2, 10};
|
|
|
|
static float MegaFilterAlt(const long Freq, const float Alt, float& Speed, float& Acc)
|
|
{
|
|
static float alt_speed=0, last_b=0;
|
|
|
|
float falt=MED2_Update(Alt*1000.0f, BAR_Med)/1000.0f;
|
|
|
|
static int count=0;
|
|
if(count>=MEGA_BAR_CNT)
|
|
{
|
|
static float last_b=0;
|
|
alt_speed=(falt-last_b)*Freq/MEGA_BAR_CNT;
|
|
last_b=falt;
|
|
count=0;
|
|
}
|
|
else count++;
|
|
|
|
Speed=MED2_Update(alt_speed*1000.0f, BAR_Med2)/1000.0f;
|
|
|
|
return falt;
|
|
}*/
|
|
//------------------------------------------------------------------------------
|
|
|
|
|
|
|
|
static float MEGA_ACC=0.01f;
|
|
|
|
static const long MEGA_Count=60; // 600ms at 100Hz
|
|
static float MEGA_Alt_Buff[MEGA_Count];
|
|
static float MEGA_Spd_Buff[MEGA_Count];
|
|
static long MEGA_Index=0;
|
|
|
|
static float MegaFilterAcc(struct ORI_Data& Data)
|
|
{
|
|
float acc;
|
|
|
|
acc=fabsf(Data.Bar.Acc)*100.0f;
|
|
float filt = MEGA_ACC;
|
|
if(acc>1) filt/=acc;
|
|
|
|
acc=fabsf(Data.Iner.Z)*100.0f;
|
|
if(acc>1) filt/=acc;
|
|
|
|
const float g=9.80665f;
|
|
|
|
static float i_speed=0;
|
|
i_speed+=Data.Iner.Z*g/Data.Freq;
|
|
|
|
|
|
MEGA_Alt_Buff[MEGA_Index++]=i_speed;
|
|
if(MEGA_Index>=MEGA_Count) MEGA_Index=0;
|
|
static float delay_speed;
|
|
delay_speed=(delay_speed+MEGA_Alt_Buff[MEGA_Index])/2.0f;
|
|
|
|
float shift=Data.Bar.Speed-delay_speed;
|
|
|
|
for(int a=0; a<MEGA_Count; a++) MEGA_Alt_Buff[a] = MEGA_Alt_Buff[a]*(1.0f-filt) + (MEGA_Alt_Buff[a]+shift)*filt;
|
|
|
|
i_speed = i_speed*(1.0f-filt) + (i_speed+shift)*filt;
|
|
|
|
Data.Speed.Z=i_speed;
|
|
|
|
Data.Pos.Z+=i_speed/Data.Freq;
|
|
|
|
return 0;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void ORI_Init()
|
|
{
|
|
IMU_Init();
|
|
MED2_Init(BAR_Med);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static void GetPos(struct ORI_Data& Data)
|
|
{
|
|
static long land=Data.Bar.Bar;
|
|
float alt=0;
|
|
alt = BAR_GetAltitude(land, Data.Bar.Bar);
|
|
|
|
float alt_speed, alt_acc;
|
|
|
|
float falt=MegaFilterAlt(Data.Freq, alt, alt_speed, alt_acc);
|
|
|
|
Data.Bar.Alt=alt;
|
|
Data.Bar.Filt=falt;
|
|
Data.Bar.Speed=alt_speed;
|
|
Data.Bar.Acc=alt_acc;
|
|
|
|
MegaFilterAcc(Data);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
|
|
static short NormAcc_X[2]{-7890, 8030}, NormAcc_Y[2]{-8090, 7930}, NormAcc_Z[2]{-7910, 7910};
|
|
|
|
static short NormMag_X[2]{-160, 160}, NormMag_Y[2]{-155, 140}, NormMag_Z[2]{-150, 140}; // IST8310
|
|
static short GyroShift[3]{-2, 7, 17};
|
|
|
|
static void NormalizeAcc(short& X, short& Y, short& Z, const short G)
|
|
{
|
|
short x=(NormAcc_X[1]-NormAcc_X[0])/2;
|
|
short y=(NormAcc_Y[1]-NormAcc_Y[0])/2;
|
|
short z=(NormAcc_Z[1]-NormAcc_Z[0])/2;
|
|
|
|
X=(X-(NormAcc_X[0]+x))*G/x;
|
|
Y=(Y-(NormAcc_Y[0]+y))*G/y;
|
|
Z=(Z-(NormAcc_Z[0]+z))*G/z;
|
|
}
|
|
|
|
static void NormalizeMag(short& X, short& Y, short& Z, const short N)
|
|
{
|
|
short x=(NormMag_X[1]-NormMag_X[0])/2;
|
|
short y=(NormMag_Y[1]-NormMag_Y[0])/2;
|
|
short z=(NormMag_Z[1]-NormMag_Z[0])/2;
|
|
|
|
X=(X-(NormMag_X[0]+x))*N/x;
|
|
Y=(Y-(NormMag_Y[0]+y))*N/y;
|
|
Z=(Z-(NormMag_Z[0]+z))*N/z;
|
|
}
|
|
|
|
void ORI_Get(ORI_Data& Data, const unsigned long Freq, IMU_Data& IMU, BAR_Data BAR)
|
|
{
|
|
Data.Freq=Freq;
|
|
//------------------------------ imu perevernuta, x z inverse
|
|
short ax = IMU.Acc.X;
|
|
short ay = IMU.Acc.Y;
|
|
short az = IMU.Acc.Z;
|
|
|
|
NormalizeAcc(ax, ay, az, 8000);
|
|
|
|
Data.Acc.X = ax;
|
|
Data.Acc.Y = ay;
|
|
Data.Acc.Z = az;
|
|
|
|
short gx = (IMU.Gyr.X - GyroShift[0]);
|
|
short gy = IMU.Gyr.Y - GyroShift[1];
|
|
short gz = (IMU.Gyr.Z - GyroShift[2]);
|
|
|
|
Data.Gyr.X = gx;
|
|
Data.Gyr.Y = gy;
|
|
Data.Gyr.Z = gz;
|
|
|
|
short mx = IMU.Mag.X;
|
|
short my = IMU.Mag.Y;
|
|
short mz = IMU.Mag.Z;
|
|
|
|
NormalizeMag(mx, my, mz, 1000);
|
|
|
|
Data.Mag.X = mx;
|
|
Data.Mag.Y = my;
|
|
Data.Mag.Z = mz;
|
|
|
|
static float bar_zero=0;
|
|
static int zero_count=0;
|
|
|
|
if(zero_count<500)
|
|
{
|
|
bar_zero=BAR.Pressure;
|
|
zero_count++;
|
|
}
|
|
|
|
Data.Bar.Bar=BAR.Pressure;
|
|
//Data.Bar.Alt=(bar_zero-BAR.Pressure)*9.0f;
|
|
|
|
Vec3 acc{((float)ax)/8000, ((float)ay)/8000, ((float)az)/8000};
|
|
Vec3 gyr{(float)gx, (float)gy, (float)gz};
|
|
Vec3 mag{(float)mx, (float)my, -(float)mz};
|
|
|
|
|
|
|
|
|
|
ORI o = WorkAccGyroMag(acc, gyr, mag, 90, 0.01f);
|
|
|
|
static int test_pitch, test_roll;
|
|
|
|
Data.Pitch=o.Pitch;
|
|
Data.Roll=o.Roll;
|
|
Data.Yaw=o.Yaw;
|
|
|
|
Data.SinX=o.sinX;
|
|
Data.SinY=o.sinY;
|
|
Data.CosZ=o.cosZ;
|
|
|
|
Data.Iner.X=o.IneX;
|
|
Data.Iner.Y=o.IneY;
|
|
Data.Iner.Z=o.IneZ-1;
|
|
|
|
//Data.Speed.X+=o.IneX*9.8f/100.0f;
|
|
//Data.Speed.Y+=o.IneY*9.8f/100.0f;
|
|
//Data.Speed.Z+=(o.IneZ-1)*9.8f/100.0f;
|
|
|
|
Data.Temp.Acc=IMU.Temp;
|
|
Data.Temp.Bar=BAR.Temp;
|
|
|
|
//GetAngle(Data, Freq);
|
|
//GetCompass(Data);
|
|
//GetIner(Data);
|
|
GetPos(Data);
|
|
}
|