byte MPU9250_DMP::begin(bool fusion, int rate)
{
i2c_port_t i2c_master_port = I2C_NUM_0;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = (gpio_num_t)21;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = (gpio_num_t)22;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 100000;
if (i2c_param_config(i2c_master_port, &conf) != ESP_OK || i2c_driver_install(i2c_master_port, conf.mode, 0, 0, 0) != ESP_OK)
return 0;
struct int_param_s int_param;
inv_error_t result = mpu_init(&int_param);
if (result)
return 0;
mpu_set_bypass(1); // Place all slaves (including compass) on primary bus
setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS | INV_XYZ_COMPASS);
_gSense = getGyroSens();
_aSense = getAccelSens();
_features = DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO;
if (fusion) _features |= DMP_FEATURE_6X_LP_QUAT;
// Set DMP FIFO rate to 10 Hz
if (dmpBegin(_features, rate) == INV_ERROR)
return 0;
return 1;
}
/******************************************************************************
* 函数名称: MPU6050_Dmp_Init
* 函数功能: MPU dmp初始化
* 入口参数: 无
* 返回值 无
******************************************************************************/
void MPU9250_Dmp_Init(void)
{
SPI_Init();
mpu_set_lpf(5);
while(mpu_init());
//mpu_set_sensor
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_configure_fifo
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_set_sample_rate
mpu_set_sample_rate(DEFAULT_MPU_HZ);
//dmp_load_motion_driver_firmvare
dmp_load_motion_driver_firmware();
//dmp_set_orientation
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//dmp_enable_feature
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_set_fifo_rate
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
//run_self_test();
mpu_set_dmp_state(1);
}
int init(){
open_bus();
unsigned char whoami=0;
i2c_read(MPU6050_ADDR, MPU6050_WHO_AM_I, 1, &whoami);
if(!silent_flag) {
printf("WHO_AM_I: %x\n", whoami);
}
struct int_param_s int_param;
if(!silent_flag) {
printf("MPU init: %i\n", mpu_init(&int_param));
printf("MPU sensor init: %i\n", mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL));
printf("MPU configure fifo: %i\n", mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL));
printf("DMP firmware: %i\n ",dmp_load_motion_driver_firmware());
printf("DMP orientation: %i\n ",dmp_set_orientation(
inv_orientation_matrix_to_scalar(gyro_orientation)));
}
unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL;
if(!silent_flag) {
printf("DMP feature enable: %i\n", dmp_enable_feature(dmp_features));
printf("DMP set fifo rate: %i\n", dmp_set_fifo_rate(DEFAULT_MPU_HZ));
printf("DMP enable %i\n", mpu_set_dmp_state(1));
}
if(!no_interrupt_flag) {
mpu_set_int_level(1); // Interrupt is low when firing
dmp_set_interrupt_mode(DMP_INT_CONTINUOUS); // Fire interrupt on new FIFO value
}
return 0;
}
void init_mpu6050(struct eeprom *e)
{
int result=0;
die_if(eeprom_open("/dev/i2c/0", 0x68, EEPROM_TYPE_8BIT_ADDR, e) < 0,
"unable to open MPU6050 device file ");
result=mpu_init();
if(!result)
{
printf("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
printf("mpu_set_sensor complete ......\n");
else
printf("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
printf("mpu_configure_fifo complete ......\n");
else
printf("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(50)) //mpu_set_sample_rate
printf("mpu_set_sample_rate complete ......\n");
else
printf("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
printf("dmp_load_motion_driver_firmware complete ......\n");
else
printf("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
printf("dmp_set_orientation complete ......\n");
else
printf("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
printf("dmp_enable_feature complete ......\n");
else
printf("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(50)) //dmp_set_fifo_rate
printf("dmp_set_fifo_rate complete ......\n");
else
printf("dmp_set_fifo_rate come across error ......\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\n");
else
printf("mpu_set_dmp_state come across error ......\n");
}
else
{
printf("mpu INIT INIT INIT error ......\r\n");
}
}
int mympu_open(unsigned int rate) {
mpu_select_device(0);
mpu_init_structures();
ret = mpu_init(NULL);
#ifdef MPU_DEBUG
if (ret) return 10+ret;
#endif
ret = mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 20+ret;
#endif
ret = mpu_set_gyro_fsr(FSR);
#ifdef MPU_DEBUG
if (ret) return 30+ret;
#endif
ret = mpu_set_accel_fsr(2);
#ifdef MPU_DEBUG
if (ret) return 40+ret;
#endif
mpu_get_power_state((unsigned char *)&ret);
#ifdef MPU_DEBUG
if (!ret) return 50+ret;
#endif
ret = mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);
#ifdef MPU_DEBUG
if (ret) return 60+ret;
#endif
dmp_select_device(0);
dmp_init_structures();
ret = dmp_load_motion_driver_firmware();
#ifdef MPU_DEBUG
if (ret) return 80+ret;
#endif
ret = dmp_set_fifo_rate(rate);
#ifdef MPU_DEBUG
if (ret) return 90+ret;
#endif
ret = mpu_set_dmp_state(1);
#ifdef MPU_DEBUG
if (ret) return 100+ret;
#endif
ret = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
// ret = dmp_enable_feature(DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL);
#ifdef MPU_DEBUG
if (ret) return 110+ret;
#endif
return 0;
}
/*******************************************************************************
* FUNCTION: ssInitIMU
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ Return 0 if successful.
*
* DESCRIPTIONS:
* Initialized the IMU.
*
*******************************************************************************/
signed short ssInitIMU(void)
{
// Initialize the IMU. Return if failed.
signed short ssErrorCode = mpu_init(NULL);
if (ssErrorCode != 0) {
return ssErrorCode;
}
// Wake up all sensors.
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
// Push both gyro and accel data into the FIFO.
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* To initialize the DMP:
* 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
* inv_mpu_dmp_motion_driver.h into the MPU memory.
* 2. Push the gyro and accel orientation matrix to the DMP.
* 3. Register gesture callbacks. Don't worry, these callbacks won't be
* executed unless the corresponding feature is enabled.
* 4. Call dmp_enable_feature(mask) to enable different features.
* 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
* 6. Call any feature-specific control functions.
*
* To enable the DMP, just call mpu_set_dmp_state(1). This function can
* be called repeatedly to enable and disable the DMP at runtime.
*
* The following is a short summary of the features supported in the DMP
* image provided in inv_mpu_dmp_motion_driver.c:
* DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
* 200Hz. Integrating the gyro data at higher rates reduces numerical
* errors (compared to integration on the MCU at a lower sampling rate).
* DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
* 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
* DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
* DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
* an event at the four orientations where the screen should rotate.
* DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
* no motion.
* DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
* DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
* DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
* be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
*/
dmp_load_motion_driver_firmware();
/*
* Known Bug -
* DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
* specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
* a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
* there will be a 25Hz interrupt from the MPU device.
*
* There is a known issue in which if you do not enable DMP_FEATURE_TAP
* then the interrupts will be at 200Hz even if fifo rate
* is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
*/
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_GYRO_CAL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL);
dmp_set_fifo_rate(10);
mpu_set_dmp_state(1);
}
int main(void)
{
uint8_t rc;
node_init();
sei();
i2c_init(I2C_FREQ(400000));
rc = hmc_init();
if (rc) {
// puts_P(PSTR("hmc init"));
}
rc = mpu_init();
if (rc) {
// puts_P(PSTR("mpu init"));
}
rc = nunchuck_init();
if (rc) {
// puts_P(PSTR("nunch init"));
}
hmc_set_continuous();
node_main();
}
/**
* @brief mpu6050_init
* @param void
* @retval 成功为0
* @note 初始化mpu6050 频率为100*1000HZ
*/
int16_t mpu6050_init(void)
{
I2C_QuickInit(I2C0_SCL_PB02_SDA_PB03,100*1000);
int result=0;
result=mpu_init();
if(!result)
{
PrintChar("mpu initialization complete......\n "); //mpu initialization complete
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_set_sensor
PrintChar("mpu_set_sensor complete ......\n");
else
PrintChar("mpu_set_sensor come across error ......\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo
PrintChar("mpu_configure_fifo complete ......\n");
else
PrintChar("mpu_configure_fifo come across error ......\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate
PrintChar("mpu_set_sample_rate complete ......\n");
else
PrintChar("mpu_set_sample_rate error ......\n");
if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare
PrintChar("dmp_load_motion_driver_firmware complete ......\n");
else
PrintChar("dmp_load_motion_driver_firmware come across error ......\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation
PrintChar("dmp_set_orientation complete ......\n");
else
PrintChar("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
PrintChar("dmp_enable_feature complete ......\n");
else
PrintChar("dmp_enable_feature come across error ......\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate
PrintChar("dmp_set_fifo_rate complete ......\n");
else
PrintChar("dmp_set_fifo_rate come across error ......\n");
run_self_test(); //自检
if(!mpu_set_dmp_state(1))
PrintChar("mpu_set_dmp_state complete ......\n");
else
PrintChar("mpu_set_dmp_state come across error ......\n");
}
return 0;
}
/**************************************************************************
函数功能：MPU6050内置DMP的初始化
入口参数：无
返回 值：无
作 者：平衡小车之家
**************************************************************************/
void DMP_Init(void)
{
u8 temp[1]={0};
i2cRead(0x68,0x75,1,temp);
printf("mpu_set_sensor complete ......\r\n");
if(temp[0]!=0x68)NVIC_SystemReset();
if(!mpu_init())
{
if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_set_sensor complete ......\r\n");
if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
printf("mpu_configure_fifo complete ......\r\n");
if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))
printf("mpu_set_sample_rate complete ......\r\n");
if(!dmp_load_motion_driver_firmware())
printf("dmp_load_motion_driver_firmware complete ......\r\n");
if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
printf("dmp_set_orientation complete ......\r\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))
printf("dmp_enable_feature complete ......\r\n");
if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))
printf("dmp_set_fifo_rate complete ......\r\n");
run_self_test();
if(!mpu_set_dmp_state(1))
printf("mpu_set_dmp_state complete ......\r\n");
}
}
/**
* This function will initial LPC17xx board.
*/
void rt_hw_board_init()
{
/* NVIC Configuration */
#define NVIC_VTOR_MASK 0x3FFFFF80
#ifdef VECT_TAB_RAM
/* Set the Vector Table base location at 0x10000000 */
SCB->VTOR = (0x10000000 & NVIC_VTOR_MASK);
#else /* VECT_TAB_FLASH */
/* Set the Vector Table base location at 0x00000000 */
SCB->VTOR = (0x00000000 & NVIC_VTOR_MASK);
#endif
/* init systick */
SysTick_Config(SystemCoreClock / RT_TICK_PER_SECOND - 1);
/* set pend exception priority */
NVIC_SetPriority(PendSV_IRQn, (1 << __NVIC_PRIO_BITS) - 1);
/*init uart device*/
rt_hw_uart_init();
rt_console_set_device(RT_CONSOLE_DEVICE_NAME);
#if LPC_EXT_SDRAM == 1
lpc_sdram_hw_init();
mpu_init();
#endif
#ifdef RT_USING_COMPONENTS_INIT
/* initialization board with RT-Thread Components */
rt_components_board_init();
#endif
}
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);
}
/**
****************************************************************************************
* @brief start mpu6050 dmp
* @description
* This function is used to initialize mpu6050 dmp.
*****************************************************************************************
*/
void mpu_start()
{
int result = mpu_init(NULL);
while(result != MPU_OK)
{
result = mpu_init(NULL);
}
if(result == MPU_OK)
{
printf("mpu initialization complete......\n "); //mpu_set_sensor
if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK)
printf("mpu_set_sensor complete ......\n");
else
printf("mpu_set_sensor come across error ......\n");
if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) //mpu_configure_fifo
printf("mpu_configure_fifo complete ......\n");
else
printf("mpu_configure_fifo come across error ......\n");
if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK) //mpu_set_sample_rate
printf("mpu_set_sample_rate complete ......\n");
else
printf("mpu_set_sample_rate error ......\n");
if(dmp_load_motion_driver_firmware() == MPU_OK) //dmp_load_motion_driver_firmvare
printf("dmp_load_motion_driver_firmware complete ......\n");
else
printf("dmp_load_motion_driver_firmware come across error ......\n");
if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK) //dmp_set_orientation
printf("dmp_set_orientation complete ......\n");
else
printf("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) == MPU_OK) //dmp_enable_feature
printf("dmp_enable_feature complete ......\n");
else
printf("dmp_enable_feature come across error ......\n");
if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK) //dmp_set_fifo_rate
printf("dmp_set_fifo_rate complete ......\n");
else
printf("dmp_set_fifo_rate come across error ......\n");
run_self_test();
if(mpu_set_dmp_state(1) == MPU_OK)
printf("mpu_set_dmp_state complete ......\n");
else
printf("mpu_set_dmp_state come across error ......\n");
}
}
//// 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;
}
// connect to the serial port
void SerialInterface::initializePort(char* portname) {
#ifndef _WIN32
_serialDescriptor = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
qDebug("Opening SerialUSB %s: ", portname);
if (_serialDescriptor == -1) {
qDebug("Failed.\n");
return;
}
struct termios options;
tcgetattr(_serialDescriptor, &options);
options.c_cflag |= (CLOCAL | CREAD | CS8);
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
tcsetattr(_serialDescriptor, TCSANOW, &options);
cfsetispeed(&options,B115200);
cfsetospeed(&options,B115200);
if (USING_INVENSENSE_MPU9150) {
// block on invensense reads until there is data to read
int currentFlags = fcntl(_serialDescriptor, F_GETFL);
fcntl(_serialDescriptor, F_SETFL, currentFlags & ~O_NONBLOCK);
// make sure there's nothing queued up to be read
tcflush(_serialDescriptor, TCIOFLUSH);
// this disables streaming so there's no garbage data on reads
if (write(_serialDescriptor, "SD\n", 3) != 3) {
qDebug("Failed.\n");
return;
}
char result[4];
if (read(_serialDescriptor, result, 4) != 4) {
qDebug("Failed.\n");
return;
}
tty_set_file_descriptor(_serialDescriptor);
mpu_init(0);
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
}
qDebug("Connected.\n");
resetSerial();
_active = true;
#endif
}
/*
* @brief MPU default configuration
*
* This function provides the default configuration mechanism for the Memory
* Protection Unit (MPU).
*/
static int arm_mpu_init(struct device *arg)
{
u32_t r_index;
if (mpu_config.num_regions > get_num_regions()) {
/* Attempt to configure more MPU regions than
* what is supported by hardware. As this operation
* is executed during system (pre-kernel) initialization,
* we want to ensure we can detect an attempt to
* perform invalid configuration.
*/
__ASSERT(0,
"Request to configure: %u regions (supported: %u)\n",
mpu_config.num_regions,
get_num_regions()
);
return -1;
}
LOG_DBG("total region count: %d", get_num_regions());
arm_core_mpu_disable();
/* Architecture-specific configuration */
mpu_init();
/* Program fixed regions configured at SOC definition. */
for (r_index = 0U; r_index < mpu_config.num_regions; r_index++) {
region_init(r_index, &mpu_config.mpu_regions[r_index]);
}
/* Update the number of programmed MPU regions. */
static_regions_num = mpu_config.num_regions;
arm_core_mpu_enable();
/* Sanity check for number of regions in Cortex-M0+, M3, and M4. */
#if defined(CONFIG_CPU_CORTEX_M0PLUS) || \
defined(CONFIG_CPU_CORTEX_M3) || \
defined(CONFIG_CPU_CORTEX_M4)
__ASSERT(
(MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos == 8,
"Invalid number of MPU regions\n");
#elif defined(DT_NUM_MPU_REGIONS)
__ASSERT(
(MPU->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos ==
DT_NUM_MPU_REGIONS,
"Invalid number of MPU regions\n");
#endif /* CORTEX_M0PLUS || CPU_CORTEX_M3 || CPU_CORTEX_M4 */
return 0;
}
void hw_init()
{
uint8_t c;
P0DIR = 0xf0; // P0.0 P0.1 P0.2 are the LEDs and they are outputs
// P0.3 is the UART TX - output
// P0.5 is the push button - input
// P0.6 is the MPU interrupt pin - input
P0CON = 0x55; // turn on the pullup for the recenter button
// cycle the LEDs
LED_RED = 0;
LED_YELLOW = 0;
LED_GREEN = 0;
for (c = 0; c < 3; ++c)
{
LED_RED = 1;
delay_ms(40);
LED_RED = 0;
LED_YELLOW = 1;
delay_ms(40);
LED_YELLOW = 0;
LED_GREEN = 1;
delay_ms(40);
LED_GREEN = 0;
LED_YELLOW = 1;
delay_ms(40);
LED_YELLOW = 0;
}
dbgInit();
i2c_init();
dputs("init started");
LED_YELLOW = 1;
mpu_init(false);
dbgFlush();
rf_head_init(); // init the radio
init_sleep(); // we need to wake up from RFIRQ
LED_YELLOW = 0;
dputs("init OK");
}
/**********************************MPU6050*************************************/
void MPU6050_INIT()
{
mpu_init();
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);//#(INV_XYZ_GYRO | INV_XYZ_ACCEL)在inv_mpu.h定义
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);//#本文件开头定义
dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//#本文定义
dmp_features = 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;//#全部在(inv_mpu_dmp_motion_driver.h)中定义
dmp_enable_feature(dmp_features);
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
run_self_test();
mpu_set_dmp_state(1);
}
/**
* @brief Function for main application entry.
*/
int main(void) {
uart_config();
printf("\033[2J\033[;H MPU9150 example. Compiled @ %s\r\n", __TIME__);
twi_init();
mpu_init();
accel_values_t acc_values;
uint32_t sample_number = 0;
while(1) {
mpu9150_read_accel(&acc_values); // Read accelerometer sensor values
// Clear terminal and print values
printf("\033[2J\033[;HSample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d", ++sample_number, acc_values.x, acc_values.y, acc_values.z);
nrf_delay_ms(250);
}
}
/**
* @brief Function for main application entry.
*/
int main(void)
{
uint32_t err_code;
// Initialize.
log_init();
NRF_LOG_INFO("\033[2J\033[;H"); // Clear screen
mpu_init();
// Start execution.
NRF_LOG_INFO("MPU Magnetometer example.");
accel_values_t acc_values;
magn_values_t magn_values;
uint32_t sample_number = 0;
const uint8_t MAG_DATA_SIZE = 10;
uint8_t magn_data[MAG_DATA_SIZE];
memset(magn_data, 0, MAG_DATA_SIZE);
while(1)
{
if(NRF_LOG_PROCESS() == false)
{
// Read accelerometer sensor values
err_code = app_mpu_read_accel(&acc_values);
APP_ERROR_CHECK(err_code);
// Clear terminal and print values
NRF_LOG_INFO("\033[3;1HAccel Sample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d\r\n", ++sample_number, acc_values.x, acc_values.y, acc_values.z);
// Read and print magnetometer sensor values
err_code = app_mpu_read_magnetometer(&magn_values, NULL);
APP_ERROR_CHECK(err_code);
NRF_LOG_INFO("\n\rMagno Sample # %d\r\nX: %06d\r\nY: %06d\r\nZ: %06d\r\n", sample_number, magn_values.x, magn_values.y, magn_values.z);
nrf_delay_ms(200);
}
}
}
int main() {
board_init();
uart_init();
esc_init();
rgbled_init();
rgbled_set(0xFF8000, 100);
spektrum_init();
rgbled_set(0xFF8000, 100);
xbee_init();
i2c_init();
alt_init();
mag_init();
mpu_init();
sonar_init();
ins_init();
inscomp_init();
altitude_init();
controller_init();
basestation_init();
flight_init();
controlpanel_run();
}
void dmp_init()
{
Timer4_init();
while (1 == mpu_init());
//mpu_set_sensor
while (1 == mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL));
//mpu_configure_fifo
while (1 == mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL));
//mpu_set_sample_rate
while (1 == mpu_set_sample_rate(DEFAULT_MPU_HZ));
//dmp_load_motion_driver_firmvare
while (1 == dmp_load_motion_driver_firmware());
//dmp_set_orientation
while (1 == dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)));
//dmp_enable_feature
while (1 == 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_set_fifo_rate
while (1 == dmp_set_fifo_rate(DEFAULT_MPU_HZ));
run_self_test();
while (1 == mpu_set_dmp_state(1));
rt_kprintf("start mpu6050\n");
rt_kprintf("start ahrs\n");
}
/**
* @brief MPU （6500）初始化
* @param None
* @retval None
*/
void InvMPU_Driver_Init(void)
{
u8 ret = 0;
long accel[3];
struct int_param_s int_param;
assert_param(MPU_SPI == &hspi1);
MPU_SPIDeselect();
ret += mpu_init(&int_param);
ret += mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
ret += mpu_set_sample_rate(1000);
ret += mpu_set_gyro_fsr( GYRO_FSR);
ret += mpu_set_accel_fsr( ACCEL_FSR);
accel[0] = 46;
accel[1] = 78;
accel[2] = 0;
mpu_set_accel_bias_6500_reg(accel);
#ifdef USE_DMP
dmp_load_motion_driver_firmware();
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_GYRO_CAL |
DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_SEND_RAW_ACCEL);
dmp_set_fifo_rate(200);
u8 offset[12] = {0,0, 0xd2, 0x2b, 0x00, 0x01, 0xba, 0xd8, 0x00, 0x00, 0x9f, 0xa0};
mpu_write_mem(D_EXT_GYRO_BIAS_X, 4, &offset[0]);
mpu_write_mem(D_EXT_GYRO_BIAS_Y, 4, &offset[4]);
mpu_write_mem(D_EXT_GYRO_BIAS_Z, 4, &offset[8]);
mpu_set_dmp_state(1);
#endif
}
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 INVMPU9150::init(int i2cBus, int frequency) {
// Set linux i2c bus
linux_set_i2c_bus(i2cBus);
// The gyro orientation
signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1 };
// Init MPU
if (mpu_init(NULL)) {
return MPU9150_INIT_MPU_INIT_ERROR;
}
// Set sensors
if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_SET_SENSORS_ERROR;
}
// Get gyro sensitivity scale factor and convert from [LSB/(degree/s)] to [LSB/(rad/s)]
float gyroSsfDegreePerSecond;
if (mpu_get_gyro_sens(& gyroSsfDegreePerSecond)) {
return MPU9150_INIT_MPU_GET_GYRO_FSR_ERROR;
}
gyroSsf = gyroSsfDegreePerSecond * 180 / M_PI;
// Get accel sensitivity scale factor
if (mpu_get_accel_sens(& accelSsf)) {
return MPU9150_INIT_MPU_GET_ACCEL_FSR_ERROR;
}
// Configure FIFO
if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) {
return MPU9150_INIT_MPU_CONFIGURE_FIFO_ERROR;
}
// Set sample rate for gyro/acc
if (mpu_set_sample_rate(frequency)) {
return MPU9150_INIT_MPU_SET_SAMPLE_RATE_ERROR;
}
// Load motion driver firmare (DMP)
if (dmp_load_motion_driver_firmware()) {
return MPU9150_INIT_DMP_LOAD_MOTION_DRIVER_FIRMWARE_ERROR;
}
// Set gyro orientation
if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) {
return MPU9150_INIT_DMP_SET_ORIENTATION_ERROR;
}
// Enable DMP features
if (dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL
| DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL)) {
return MPU9150_INIT_DMP_ENABLE_FEATURES_ERROR;
}
// Set sample rate for DMP
if (dmp_set_fifo_rate(frequency)) {
return MPU9150_INIT_DMP_SET_FIFO_RATE_ERROR;
}
// Enable DMP
if (mpu_set_dmp_state(1)) {
return MPU9150_INIT_DMP_SET_STATE_ERROR;
}
return MPU9150_OK;
}
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;
}
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;
}
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;
}
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;
}
void prf_init_func(void)
{
#if (BLE_ACCEL)
accel_init();
#endif // (BLE_ACCEL)
#if (BLE_HT_THERMOM)
htpt_init();
#endif // (BLE_HT_THERMOM)
#if (BLE_HT_COLLECTOR)
htpc_init();
#endif // (BLE_HT_COLLECTOR)
#if (BLE_DIS_SERVER)
diss_init();
#endif // (BLE_DIS_SERVER)
#if (BLE_DIS_CLIENT)
disc_init();
#endif // (BLE_DIS_CLIENT)
#if (BLE_BP_SENSOR)
blps_init();
#endif // (BLE_BP_SENSOR)
#if (BLE_BP_COLLECTOR)
blpc_init();
#endif // (BLE_BP_COLLECTOR)
#if (BLE_TIP_SERVER)
tips_init();
#endif // (BLE_TIP_SERVER)
#if (BLE_TIP_CLIENT)
tipc_init();
#endif // (BLE_TIP_CLIENT)
#if (BLE_HR_SENSOR)
hrps_init();
#endif // (BLE_HR_SENSOR)
#if (BLE_HR_COLLECTOR)
hrpc_init();
#endif // (BLE_HR_COLLECTOR)
#if (BLE_FINDME_LOCATOR)
findl_init();
#endif // (BLE_FINDME_LOCATOR)
#if (BLE_FINDME_TARGET)
findt_init();
#endif // (BLE_FINDME_TARGET)
#if (BLE_PROX_MONITOR)
proxm_init();
#endif // (BLE_PROX_MONITOR)
#if (BLE_PROX_REPORTER)
proxr_init();
#endif // (BLE_PROX_REPORTER)
#if (BLE_STREAMDATA_HOST)
streamdatah_init();
#endif // (BLE_STREAMDATA_HOST)
#if (BLE_STREAMDATA_DEVICE)
streamdatad_init();
#endif // (BLE_STREAMDATA_DEVICE)
#if (BLE_SPOTA_RECEIVER)
spotar_init();
#endif // (BLE_SPOTA_RECEIVER)
#if (BLE_SPS_CLIENT)
sps_client_init();
#endif // (BLE_SPS_CLIENT)
#if (BLE_SPS_SERVER)
sps_server_init();
#endif // (BLE_SPS_SERVER)
#if (BLE_SP_SERVER)
scpps_init();
#endif // (BLE_SP_SERVER)
#if (BLE_SP_CLIENT)
scppc_init();
#endif // (BLE_SP_CLIENT)
#if (BLE_BATT_SERVER)
bass_init();
#endif // (BLE_BATT_SERVER)
#if (BLE_BATT_CLIENT)
basc_init();
#endif // (BLE_BATT_CLIENT)
#if (BLE_HID_DEVICE)
hogpd_init();
#endif // (BLE_HID_DEVICE)
#if (BLE_HID_BOOT_HOST)
hogpbh_init();
#endif // (BLE_HID_BOOT_HOST)
#if (BLE_HID_REPORT_HOST)
hogprh_init();
#endif // (BLE_HID_REPORT_HOST)
#if (BLE_GL_COLLECTOR)
glpc_init();
#endif // (BLE_GL_COLLECTOR)
#if (BLE_GL_SENSOR)
glps_init();
#endif // (BLE_GL_SENSOR)
#if (BLE_NEB_CLIENT)
nbpc_init();
#endif // (BLE_NEB_CLIENT)
#if (BLE_NEB_SERVER)
nbps_init();
#endif // (BLE_NEB_SERVER)
#if (BLE_RSC_COLLECTOR)
rscpc_init();
#endif // (BLE_RSC_COLLECTOR)
#if (BLE_RSC_SENSOR)
rscps_init();
#endif // (BLE_RSC_SENSOR)
#if (BLE_CSC_COLLECTOR)
cscpc_init();
#endif // (BLE_CSC_COLLECTOR)
#if (BLE_CSC_SENSOR)
cscps_init();
#endif // (BLE_CSC_SENSOR)
#if (BLE_CP_COLLECTOR)
cppc_init();
#endif // (BLE_CP_COLLECTOR)
#if (BLE_CP_SENSOR)
cpps_init();
#endif // (BLE_CP_SENSOR)
#if (BLE_LN_COLLECTOR)
lanc_init();
#endif // (BLE_LN_COLLECTOR)
#if (BLE_LN_SENSOR)
lans_init();
#endif // (BLE_LN_SENSOR)
#if (BLE_AN_CLIENT)
anpc_init();
#endif // (BLE_AN_CLIENT)
#if (BLE_AN_SERVER)
anps_init();
#endif // (BLE_AN_SERVER)
#if (BLE_ANC_CLIENT)
ancc_init();
#endif // (BLE_ANC_CLIENT)
#if (BLE_PAS_CLIENT)
paspc_init();
#endif // (BLE_PAS_CLIENT)
#if (BLE_PAS_SERVER)
pasps_init();
#endif // (BLE_PAS_SERVER)
#if (BLE_SAMPLE128)
sample128_init();
#endif // (BLE_SAMPLE128)
#if (BLE_WPT_CLIENT)
wptc_init();
#endif // WPT_CLIENT
#if (BLE_WPTS)
wpts_init();
#endif // BLE_WPTS
#if (BLE_IEU)
ieu_init();
#endif // (BLE_IEU)
#if (BLE_MPU)
mpu_init();
#endif // (BLE_MPU)
#if (BLE_UDS_SERVER)
udss_init();
#endif // (BLE_UDS_SERVER)
#if (BLE_WSS_SERVER)
wsss_init();
#endif // (BLE_WSS_SERVER)
}
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;
}
