51 lines
997 B
C++
51 lines
997 B
C++
#include "HAL.h"
|
|
#include <stdlib.h>
|
|
#include "Pedometer.h"
|
|
#include "mpu9250.h"
|
|
#include "Common/DataProc/DataProc.h"
|
|
#include <stdio.h>
|
|
|
|
static CommitFunc_t CommitFunc;
|
|
static void* UserData;
|
|
|
|
void IMU_SetCommitCallback(CommitFunc_t func, void* userData)
|
|
{
|
|
CommitFunc = func;
|
|
UserData = userData;
|
|
}
|
|
|
|
uint8_t IMU_Init()
|
|
{
|
|
MPU9250_Init();
|
|
Pedomter_Init();
|
|
return 0;
|
|
}
|
|
|
|
void IMU_Update()
|
|
{
|
|
float temp;
|
|
|
|
IMU_Info_t imu;
|
|
|
|
MPU9250_GetData(&imu.acc[0], &imu.acc[1], &imu.acc[2],
|
|
&imu.gyr[0], &imu.gyr[1], &imu.gyr[2], &temp);
|
|
|
|
|
|
imu.ax = (int16_t)(imu.acc[0] * 2000);
|
|
imu.ay = (int16_t)(imu.acc[1] * 2000);
|
|
imu.az = (int16_t)(imu.acc[2] * 2000);
|
|
|
|
imu.gx = rand() % 1000 - 500;
|
|
imu.gy = rand() % 1000 - 500;
|
|
imu.gz = rand() % 1000 - 500;
|
|
|
|
imu.steps = Pedometer_Update(imu.ax, imu.ay, imu.az);
|
|
|
|
printf("%.2f, %.2f, %.2f\n", imu.acc[0], imu.acc[1], imu.acc[2]);
|
|
if (CommitFunc)
|
|
{
|
|
CommitFunc(&imu, UserData);
|
|
}
|
|
}
|
|
|