//// MPU9150 IMU ////
int initialize_imu(int sample_rate, signed char orientation[9]){
printf("Initializing IMU\n");
//set up gpio interrupt pin connected to imu
if(gpio_export(INTERRUPT_PIN)){
printf("can't export gpio %d \n", INTERRUPT_PIN);
return (-1);
}
gpio_set_dir(INTERRUPT_PIN, INPUT_PIN);
gpio_set_edge(INTERRUPT_PIN, "falling"); // Can be rising, falling or both
linux_set_i2c_bus(1);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
mpu_set_sample_rate(sample_rate);
// compass run at 100hz max.
if(sample_rate > 100){
// best to sample at a fraction of the gyro/accel
mpu_set_compass_sample_rate(sample_rate/2);
}
else{
mpu_set_compass_sample_rate(sample_rate);
}
mpu_set_lpf(188); //as little filtering as possible
dmp_load_motion_driver_firmware(sample_rate);
dmp_set_orientation(inv_orientation_matrix_to_scalar(orientation));
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(sample_rate);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
if(loadGyroCalibration()){
printf("\nGyro Calibration File Doesn't Exist Yet\n");
printf("Use calibrate_gyro example to create one\n");
printf("Using 0 offset for now\n");
};
// set the imu_interrupt_thread to highest priority since this is used
// for real-time control with time-sensitive IMU data
set_imu_interrupt_func(&null_func);
pthread_t imu_interrupt_thread;
struct sched_param params;
pthread_create(&imu_interrupt_thread, NULL, imu_interrupt_handler, (void*) NULL);
params.sched_priority = sched_get_priority_max(SCHED_FIFO);
pthread_setschedparam(imu_interrupt_thread, SCHED_FIFO, ¶ms);
return 0;
}
void MPU9250Device::init() {
mpu_init(&this->revision);
mpu_set_compass_sample_rate(100); // defaults to 100 in the libs
/* Get/set hardware configuration. Start gyro. Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
// mpu_set_gyro_fsr (2000);//250
// mpu_set_accel_fsr(2);//4
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);
dmp_load_motion_driver_firmware();
dmp_set_orientation(GYRO_ORIENTATION);
//dmp_register_tap_cb(&tap_cb);
unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |
DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
//dmp_features = dmp_features | DMP_FEATURE_TAP ;
dmp_enable_feature(dmp_features);
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
}
uint8_t mpu_set_sample_rate(uint16_t rate){
uint8_t data;
// センサー使用しない場合は終了
if (!(mpu9250_st.sensors))
return -1;
if (mpu9250_st.dmp_on){
return -1;
} else {
// ローパワーモード？とりあえずマスク
#if 0
if (mpu9250_st.lp_accel_mode) {
if (rate && (rate <= 40)) {
/* Just stay in low-power accel mode. */
mpu_lp_accel_mode(rate);
return 0;
}
/* Requested rate exceeds the allowed frequencies in LP accel mode,
* switch back to full-power mode.
*/
mpu_lp_accel_mode(0);
}
#endif
// 上下限カット
if (rate < 4)
rate = 4;
else if (rate > 1000)
rate = 1000;
// レジスタ書き込み値に変換
data = 1000 / rate - 1;
// レジスタ書き込み
if (mpu9250_writes(MPU6500_RATE_DIV, 1, &data))
return -1;
// 値の保存
mpu9250_st.sample_rate = 1000 / (1 + data);
#ifdef USE_AK8963
mpu_set_compass_sample_rate(min(mpu9250_st.compass_sample_rate, MAX_COMPASS_SAMPLE_RATE));
#endif
/* Automatically set LPF to 1/2 sampling rate. */
mpu_set_lpf(mpu9250_st.sample_rate >> 1);
return 0;
}
}
int __init mpu9150_init(int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printk(KERN_WARNING "Invalid sample rate %d\n", sample_rate);
sprintf(status_string,"Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printk(KERN_WARNING "Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
printk("\nInitializing IMU ...");
if (mmpu_init()) {
printk(KERN_WARNING "\nmpu_init() failed\n");
sprintf(status_string,"mpu_init() failed\n");
return -1;
}
//printk(".");
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printk(KERN_WARNING "\nmpu_set_sensors() failed\n");
sprintf(status_string,"mpu_set_sensors() failed\n");
return -1;
}
//printk(".");
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printk(KERN_WARNING "\nmpu_configure_fifo() failed\n");
sprintf(status_string,"mpu_configure_fifo() failed\n");
return -1;
}
//printk(".");
if (mpu_set_sample_rate(sample_rate)) {
printk(KERN_WARNING "\nmpu_set_sample_rate() failed\n");
sprintf(status_string,"mpu_set_sample_rate() failed\n");
return -1;
}
//printk(".");
if (mpu_set_compass_sample_rate(sample_rate)) {
printk(KERN_WARNING "\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
//printk(".");
if (dmp_load_motion_driver_firmware()) {
printk(KERN_WARNING "\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
//printk(".");
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printk(KERN_WARNING "\ndmp_set_orientation() failed\n");
return -1;
}
//printk(".");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printk(KERN_WARNING "\ndmp_enable_feature() failed\n");
return -1;
}
//printk(".");
if (dmp_set_fifo_rate(sample_rate)) {
printk(KERN_WARNING "\ndmp_set_fifo_rate() failed\n");
return -1;
}
//printk(".");
if (mpu_set_dmp_state(1)) {
printk(KERN_WARNING "\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printk(" done\n");
return 0;
}
int mpulib_init(int i2c_bus, int sample_rate, int mix_factor) {
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS) {
printf("Invalid I2C bus %d\n", i2c_bus);
return -1;
}
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
int compass_sample_rate = sample_rate;
if( compass_sample_rate > MAX_COMPASS_SAMPLE_RATE ) {
compass_sample_rate = MAX_COMPASS_SAMPLE_RATE;
}
if (mpu_set_compass_sample_rate(compass_sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE) {
printf("Invalid sample rate %d\n", sample_rate);
return -1;
}
if (mix_factor < 0 || mix_factor > 100) {
printf("Invalid mag mixing factor %d\n", mix_factor);
return -1;
}
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_set_sensors() failed\n");
return -1;
}
#endif
printf(".");
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
if (mpu_set_sample_rate(200)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
#if defined AK8963_SECONDARY || defined AK8975_SECONDARY
if (mpu_set_compass_sample_rate(50)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
uint8_t mpu_init(){
uint8_t data[6];
uint8_t datai;
/* Reset device. */
data[0] = BIT_RESET;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
delay_ms(100);
/* Wake up chip. */
data[0] = 0x00;
if (mpu9250_writes(MPU6500_PWR_MGNT_1, 1, data))
return -1;
// とりあえず
//data[0] = 0x00;
//mpu9250_writes(MPU6500_USER_CTRL, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
//delay_ms(10);
//mpu9250_writes(MPU6500_LPF, 1, data);
//delay_ms(10);
data[0] = 0x02;
mpu9250_writes(MPU6500_INT_PIN_CFG, 1, data);
delay_ms(10);
//datai = 0;
//datai |= BIT_STBY_XYZA;
data[0] = 0x00;
mpu9250_writes(MPU6500_PWR_MGNT_2, 1, data[0]);
delay_ms(10);
//data[0] = 0x01;
//mpu9250_writes(MPU6500_INT_ENABLE, 1, data);
//delay_ms(10);
#if 0
/* user control */
data[0] = MPU9250_I2C_IF_DIS | MPU9250_I2C_MST_EN;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
if (mpu9250_writes(MPU6500_USER_CTRL, 1, data))
return -1;
delay_ms(10);
mpu_set_bypass(0);
delay_ms(10);
mpu9250_st.accel_half = 0;
/* MPU6500 shares 4kB of memory between the DMP and the FIFO. Since the
* first 3kB are needed by the DMP, we'll use the last 1kB for the FIFO.
*/
data[0] = BIT_FIFO_SIZE_1024 | 0x8;
if (mpu9250_writes(MPU6500_ACCEL_CFG2, 1, data))
return -1;
/* Set to invalid values to ensure no I2C writes are skipped. */
//mpu9250_st.sensors = 0xFF;
//mpu9250_st.gyro_fsr = 0xFF;
//mpu9250_st.accel_fsr = 0xFF;
//mpu9250_st.lpf = 0xFF;
//mpu9250_st.sample_rate = 0xFFFF;
//mpu9250_st.fifo_enable = 0xFF;
//mpu9250_st.bypass_mode = 0xFF;
//mpu9250_st.compass_sample_rate = 0xFFFF;
/* mpu_set_sensors always preserves this setting. */
mpu9250_st.clk_src = INV_CLK_PLL;
/* Handled in next call to mpu_set_bypass. */
mpu9250_st.active_low_int = 1;
mpu9250_st.latched_int = 0;
mpu9250_st.int_motion_only = 0;
mpu9250_st.lp_accel_mode = 0;
//とりあえずマスク
//memset(&mpu9250_st.cache, 0, sizeof(mpu9250_st.cache));
mpu9250_st.dmp_on = 0;
mpu9250_st.dmp_loaded = 0;
mpu9250_st.dmp_sample_rate = 0;
// ジャイロフルスケール設定
if (mpu_set_gyro_fsr(2000))
return -1;
// 加速度フルスケール設定
if (mpu_set_accel_fsr(2))
return -1;
// LPF設定
if (mpu_set_lpf(42))
return -1;
// サンプルレート設定
if (mpu_set_sample_rate(100))
return -1;
// ひとまずマスク
/*
if (mpu_configure_fifo(0))
return -1;
if (int_param)
reg_int_cb(int_param);
*/
#ifdef USE_AK8963
if (setup_compass())
return -1;
if (mpu_set_compass_sample_rate(100))
return -1;
#else
/* Already disabled by setup_compass. */
if (mpu_set_bypass(0))
return -1;
#endif
/*
mpu_set_sensors(0);
*/
#endif
return 0;
}
boolean MPU9150Lib::init(int mpuRate, int magMix)
{
struct int_param_s int_param;
int result;
long accelOffset[3];
m_magMix = magMix;
m_lastDMPYaw = 0;
m_lastYaw = 0;
// get calibration data if it's there
if (calSensRead(&m_calData)) { // use calibration data if it's there and wanted
m_useMagCalibration &= m_calData.magValid;
m_useAccelCalibration &= m_calData.accelValid;
// Process calibration data for runtime
if (m_useMagCalibration) {
m_magXOffset = (short)(((long)m_calData.magMaxX + (long)m_calData.magMinX) / 2);
m_magXRange = m_calData.magMaxX - m_magXOffset;
m_magYOffset = (short)(((long)m_calData.magMaxY + (long)m_calData.magMinY) / 2);
m_magYRange = m_calData.magMaxY - m_magYOffset;
m_magZOffset = (short)(((long)m_calData.magMaxZ + (long)m_calData.magMinZ) / 2);
m_magZRange = m_calData.magMaxZ - m_magZOffset;
}
if (m_useAccelCalibration) {
accelOffset[0] = -((long)m_calData.accelMaxX + (long)m_calData.accelMinX) / 2;
accelOffset[1] = -((long)m_calData.accelMaxY + (long)m_calData.accelMinY) / 2;
accelOffset[2] = -((long)m_calData.accelMaxZ + (long)m_calData.accelMinZ) / 2;
mpu_set_accel_bias(accelOffset);
m_accelXRange = m_calData.accelMaxX + accelOffset[0];
m_accelYRange = m_calData.accelMaxY + accelOffset[1];
m_accelZRange = m_calData.accelMaxZ + accelOffset[2];
}
}
#ifdef MPULIB_DEBUG
if (m_useMagCalibration)
Serial.println("Using mag cal");
if (m_useAccelCalibration)
Serial.println("Using accel cal");
#endif
mpu_init_structures();
// Not using interrupts so set up this structure to keep the driver happy
int_param.cb = NULL;
int_param.pin = 0;
int_param.lp_exit = 0;
int_param.active_low = 1;
result = mpu_init(&int_param);
if (result != 0) {
#ifdef MPULIB_DEBUG
Serial.print("mpu_init failed with code: ");
Serial.println(result);
#endif
return false;
}
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); // enable all of the sensors
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // get accel and gyro data in the FIFO also
mpu_set_sample_rate(mpuRate); // set the update rate
mpu_set_compass_sample_rate(mpuRate); // set the compass update rate to match
#ifdef MPULIB_DEBUG
Serial.println("Loading firmware");
#endif
if ((result = dmp_load_motion_driver_firmware()) != 0) { // try to load the DMP firmware
#ifdef MPULIB_DEBUG
Serial.print("Failed to load dmp firmware: ");
Serial.println(result);
#endif
return false;
}
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)); // set up the correct orientation
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
dmp_set_fifo_rate(mpuRate);
if (mpu_set_dmp_state(1) != 0) {
#ifdef MPULIB_DEBUG
Serial.println("mpu_set_dmp_state failed");
#endif
return false;
}
return true;
}
inv_error_t MPU9250_DMP::setCompassSampleRate(unsigned short rate)
{
return mpu_set_compass_sample_rate(rate);
}
int mpu9150_init(int i2c_bus, int sample_rate, int mix_factor)
{
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
if (i2c_bus < 0 || i2c_bus > 3)
return -1;
if (sample_rate < 2 || sample_rate > 50)
return -1;
if (mix_factor < 0 || mix_factor > 100)
return -1;
yaw_mixing_factor = mix_factor;
linux_set_i2c_bus(i2c_bus);
printf("\nInitializing IMU .");
fflush(stdout);
if (mpu_init(NULL)) {
printf("\nmpu_init() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#else
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS)) {
#endif
printf("\nmpu_set_sensors() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
printf("\nmpu_configure_fifo() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_sample_rate(sample_rate)) {
printf("\nmpu_set_sample_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
#ifdef AK89xx_SECONDARY
if (mpu_set_compass_sample_rate(sample_rate)) {
printf("\nmpu_set_compass_sample_rate() failed\n");
return -1;
}
#endif
printf(".");
fflush(stdout);
if (dmp_load_motion_driver_firmware()) {
printf("\ndmp_load_motion_driver_firmware() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
printf("\ndmp_set_orientation() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("\ndmp_enable_feature() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (dmp_set_fifo_rate(sample_rate)) {
printf("\ndmp_set_fifo_rate() failed\n");
return -1;
}
printf(".");
fflush(stdout);
if (mpu_set_dmp_state(1)) {
printf("\nmpu_set_dmp_state(1) failed\n");
return -1;
}
printf(" done\n\n");
return 0;
}
/* New functions to enable / disable 6axis on the fly */
int enableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure enabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion enabled\n");
return 0;
}
int disableAccelerometerFusion(void) {
if (dmp_enable_feature(DMP_FEATURE_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
printf("Failure disabling accelerometer fusion\n");
return -1;
}
printf("mpu9150.c: Accelerometer fusion disabled\n");
return 0;
}
void mpu9150_exit()
{
// turn off the DMP on exit
if (mpu_set_dmp_state(0))
printf("mpu_set_dmp_state(0) failed\n");
// TODO: Should turn off the sensors too
}
void mpu9150_set_accel_cal(caldata_t *cal)
{
int i;
long bias[3];
if (!cal) {
use_accel_cal = 0;
return;
}
memcpy(&accel_cal_data, cal, sizeof(caldata_t));
for (i = 0; i < 3; i++) {
if (accel_cal_data.range[i] < 1)
accel_cal_data.range[i] = 1;
else if (accel_cal_data.range[i] > ACCEL_SENSOR_RANGE)
accel_cal_data.range[i] = ACCEL_SENSOR_RANGE;
bias[i] = -accel_cal_data.offset[i];
}
if (debug_on) {
printf("\naccel cal (range : offset)\n");
for (i = 0; i < 3; i++)
printf("%d : %d\n", accel_cal_data.range[i], accel_cal_data.offset[i]);
}
mpu_set_accel_bias(bias);
use_accel_cal = 1;
}
