bool MPUManager::__LoadDataFromDevice( __CaptureData_t *p_pData )
{
static short sDataGyro[3] = {0};
static short sDataAccel[3] = {0};
static unsigned short ucLen = 0;
static unsigned char ucMore = 0;
static short sMask;
static long lQuaternion[4];
if( 0 == __mpu_get_fifo_bytes_count(&ucLen) && ucLen >= m_usPackageLength )
{
if( 0 == dmp_read_fifo( sDataGyro, sDataAccel, lQuaternion, &p_pData->m_ulTimestamp, &sMask, &ucMore ) )
{
if( m_usDefaultFeatures&DMP_FEATURE_SEND_CAL_GYRO )
{
p_pData->m_sDataGyro[0] = (float)sDataGyro[0]/m_fCurGyroSensitivity;
p_pData->m_sDataGyro[1] = (float)sDataGyro[1]/m_fCurGyroSensitivity;
p_pData->m_sDataGyro[2] = (float)sDataGyro[2]/m_fCurGyroSensitivity;
}
if( m_usDefaultFeatures&DMP_FEATURE_SEND_RAW_ACCEL )
{
p_pData->m_sDataAccel[0] = (float)sDataAccel[0]/m_usCurAccelSensitivity;
p_pData->m_sDataAccel[1] = (float)sDataAccel[1]/m_usCurAccelSensitivity;
p_pData->m_sDataAccel[2] = (float)sDataAccel[2]/m_usCurAccelSensitivity;
}
if( m_usDefaultFeatures&DMP_FEATURE_6X_LP_QUAT )
{
p_pData->m_fDataQuat[0] = (float)lQuaternion[0]/QUATERNION_CONSTANT;
p_pData->m_fDataQuat[1] = (float)lQuaternion[1]/QUATERNION_CONSTANT;
p_pData->m_fDataQuat[2] = (float)lQuaternion[2]/QUATERNION_CONSTANT;
p_pData->m_fDataQuat[3] = (float)lQuaternion[3]/QUATERNION_CONSTANT;
}
if( !(m_usDefaultFeatures&DMP_FEATURE_SEND_TEMPERATURE) || 0 != __mpu_get_temperature( &p_pData->m_fTemperature ) )
{
p_pData->m_fTemperature = 0.0f;
}
return true;
}
}
return false;
}
/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
}
void main(void)
{
int_clk(); //时钟初始化
P2DIR|=BIT4+BIT5; //陀螺仪I2C端口初始化
MPU6050_INIT();
Bluth_UART_Init(); //蓝牙串口
while(!Pid_Set_OK);//等待设置PID
PWM_INIT(); //四路PWM初始化
TIMEA1_INIT(); //PID控制5MS中断初始化 中断初始化必须在 ESP模块后面 因为代码中嵌套了对串口中断的使用
// HC_RS04_Init();
// TIMEA2_INIT();
while(1)
{
// HC_RS04_Send();
// delay_ms(100);
dmp_read_fifo(gyro, accel, quad, &sensor_timestamp, &sensors, &more); //读取角速度角速度四元数 经过测试发现该函数执行的时间竟然是不确定的，不适合放在中断，
}
}
int mympu_update() {
do {
ret = dmp_read_fifo(gyro,NULL,q._l,NULL,&sensors,&fifoCount);
/* will return:
0 - if ok
1 - no packet available
2 - if BIT_FIFO_OVERFLOWN is set
3 - if frame corrupted
<0 - if error
*/
if (ret!=0) return ret;
} while (fifoCount>1);
q._f.w = (float)q._l[0] / (float)QUAT_SENS;
q._f.x = (float)q._l[1] / (float)QUAT_SENS;
q._f.y = (float)q._l[2] / (float)QUAT_SENS;
q._f.z = (float)q._l[3] / (float)QUAT_SENS;
mympu.quat.w = q._f.w;
mympu.quat.x = q._f.x;
mympu.quat.y = q._f.y;
mympu.quat.z = q._f.z;
quaternionToEuler( &q._f, &mympu.ypr[2], &mympu.ypr[1], &mympu.ypr[0] );
/* need to adjust signs and do the wraps depending on the MPU mount orientation */
/* if axis is no centered around 0 but around i.e 90 degree due to mount orientation */
/* then do: mympu.ypr[x] = wrap_180(90.f+rad2deg(mympu.ypr[x])); */
mympu.ypr[0] = rad2deg(mympu.ypr[0]);
mympu.ypr[1] = rad2deg(mympu.ypr[1]);
mympu.ypr[2] = rad2deg(mympu.ypr[2]);
/* need to adjust signs depending on the MPU mount orientation */
mympu.gyro[0] = -(float)gyro[2] / GYRO_SENS;
mympu.gyro[1] = (float)gyro[1] / GYRO_SENS;
mympu.gyro[2] = (float)gyro[0] / GYRO_SENS;
return 0;
}
u8 get_dmp()
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT)
{
q0 = (float)quat[0] / q30;
q1 = (float)quat[1] / q30;
q2 = (float)quat[2] / q30;
q3 = (float)quat[3] / q30;
mpu_gryo_pitch = MoveAve_WMA(gyro[0], MPU6050_GYR_FIFO[0], 8);
mpu_gryo_roll = MoveAve_WMA(gyro[1], MPU6050_GYR_FIFO[1], 8);
mpu_gryo_yaw = MoveAve_WMA(gyro[2], MPU6050_GYR_FIFO[2], 8);
ahrs.gryo_pitch = -mpu_gryo_pitch * gyroscale / 32767.0f;
ahrs.gryo_roll = -mpu_gryo_roll * gyroscale / 32767.0f;
ahrs.gryo_yaw = -mpu_gryo_yaw * gyroscale / 32767.0f;
ahrs.degree_roll = -asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3f + settings.angle_diff_roll; //+ Pitch_error; // pitch
ahrs.degree_pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3f + settings.angle_diff_pitch; //+ Roll_error; // roll
if (!has_hmc5883)
ahrs.degree_yaw = atan2(2 * (q1*q2 + q0*q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 57.3f; //+ Yaw_error;
ahrs.time_span = Timer4_GetSec();
if (en_out_ahrs)
rt_kprintf("%d,%d,%d %d\n",
(s32)(ahrs.degree_pitch),
(s32)(ahrs.degree_roll),
(s32)(ahrs.degree_yaw),
(u32)(1.0f / ahrs.time_span));
rt_event_send(&ahrs_event, AHRS_EVENT_Update);
dmp_retry = 0;
return 1;
}
lost:
dmp_retry++;
return 0;
}
void MPU9250_GetDMPData()
{
short gyro[4];
short accel[4];
long quat[4];
unsigned long sensor_timestamp;
short sensors;
unsigned char more;
float q0, q1, q2, q3;
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if(sensors & INV_XYZ_ACCEL)
{
Accel.X = accel[0];
Accel.Y = accel[1];
Accel.Z = accel[2];
}
if(sensors & INV_XYZ_GYRO)
{
Gyro.X = -gyro[0];
Gyro.Y = -gyro[1];
Gyro.Z = gyro[2];
}
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / 1073741824.0;
q1=quat[1] / 1073741824.0;
q2=quat[2] / 1073741824.0;
q3=quat[3] / 1073741824.0;
Att.Roll = -asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Att.Pitch = -atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Att.Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_3, ~GPIOPinRead(GPIO_PORTE_BASE, GPIO_PIN_3) );
}
}
inv_error_t MPU9250_DMP::dmpUpdateFifo(void)
{
short gyro[3];
short accel[3];
long quat[4] = {0};
unsigned long timestamp;
short sensors;
unsigned char more;
if (dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more)
!= INV_SUCCESS)
{
return INV_ERROR;
}
if (sensors & INV_XYZ_ACCEL)
{
ax = accel[X_AXIS];
ay = accel[Y_AXIS];
az = accel[Z_AXIS];
}
if (sensors & INV_X_GYRO)
gx = gyro[X_AXIS];
if (sensors & INV_Y_GYRO)
gy = gyro[Y_AXIS];
if (sensors & INV_Z_GYRO)
gz = gyro[Z_AXIS];
if (sensors & INV_WXYZ_QUAT)
{
qw = quat[0];
qx = quat[1];
qy = quat[2];
qz = quat[3];
}
time = timestamp;
return INV_SUCCESS;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t MPU_DMP_Get_Data(DMP_Data *dd)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(dd->gyro, dd->accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat[0] / q30; //q30格式转换为浮点数
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
//计算得到俯仰角/横滚角/航向角
dd->yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
dd->pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
dd->roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
// 在方向定义处将X和Y颠倒, 在解析Pitch和Roll时再颠倒回来, 以便使Pitch能够支持在-180~180度
//dd->yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
//dd->pitch = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
//dd->roll = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
}else return 2;
return 0;
}
int ms_update() {
if (!dmpReady) {
printf("Error: DMP not ready!!\n");
return -1;
}
while (dmp_read_fifo(g,a,_q,&sensors,&fifoCount)!=0); //gyro and accel can be null because of being disabled in the efeatures
q = _q;
GetGravity(&gravity, &q);
GetYawPitchRoll(ypr, &q, &gravity);
mpu_get_temperature(&t);
temp=(float)t/65536L;
mpu_get_compass_reg(c);
//scaling for degrees output
for (int i=0;i<DIM;i++){
ypr[i]*=180/M_PI;
}
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of Pitch, MPU is attached upside down
ypr[1]*=-1.0;
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
accel[i] = (float)(a[DIM-i-1]);
compass[i] = (float)(c[DIM-i-1]);
}
return 0;
}
/**
****************************************************************************************
* @brief read data from mpu6050
* @description
* This function is to use DMP to read data from mpu6050 sensor.
*****************************************************************************************
*/
int mpu_read(MPU_Data* data)
{
#define q30 1073741824.0f
#define u8 uint8_t
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short sensors;
unsigned char more;
long quat[4];
dmp_read_fifo(data->gyro, data->accel, quat, &sensor_timestamp, &sensors,&more);
/* Gyro and accel data are written to the FIFO by the DMP in chip
* frame and hardware units. This behavior is convenient because it
* keeps the gyro and accel outputs of dmp_read_fifo and
* mpu_read_fifo consistent.
*/
/* Unlike gyro and accel, quaternions are written to the FIFO in
* the body frame, q30. The orientation is set by the scalar passed
* to dmp_set_orientation during initialization.
*/
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
data->Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
data->Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
data->Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
return MPU_OK;
}
return MPU_ERR;
}
int mpu6050_readangle_dmp(float *pitch_dmp,float *roll_dmp,float *yaw_dmp,int16_t *gyro_x,int16_t *gyro_y,int16_t *gyro_z)
{
static int16_t gyro_y_save=0,gyro_z_save=0;
static float roll_save =0,pitch_save =0,yaw_save =0;
float GyroscopeAngleSpeed;
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
int16_t gdata[3];
uint8_t buf[6];
while(I2C_BurstRead(HW_I2C0, 0x68, 0x43, 1, buf, 6));
gdata[0] = (int16_t)(((uint16_t)buf[0]<<8)+buf[1]);
gdata[1] = (int16_t)(((uint16_t)buf[2]<<8)+buf[3]);
gdata[2] = (int16_t)(((uint16_t)buf[4]<<8)+buf[5]);
if(sensors & INV_XYZ_GYRO )
{
q0 = quat[0] / q30;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
*pitch_dmp = pitch_save= Pitch;
*roll_dmp = roll_save = Roll;
*yaw_dmp = yaw_save = Yaw;
*gyro_x = gdata[0]+ flashcontrol_read(GYROSCOPE_OFFSET);
*gyro_y = gyro_y_save = gyro[1];
*gyro_z = gyro_z_save = gyro[2];
return 0;//success
}
GyroscopeAngleSpeed = (gdata[0] + flashcontrol_read(GYROSCOPE_OFFSET)) *flashcontrol_read(GYROSCOPE_ANGLE_RATIO);
roll_save += GyroscopeAngleSpeed;
*pitch_dmp = pitch_save;
*roll_dmp = roll_save ;
*yaw_dmp = yaw_save ;
*gyro_x = gdata[0]+ flashcontrol_read(GYROSCOPE_OFFSET);
*gyro_y = gyro_y_save;
*gyro_z = gyro_z_save;
return 1;
}
boolean MPU9150Lib::read()
{
short intStatus;
int result;
short sensors;
unsigned char more;
unsigned long timestamp;
mpu_get_int_status(&intStatus); // get the current MPU state
if ((intStatus & MPU_INT_STATUS_DMP_0) == 0) // return false if definitely not ready yet
return false;
// get the data from the fifo
if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, ×tamp, &sensors, &more)) != 0) {
return false;
}
// got the fifo data so now get the mag data
if ((result = mpu_get_compass_reg(m_rawMag, ×tamp)) != 0) {
#ifdef MPULIB_DEBUG
Serial.print("Failed to read compass: ");
Serial.println(result);
return false;
#endif
}
// got the raw data - now process
m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W]; // get float version of quaternion
m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
MPUQuaternionNormalize(m_dmpQuaternion); // and normalize
MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
// *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
if (m_useMagCalibration) {
m_calMag[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
}
else {
m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
}
// Scale accel data
if (m_useAccelCalibration) {
m_calAccel[VEC3_X] = -(short)(((long)m_rawAccel[VEC3_X] * (long)SENSOR_RANGE) / (long)m_accelXRange);
m_calAccel[VEC3_Y] = (short)(((long)m_rawAccel[VEC3_Y] * (long)SENSOR_RANGE) / (long)m_accelYRange);
m_calAccel[VEC3_Z] = (short)(((long)m_rawAccel[VEC3_Z] * (long)SENSOR_RANGE) / (long)m_accelZRange);
}
else {
m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
}
dataFusion();
return true;
}
int ms_open(unsigned int rate) {
dmpReady=1;
initialized = 0;
// initialize device
printf("Initializing MPU...\n");
r=mpu_init(NULL);
if (r != 0) {
printf("MPU init failed! %i\n",r);
return -1;
}
printf("Setting MPU sensors...\n");
if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to set sensors!\n");
return -1;
}
printf("Setting GYRO sensitivity...\n");
if (mpu_set_gyro_fsr(FSR)!=0) {
printf("Failed to set gyro sensitivity!\n");
return -1;
}
printf("Setting ACCEL sensitivity...\n");
if (mpu_set_accel_fsr(2)!=0) {
printf("Failed to set accel sensitivity!\n");
return -1;
}
// verify connection
printf("Powering up MPU...\n");
mpu_get_power_state(&devStatus);
printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);
//fifo config
printf("Setting MPU fifo...\n");
if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to initialize MPU fifo!\n");
return -1;
}
// load and configure the DMP
printf("Loading DMP firmware...\n");
if (dmp_load_motion_driver_firmware()!=0) {
printf("Failed to load DMP firmware!\n");
return -1;
}
printf("Setting GYRO orientation...\n");
if (dmp_set_orientation( inv_orientation_matrix_to_scalar( gyro_orientation ) )!=0) {
printf("Failed to set gyro orientation!\n");
return -1;
}
printf("Setting DMP fifo rate to: %i\n",rate);
if (dmp_set_fifo_rate(rate)!=0) {
printf("Failed to set dmp fifo rate!\n");
return -1;
}
printf("Activating DMP...\n");
if (mpu_set_dmp_state(1)!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
//dmp_set_orientation()
//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Configuring DMP...\n");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
//if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Failed to enable DMP features!\n");
return -1;
}
printf("Resetting fifo queue...\n");
if (mpu_reset_fifo()!=0) {
printf("Failed to reset fifo!\n");
return -1;
}
printf("Checking... ");
do {
delay_ms(5*1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate
r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
} while (r!=0 || fifoCount<5); //packtets!!!
printf("Collected: %i packets.\n",fifoCount);
ms_update();
initialized = 1;
uint16_t lpf,g_fsr,a_sens,dmp_rate;
uint8_t dmp,a_fsr;
float g_sens;
printf("MPU config:\n");
mpu_get_lpf(&lpf);
printf("MPU LPF: %u\n",lpf);
dmp_get_fifo_rate(&dmp_rate);
printf("DMP Fifo Rate: %u\n",dmp_rate);
mpu_get_dmp_state(&dmp);
printf("MPU DMP State: %u\n",dmp);
mpu_get_gyro_fsr(&g_fsr);
printf("MPU gyro FSR: %u\n",g_fsr);
mpu_get_gyro_sens(&g_sens);
printf("MPU gyro sens: %2.3f\n",g_sens);
mpu_get_accel_fsr(&a_fsr);
printf("MPU accel FSR: %u\n",a_fsr);
mpu_get_accel_sens(&a_sens);
printf("MPU accel sens: %u\n",a_sens);
return 0;
}
int main(void)
{
uint8_t more, ack;
uint8_t rf_pckt_ok = 0, rf_pckt_lost = 0;
uint8_t voltage_counter = 0, temperature_counter = 0;
bool read_result;
mpu_packet_t pckt;
hw_init();
for (;;)
{
pckt.flags = 0; // reset the flags
// get the battery voltage every VOLTAGE_READ_EVERY iterations
if (++voltage_counter == VOLTAGE_READ_EVERY)
{
pckt.flags |= FLAG_VOLTAGE_VALID;
pckt.voltage = get_battery_voltage();
voltage_counter = 0;
}
// same as with the voltage, send the temperature
if (++temperature_counter == TEMPERATURE_READ_EVERY)
{
pckt.flags |= FLAG_TEMPERATURE_VALID;
mpu_get_temperature(&pckt.temperature);
temperature_counter = 0;
}
// Waits for the interrupt from the MPU-6050.
// Instead of polling, I should put the MCU to sleep and then have it awaken by the MPU-6050.
// However, I have not succeeded in the making the wakeup work reliably.
while (MPU_IRQ)
dbgPoll();
while (!MPU_IRQ)
;
do {
// read all the packets in the MPU fifo
do {
read_result = dmp_read_fifo(&pckt, &more);
} while (more);
if (read_result)
{
pckt.flags |= (RECENTER_BTN == 0 ? FLAG_RECENTER : 0);
// send the message
if (rf_head_send_message(&pckt, sizeof(pckt)))
++rf_pckt_ok;
else
++rf_pckt_lost;
// update the LEDs
if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL)
{
if (rf_pckt_ok > rf_pckt_lost)
LED_GREEN = 1;
else
LED_RED = 1;
} else if (rf_pckt_lost + rf_pckt_ok == LED_PCKT_TOTAL + LED_PCKT_LED_ON) {
LED_RED = 0;
LED_GREEN = 0;
rf_pckt_ok = rf_pckt_lost = 0;
}
// check for an ACK payload
if (rf_head_read_ack_payload(&ack, 1))
{
if (ack == CMD_CALIBRATE)
{
mpu_calibrate_bias();
} else if (ack == CMD_READ_TRACKER_SETTINGS) {
rf_head_send_message(get_tracker_settings(), sizeof(tracker_settings_t));
} else if (ack >= CMD_RF_PWR_LOWEST && ack <= CMD_RF_PWR_HIGHEST) {
tracker_settings_t new_settings;
memcpy(&new_settings, get_tracker_settings(), sizeof(tracker_settings_t));
new_settings.rf_power = ack;
save_tracker_settings(&new_settings);
}
}
}
} while (more);
}
}
// Protected
void IMUTask::task()
{
int8_t result;
// eventGroup for notifications
eventGroup = xEventGroupCreate();
if (eventGroup == NULL) {
debug::log("IMUTask: Failed to create eventGroup");
}
EventBits_t uxBits;
/* Push both gyro and accel data into the FIFO. */
// this starts the interrupts firing
//result = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//debug::log("IMUTask: mpu_configure_fifo returned " + String(result));
setup();
// everthing setup start the dmp generating interrupts
enable_dmp();
while(true) {
uxBits = xEventGroupWaitBits(eventGroup,
IMU_INT_BIT,
pdFALSE,
pdFALSE,
portMAX_DELAY);
if( ( uxBits & IMU_INT_BIT ) != 0 ) {
// interrupt has fired
if (dmp_state == IMU_DMP_ON) {
// interrupt as a result of DMP
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more = 0;
long quat[4];
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
int success = dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
&more);
if (!more) {
//hal.new_gyro = 0;
// TODO: more data to pull need a way to fetch it
}
// lets try printing the
} else {
// TODO: interrupt not from DMP
}
xEventGroupClearBits(eventGroup,
IMU_INT_BIT);
} else {
// wait timed out
// SerialUSB.println("IMUTask: Wait time out");
}
}
}
int mpu6050_init()
{
/*
** Configures I2C to Master mode to generate start
** condition on I2C bus and to transmit data at a
** speed of 100khz
*/
SetupI2C();
rt_hw_interrupt_install(SYS_INT_I2C0INT, mpu6050_isr, NULL, "mpu6050");
rt_hw_interrupt_mask(SYS_INT_I2C0INT);
int result = mpu_init();
if(!result)
{
rt_kprintf("mpu initialization complete......\n "); //mpu_set_sensor
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
rt_kprintf("mpu_set_sensor complete ......\n");
else
rt_kprintf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
rt_kprintf("mpu_configure_fifo complete ......\n");
else
rt_kprintf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
rt_kprintf("mpu_set_sample_rate complete ......\n");
else
rt_kprintf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
rt_kprintf("dmp_load_motion_driver_firmware complete ......\n");
else
rt_kprintf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
rt_kprintf("dmp_set_orientation complete ......\n");
else
rt_kprintf("dmp_set_orientation come across error ......\n");
if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL)) //dmp_enable_feature
rt_kprintf("dmp_enable_feature complete ......\n");
else
rt_kprintf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
rt_kprintf("dmp_set_fifo_rate complete ......\n");
else
rt_kprintf("dmp_set_fifo_rate come across error ......\n");
run_self_test();
if(!mpu_set_dmp_state(1))
rt_kprintf("mpu_set_dmp_state complete ......\n");
else
rt_kprintf("mpu_set_dmp_state come across error ......\n");
}
while(1)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
&more);
/* Gyro and accel data are written to the FIFO by the DMP in chip
* frame and hardware units. This behavior is convenient because it
* keeps the gyro and accel outputs of dmp_read_fifo and
* mpu_read_fifo consistent.
*/
/* if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in
* the body frame, q30. The orientation is set by the scalar passed
* to dmp_set_orientation during initialization.
*/
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
// printf("Pitch=%.2f ,Roll=%.2f ,Yaw=%.2f \n",Pitch,Roll,Yaw);
UART1_ReportIMU(Yaw*10, Pitch*10, Roll*10,0,0,0,100);
delay_ms(10);
}
}
return 0;
}
/**
* @brief Main program
* @param None
* @retval None
*/
int main(void)
{
SysTick_Config(SystemCoreClock / 1000);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_ResetBits(GPIOE, GPIO_Pin_2); //start with gps off to make sure it activates when wanted
GPIO_Start();
ADC_Start();
Flash_Start();
unsigned long tickey = getSysTick()+1000;
GPIO_ResetBits(GPIOA, GPIO_Pin_10); //LCD Reset must be held 10us
GPIO_SetBits(GPIOG, GPIO_Pin_3); //flash deselect
GPIO_SetBits(GPIOC, GPIO_Pin_8); //flash #hold off, we have dedicated pins
GPIO_SetBits(GPIOC, GPIO_Pin_1); //osc enable
GPIO_ResetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOE, GPIO_Pin_6); //buck enable
while(getSysTick()<tickey);
GPIO_SetBits(GPIOE, GPIO_Pin_2); //gps on/off
GPIO_SetBits(GPIOC, GPIO_Pin_11); //xbee reset
GPIO_SetBits(GPIOA, GPIO_Pin_10); //LCD unreset
UART4_Start();
UART5_Start();
MPU_Start();
//========================BUTTONS====================
InitButton(&button1, GPIOE, GPIO_Pin_4);
#ifdef BOARD_V1
InitButton(&button2, GPIOE, GPIO_Pin_5);
#else
InitButton(&button2, GPIOA, GPIO_Pin_9);
#endif
//=======================END BUTTONS==================
/* LCD Configuration */
LCD_Config();
/* Enable The LCD */
LTDC_Cmd(ENABLE);
LCD_SetLayer(LCD_FOREGROUND_LAYER);
GUI_ClearBackground();
int count = 0;
delay(20000);
#ifndef ORIGIN
GUI_InitNode(1, 72, 83, 0xe8ec);
GUI_InitNode(2, 86, 72, 0xfd20);
GUI_InitNode(3, 'R', 'F', 0x001f);
#endif
int screencount = 0;
#ifdef INSIDE
origin_state.lati=KRESGE_LAT;
origin_state.longi=KRESGE_LONG;
origin_state.gpslock=1;
#endif
unsigned long tickey2 = getSysTick()+2000; //2 second counter
unsigned long tickey3 = getSysTick()+4000; //4 second delay to check gps state
/* Infinite loop */
while (1)
{
UpdateButton(&button1);
UpdateButton(&button2);
if( buttonRisingEdge(&button1)){//right
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);//yellow
//UART_Transmit(&huart4, gps_init_msg, cmdData1Len, 500);
origin_state.pingnum+=1;
origin_state.pingactive=1;
origin_state.whodunnit = origin_state.id;
origin_state.pingclearedby = 0;
}
if(buttonRisingEdge(&button2)){//left
//UART_Transmit(&huart4, gps_get_time_msg, cmdData2Len, 500);
GPIO_ToggleBits(GPIOA, GPIO_Pin_2); //green
if(origin_state.pingactive&&(origin_state.whodunnit != origin_state.id)){
origin_state.pingactive=0;
}
}
if(origin_state.gpson>2 &&(getSysTick()>tickey3)){
GPIO_ResetBits(GPIOE, GPIO_Pin_2);
delay(20000);
GPIO_SetBits(GPIOE, GPIO_Pin_2);
delay(20000);
char setme[80];
sprintf(setme, "%s%c%c", gps_init_msg, 0x0D, 0x0A);
UART_Transmit(UART4, setme, sizeof(setme)/sizeof(setme[0]), 5000);
origin_state.gpson=0;
tickey3+=4000;
}
if(getReset()){
NVIC_SystemReset();
}
#ifdef ORIGIN
// long actHeading=0;
// inv_get_sensor_type_heading(&actHeading, &headingAcc, &headingTime);
// degrees=((double)actHeading)/((double)65536.0);
// origin_state.heading=degrees;
long actHeading[3] = {0,0,0};
inv_get_sensor_type_euler(actHeading, &headingAcc, &headingTime);
degrees=((double)actHeading[2])/((double)65536.0);
//origin_state.heading=degrees;
long tempyraiture;
mpu_get_temperature(&tempyraiture, NULL);
// short garbage[3];
// mpu_get_compass_reg(garbage, NULL);
// double compass_angle = atan2(-garbage[0], -garbage[1])*180/3.1415;
// //origin_state.heading = .9*degrees + .1*compass_angle;
// origin_state.heading = compass_angle;
#endif
if(getSysTick()>tickey2){
tickey2 +=2000;
sendMessage();
}
processGPS();
processXbee();
if(getSysTick()>tickey){
tickey +=53;
GPIO_ToggleBits(GPIOC, GPIO_Pin_3);
#ifndef ORIGIN
GUI_UpdateNode(1, degrees*3.1415/180.0+3.14*1.25, screencount, (screencount>10), 0);
GUI_UpdateNode(2, degrees*3.1415/180.0+3.14, screencount, (screencount>30), 0);
GUI_UpdateNode(3, degrees*3.1415/180.0+0, screencount, (screencount>50), 0);
#else
GUI_UpdateNodes();
#endif
GUI_UpdateArrow(-degrees*3.1415/180.0);
GUI_UpdateBattery(getBatteryStatus());
GUI_DrawTime();
if (count > 50){
GUI_UpdateBottomButton(1, 0xe8ec);
} else {
GUI_UpdateBottomButton(0, 0);
}
GUI_Redraw();
screencount += 1;
#ifndef ORIGIN
degrees += 3.6;
if (screencount%100 == 0){
screencount = 0;
degrees = 0;
}
#else
if (screencount%100 == 0){
screencount = 0;
}
#endif
}
//Sensors_I2C_ReadRegister((unsigned char)0x68, (unsigned char)MPU_WHOAMI, 1, inImu);
//==================================IMU================================
unsigned long sensor_timestamp;
int new_data = 0;
get_tick_count(×tamp);
#ifdef COMPASS_ENABLED
/* We're not using a data ready interrupt for the compass, so we'll
* make our compass reads timer-based instead.
*/
if ((timestamp > hal.next_compass_ms) && !hal.lp_accel_mode &&
hal.new_gyro && (hal.sensors & COMPASS_ON)) {
hal.next_compass_ms = timestamp + COMPASS_READ_MS;
new_compass = 1;
}
#endif
/* Temperature data doesn't need to be read with every gyro sample.
* Let's make them timer-based like the compass reads.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.motion_int_mode) {
/* Enable motion interrupt. */
mpu_lp_motion_interrupt(500, 1, 5);
/* Notify the MPL that contiguity was broken. */
inv_accel_was_turned_off();
inv_gyro_was_turned_off();
inv_compass_was_turned_off();
inv_quaternion_sensor_was_turned_off();
/* Wait for the MPU interrupt. */
while (!hal.new_gyro) {}
/* Restore the previous sensor configuration. */
mpu_lp_motion_interrupt(0, 0, 0);
hal.motion_int_mode = 0;
}
if (!hal.sensors || !hal.new_gyro) {
continue;
}
if (hal.new_gyro && hal.lp_accel_mode) {
short accel_short[3];
long accel[3];
mpu_get_accel_reg(accel_short, &sensor_timestamp);
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
hal.new_gyro = 0;
} else if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
inv_build_quat(quat, 0, sensor_timestamp);
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel_short[3];
unsigned char sensors, more;
long accel[3], temperature;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
}
#ifdef COMPASS_ENABLED
if (new_compass) {
short compass_short[3];
long compass[3];
new_compass = 0;
/* For any MPU device with an AKM on the auxiliary I2C bus, the raw
* magnetometer registers are copied to special gyro registers.
*/
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp)) {
compass[0] = (long)compass_short[0];
compass[1] = (long)compass_short[1];
compass[2] = (long)compass_short[2];
/* NOTE: If using a third-party compass calibration library,
* pass in the compass data in uT * 2^16 and set the second
* parameter to INV_CALIBRATED | acc, where acc is the
* accuracy from 0 to 3.
*/
inv_build_compass(compass, 0, sensor_timestamp);
}
new_data = 1;
}
#endif
if (new_data) {
inv_execute_on_data();
/* This function reads bias-compensated sensor data and sensor
* fusion outputs from the MPL. The outputs are formatted as seen
* in eMPL_outputs.c. This function only needs to be called at the
* rate requested by the host.
*/
read_from_mpl();
}
//========================================IMU==================================
}
}
int main(int argc, char **argv){
init();
short accel[3], gyro[3], sensors[1];
long quat[4];
unsigned long timestamp;
unsigned char more[0];
time_t sec, current_time; // set to the time before calibration
int running = 0; // set to 1 once the calibration has been done
time(&sec);
printf("Read system time\n");
while (1){
short status;
mpu_get_int_status(&status);
if (! (status & MPU_INT_STATUS_DMP) ){
continue;
}
int fifo_read = dmp_read_fifo(gyro, accel, quat, ×tamp, sensors, more);
if (fifo_read != 0) {
printf("Error reading fifo.\n");
}
if (fifo_read == 0 && sensors[0] && running) {
float angles[NOSENTVALS];
float temp_quat[4];
rescale_l(quat, temp_quat, QUAT_SCALE, 4);
if (!quat_offset[0]) {
// check if the IMU has finished calibrating
if(abs(last_quat[1]-temp_quat[1]) < THRESHOLD) {
// the IMU has finished calibrating
int i;
quat_offset[0] = temp_quat[0]; // treat the w value separately as it does not need to be reversed
for(i=1;i<4;++i){
quat_offset[i] = -temp_quat[i];
}
}
else {
memcpy(last_quat, temp_quat, 4*sizeof(float));
}
}
else {
q_multiply(quat_offset, temp_quat, angles+9); // multiply the current quaternstion by the offset caputured above to re-zero the values
// rescale the gyro and accel values received from the IMU from longs that the
// it uses for efficiency to the floats that they actually are and store these values in the angles array
rescale_s(gyro, angles+3, GYRO_SCALE, 3);
rescale_s(accel, angles+6, ACCEL_SCALE, 3);
// turn the quaternation (that is already in angles) into euler angles and store it in the angles array
euler(angles+9, angles);
printf("Yaw: %+5.1f\tRoll: %+5.1f\tPitch: %+5.1f\n", angles[0]*180.0/PI, angles[1]*180.0/PI, angles[2]*180.0/PI);
// send the values in angles over UDP as a string (in udp.c/h)
udp_send(angles, 13);
}
}
else {
time(¤t_time);
// check if more than CALIBRATION_TIME seconds has passed since calibration started
if(abs(difftime(sec, current_time)) > CALIBRATION_TIME) {
printf("Calibration time up...\n");
// allow all of the calculating, broadcasting and offset code from above to run
running = 1;
}
else
printf("Calibrating...\n");
}
}
}
int ms_open() {
dmpReady=1;
initialized = 0;
for (int i=0;i<DIM;i++){
lastval[i]=10;
}
// initialize device
printf("Initializing MPU...\n");
if (mpu_init(NULL) != 0) {
printf("MPU init failed!\n");
return -1;
}
printf("Setting MPU sensors...\n");
if (mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to set sensors!\n");
return -1;
}
printf("Setting GYRO sensitivity...\n");
if (mpu_set_gyro_fsr(2000)!=0) {
printf("Failed to set gyro sensitivity!\n");
return -1;
}
printf("Setting ACCEL sensitivity...\n");
if (mpu_set_accel_fsr(2)!=0) {
printf("Failed to set accel sensitivity!\n");
return -1;
}
// verify connection
printf("Powering up MPU...\n");
mpu_get_power_state(&devStatus);
printf(devStatus ? "MPU6050 connection successful\n" : "MPU6050 connection failed %u\n",devStatus);
//fifo config
printf("Setting MPU fifo...\n");
if (mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL)!=0) {
printf("Failed to initialize MPU fifo!\n");
return -1;
}
// load and configure the DMP
printf("Loading DMP firmware...\n");
if (dmp_load_motion_driver_firmware()!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
printf("Activating DMP...\n");
if (mpu_set_dmp_state(1)!=0) {
printf("Failed to enable DMP!\n");
return -1;
}
//dmp_set_orientation()
//if (dmp_enable_feature(DMP_FEATURE_LP_QUAT|DMP_FEATURE_SEND_RAW_GYRO)!=0) {
printf("Configuring DMP...\n");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL)!=0) {
printf("Failed to enable DMP features!\n");
return -1;
}
printf("Setting DMP fifo rate...\n");
if (dmp_set_fifo_rate(rate)!=0) {
printf("Failed to set dmp fifo rate!\n");
return -1;
}
printf("Resetting fifo queue...\n");
if (mpu_reset_fifo()!=0) {
printf("Failed to reset fifo!\n");
return -1;
}
printf("Checking... ");
do {
delay_ms(1000/rate); //dmp will habve 4 (5-1) packets based on the fifo_rate
r=dmp_read_fifo(g,a,_q,&sensors,&fifoCount);
} while (r!=0 || fifoCount<5); //packtets!!!
printf("Done.\n");
initialized = 1;
return 0;
}
