int mpulib_read(mpudata_t *mpu) {
if (mpulib_read_dmp(mpu) != 0)
return -1;
if (mpulib_read_mag(mpu) != 0)
return -1;
calibrate_data(mpu);
return data_fusion(mpu);
}
int mpu9150_read(mpudata_t *mpu)
{
if (mpu9150_read_dmp(mpu) != 0)
return -1;
#ifdef AK89xx_SECONDARY
if (mpu9150_read_mag(mpu) != 0)
return -1;
#endif
calibrate_data(mpu);
return data_fusion(mpu);
}
int mpu9250_read(mpudata_t *mpu)
{
//printf("read dmp\n");
if (mpu9250_read_dmp(mpu) != 0){
// printf("read dmp failed\n");
return -1;
}
calibrate_data(mpu);
return data_fusion(mpu);
}
bool FusionLocalAlgoTest2(FusionInput &input, FusionOutput &output)
{
// This is how to use global var.
/*
g_GlobalVar[0].m_G[0] = 0;
g_GlobalVar[0].m_G[1] = 0;
map<SensorId, NoiseDataPacket> &noiseDatas = input.m_NoiseDataPackets;
int interval = input.m_Interval;
int nTargets = noiseDatas[SensorIdRadar].m_TargetNoiseDatas.size();
for (int iTarget = 0; iTarget < nTargets; ++iTarget)
{
TrueDataFrame frame;
frame.m_Time = noiseDatas[SensorIdRadar].m_TargetNoiseDatas[iTarget].m_Time;
frame.m_Id = 100 + iTarget + frame.m_Time / 100 * 100;
frame.m_Type = iTarget % TargetTypeLast;
for (int iSensor = SensorIdRadar; iSensor < SensorIdLast; ++iSensor)
{
// frame += noiseDatas[(SensorId)iSensor].m_TargetNoiseDatas[iTarget];
}
frame.m_Pos = Position(800 - frame.m_Time, 100 + iTarget * 250, 100);
frame.m_Vel = Velocity(-1, 0, 0);
frame.m_IsKeyTarget = iTarget % 2 ? true : false;
// frame /= SensorIdLast;
output.m_FusionDataPacket.m_FusionDatas.push_back(frame);
// frame = noiseDatas[SensorIdRadar].m_TargetNoiseDatas[iTarget];
}
*/
FusionMethod = 2;
FrameRead_RadarAIS(input);
FrameRead_TongLei(input);
data_fusion(&sEcho_TongLei_Org[0],&sEcho_RadarAIS_Org[0],&iEchoNum_RadarAIS_Org,&iEchoNum_TongLei_Org,&iSpt_Org,&sTracks_Org[0],&sECHO_Param_Org,&g_iTrkNum_Org,&FusionMethod);
OutFile(iSpt_Org, output);
iSpt_Org++;
return true;
}
