111 lines
2.2 KiB
C++
111 lines
2.2 KiB
C++
#include "stm32g4xx.h"
|
|
#include "Tick.h"
|
|
#include "Tim.h"
|
|
#include "IIMU.h"
|
|
#include "ICM20948.h"
|
|
#include "IRS.h"
|
|
#include "Vector.h"
|
|
#include "Autopilot.h"
|
|
|
|
extern "C" void SystemClock_Config();
|
|
|
|
constexpr long TIM_PRIORITY = 2;
|
|
|
|
IMU_Data DataIMU;
|
|
|
|
IRS MainIRS;
|
|
|
|
IIMU* IMUObj;
|
|
|
|
float GetCurrentTime()
|
|
{
|
|
return ((float)TICK_GetCount()) / 1000.0f;
|
|
}
|
|
|
|
void DoneProcIMU(IMU_Data& Data)
|
|
{
|
|
DataIMU = Data;
|
|
// MagReady=true;
|
|
}
|
|
|
|
void TimerUpdateSensor()
|
|
{
|
|
//IMUObj->GetAsync(DoneProcIMU);
|
|
if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
|
|
//if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
|
|
}
|
|
|
|
void TimerUpdateMain()
|
|
{
|
|
float time = GetCurrentTime();
|
|
|
|
Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
|
|
Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
|
|
|
|
// Vector3 mag;
|
|
// if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
|
|
// else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
|
|
|
|
MainIRS.UpdateGyro(gyr);
|
|
MainIRS.UpdateAccel(acc);
|
|
/*if(MagReady)
|
|
{
|
|
MagReady=false;
|
|
MainIRS.UpdateMagnet(mag);
|
|
}
|
|
|
|
MainGPS.UpdateAverage(time);
|
|
MainBar.UpdateAverage();
|
|
MainOF.UpdateAverage(IRS::Period);
|
|
MainLen.UpdateAverage();
|
|
|
|
if(DataFLOW.Update)
|
|
{
|
|
DataFLOW.Update=false;
|
|
UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
|
|
}
|
|
|
|
if(DataTOF.Update)
|
|
{
|
|
DataTOF.Update=false;
|
|
UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
|
|
}
|
|
|
|
UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
|
|
|
|
if(DataGPS.Update)
|
|
{
|
|
DataGPS.Update=false;
|
|
Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
|
|
UpdateGPS(gps, time, DataGPS.Valid);
|
|
}*/
|
|
|
|
Vector3 pos;
|
|
MainIRS.RestoreAllShift(pos);
|
|
/*MainVel.PointBegin-=pos;
|
|
|
|
AutoControl(gyr);
|
|
|
|
ReadStateINS();
|
|
|
|
if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
|
|
}
|
|
|
|
int main()
|
|
{
|
|
SystemClock_Config(); // 170MHz
|
|
IMU_InitPower();
|
|
|
|
TICK_Init();
|
|
|
|
TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
|
|
|
|
IMUObj = TryFindIMU(MainDrone.IMUInit);
|
|
|
|
TIM6_Enable();
|
|
|
|
while (true)
|
|
{
|
|
|
|
}
|
|
} |