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; }