add firmware
This commit is contained in:
Dana Markova
281
dev/ori.cpp
Normal file
281
dev/ori.cpp
Normal file
@ -0,0 +1,281 @@
|
||||
#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);
|
||||
}
|
||||
Reference in New Issue
Block a user