diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 33e4379ed1..4941df12c5 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -227,5 +227,6 @@ protected:
#include "AP_InertialSensor_UserInteract_MAVLink.h"
#include "AP_InertialSensor_Flymaple.h"
#include "AP_InertialSensor_L3G4200D.h"
+#include "AP_InertialSensor_MPU9150.h"
#endif // __AP_INERTIAL_SENSOR_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
new file mode 100644
index 0000000000..3b3ee6bd40
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
@@ -0,0 +1,1183 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+
+ -- Coded by Victor Mayoral Vilches --
+
+ The MPU9150 is a sensor composed by a MPU6050 with a AK8975 on the auxiliary bus.
+ Please check the following links for datasheets and documentation:
+ - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf
+ - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf
+*/
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include
+#include "AP_InertialSensor_MPU9150.h"
+#include
+#include
+
+const extern AP_HAL::HAL& hal;
+
+///////
+/* Hardware registers needed by driver. */
+struct gyro_reg_s {
+ uint8_t who_am_i;
+ uint8_t rate_div;
+ uint8_t lpf;
+ uint8_t prod_id;
+ uint8_t user_ctrl;
+ uint8_t fifo_en;
+ uint8_t gyro_cfg;
+ uint8_t accel_cfg;
+ uint8_t motion_thr;
+ uint8_t motion_dur;
+ uint8_t fifo_count_h;
+ uint8_t fifo_r_w;
+ uint8_t raw_gyro;
+ uint8_t raw_accel;
+ uint8_t temp;
+ uint8_t int_enable;
+ uint8_t dmp_int_status;
+ uint8_t int_status;
+ uint8_t pwr_mgmt_1;
+ uint8_t pwr_mgmt_2;
+ uint8_t int_pin_cfg;
+ uint8_t mem_r_w;
+ uint8_t accel_offs;
+ uint8_t i2c_mst;
+ uint8_t bank_sel;
+ uint8_t mem_start_addr;
+ uint8_t prgm_start_h;
+ uint8_t raw_compass;
+ uint8_t yg_offs_tc;
+ uint8_t s0_addr;
+ uint8_t s0_reg;
+ uint8_t s0_ctrl;
+ uint8_t s1_addr;
+ uint8_t s1_reg;
+ uint8_t s1_ctrl;
+ uint8_t s4_ctrl;
+ uint8_t s0_do;
+ uint8_t s1_do;
+ uint8_t i2c_delay_ctrl;
+};
+
+/* Information specific to a particular device. */
+struct hw_s {
+ uint8_t addr;
+ uint16_t max_fifo;
+ uint8_t num_reg;
+ uint16_t temp_sens;
+ int16_t temp_offset;
+ uint16_t bank_size;
+ uint16_t compass_fsr;
+};
+
+/* Information for self-test. */
+struct test_s {
+ uint32_t gyro_sens;
+ uint32_t accel_sens;
+ uint8_t reg_rate_div;
+ uint8_t reg_lpf;
+ uint8_t reg_gyro_fsr;
+ uint8_t reg_accel_fsr;
+ uint16_t wait_ms;
+ uint8_t packet_thresh;
+ float min_dps;
+ float max_dps;
+ float max_gyro_var;
+ float min_g;
+ float max_g;
+ float max_accel_var;
+};
+
+/* Gyro driver state variables. */
+struct gyro_state_s {
+ const struct gyro_reg_s *reg;
+ const struct hw_s *hw;
+ const struct test_s *test;
+};
+
+/* Filter configurations. The values correspond to the DLPF_CFG register.
+Note that the gyro and accel frequencies are slightly different.
+ (DLPF_CFG register, RM-MPU-9150A00.pdf, pg. 13)
+*/
+enum lpf_e {
+ INV_FILTER_256HZ_NOLPF2 = 0,
+ INV_FILTER_188HZ = 1,
+ INV_FILTER_98HZ = 2,
+ INV_FILTER_42HZ = 3,
+ INV_FILTER_20HZ = 4,
+ INV_FILTER_10HZ = 5,
+ INV_FILTER_5HZ = 6,
+ INV_FILTER_2100HZ_NOLPF = 7,
+ NUM_FILTER
+};
+
+/* Full scale ranges. FS_SEL register.
+ (RM-MPU-9150A00.pdf, pg. 14)
+ */
+enum gyro_fsr_e {
+ INV_FSR_250DPS = 0,
+ INV_FSR_500DPS = 1,
+ INV_FSR_1000DPS = 2,
+ INV_FSR_2000DPS = 3,
+ NUM_GYRO_FSR
+};
+
+/* Full scale ranges. AFS_SEL register.
+ (RM-MPU-9150A00.pdf, pg. 15)
+*/
+enum accel_fsr_e {
+ INV_FSR_2G = 0,
+ INV_FSR_4G = 1,
+ INV_FSR_8G = 2,
+ INV_FSR_16G = 4,
+ NUM_ACCEL_FSR
+};
+
+/* Clock sources. */
+enum clock_sel_e {
+ INV_CLK_INTERNAL = 0,
+ INV_CLK_PLL,
+ NUM_CLK
+};
+
+#define MPU9150_ADDRESS 0x68
+
+#define INV_X_GYRO (0x40)
+#define INV_Y_GYRO (0x20)
+#define INV_Z_GYRO (0x10)
+#define INV_XYZ_GYRO (INV_X_GYRO | INV_Y_GYRO | INV_Z_GYRO)
+#define INV_XYZ_ACCEL (0x08)
+#define INV_XYZ_COMPASS (0x01)
+
+#define MPU_INT_STATUS_DATA_READY (0x0001)
+#define MPU_INT_STATUS_DMP (0x0002)
+#define MPU_INT_STATUS_PLL_READY (0x0004)
+#define MPU_INT_STATUS_I2C_MST (0x0008)
+#define MPU_INT_STATUS_FIFO_OVERFLOW (0x0010)
+#define MPU_INT_STATUS_ZMOT (0x0020)
+#define MPU_INT_STATUS_MOT (0x0040)
+#define MPU_INT_STATUS_FREE_FALL (0x0080)
+#define MPU_INT_STATUS_DMP_0 (0x0100)
+#define MPU_INT_STATUS_DMP_1 (0x0200)
+#define MPU_INT_STATUS_DMP_2 (0x0400)
+#define MPU_INT_STATUS_DMP_3 (0x0800)
+#define MPU_INT_STATUS_DMP_4 (0x1000)
+#define MPU_INT_STATUS_DMP_5 (0x2000)
+
+#define BIT_I2C_MST_VDDIO (0x80)
+#define BIT_FIFO_EN (0x40)
+#define BIT_DMP_EN (0x80)
+#define BIT_FIFO_RST (0x04)
+#define BIT_DMP_RST (0x08)
+#define BIT_FIFO_OVERFLOW (0x10)
+#define BIT_DATA_RDY_EN (0x01)
+#define BIT_DMP_INT_EN (0x02)
+#define BIT_MOT_INT_EN (0x40)
+#define BITS_FSR (0x18)
+#define BITS_LPF (0x07)
+#define BITS_HPF (0x07)
+#define BITS_CLK (0x07)
+#define BIT_FIFO_SIZE_1024 (0x40)
+#define BIT_FIFO_SIZE_2048 (0x80)
+#define BIT_FIFO_SIZE_4096 (0xC0)
+#define BIT_RESET (0x80)
+#define BIT_SLEEP (0x40)
+#define BIT_S0_DELAY_EN (0x01)
+#define BIT_S2_DELAY_EN (0x04)
+#define BITS_SLAVE_LENGTH (0x0F)
+#define BIT_SLAVE_BYTE_SW (0x40)
+#define BIT_SLAVE_GROUP (0x10)
+#define BIT_SLAVE_EN (0x80)
+#define BIT_I2C_READ (0x80)
+#define BITS_I2C_MASTER_DLY (0x1F)
+#define BIT_AUX_IF_EN (0x20)
+#define BIT_ACTL (0x80)
+#define BIT_LATCH_EN (0x20)
+#define BIT_ANY_RD_CLR (0x10)
+#define BIT_BYPASS_EN (0x02)
+#define BITS_WOM_EN (0xC0)
+#define BIT_LPA_CYCLE (0x20)
+#define BIT_STBY_XA (0x20)
+#define BIT_STBY_YA (0x10)
+#define BIT_STBY_ZA (0x08)
+#define BIT_STBY_XG (0x04)
+#define BIT_STBY_YG (0x02)
+#define BIT_STBY_ZG (0x01)
+#define BIT_STBY_XYZA (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA)
+#define BIT_STBY_XYZG (BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG)
+// AK8975_SECONDARY
+#define SUPPORTS_AK89xx_HIGH_SENS (0x00)
+#define AK89xx_FSR (9830)
+#define MAX_COMPASS_SAMPLE_RATE (100)
+
+// Gyroscope scale (uncertain where the 0.01745 value comes from)
+#define MPU9150_GYRO_SCALE_2000 (0.0174532f / 16.4f)
+#define MPU9150_GYRO_SCALE_1000 (0.0174532f / 32.8f)
+#define MPU9150_GYRO_SCALE_500 (0.0174532f / 65.5f)
+#define MPU9150_GYRO_SCALE_250 (0.0174532f / 131f)
+
+// Accelerometer scale adjustment
+#define MPU9150_ACCEL_SCALE_16G (GRAVITY_MSS / 2048.0f)
+#define MPU9150_ACCEL_SCALE_8G (GRAVITY_MSS / 4096.0f)
+#define MPU9150_ACCEL_SCALE_4G (GRAVITY_MSS / 8192.0f)
+#define MPU9150_ACCEL_SCALE_2G (GRAVITY_MSS / 16384.0f)
+
+
+const struct gyro_reg_s reg = {
+/* .who_am_i */ 0x75,
+/* .rate_div */ 0x19,
+/* .lpf */ 0x1A,
+/* .prod_id */ 0x0C,
+/* .user_ctrl */ 0x6A,
+/* .fifo_en */ 0x23,
+/* .gyro_cfg */ 0x1B,
+/* .accel_cfg */ 0x1C,
+/* .motion_thr */ 0x1F,
+/* .motion_dur */ 0x20,
+/* .fifo_count_h */ 0x72,
+/* .fifo_r_w */ 0x74,
+/* .raw_gyro */ 0x43,
+/* .raw_accel */ 0x3B,
+/* .temp */ 0x41,
+/* .int_enable */ 0x38,
+/* .dmp_int_status*/ 0x39,
+/* .int_status */ 0x3A,
+/* .pwr_mgmt_1 */ 0x6B,
+/* .pwr_mgmt_2 */ 0x6C,
+/* .int_pin_cfg */ 0x37,
+/* .mem_r_w */ 0x6F,
+/* .accel_offs */ 0x06,
+/* .i2c_mst */ 0x24,
+/* .bank_sel */ 0x6D,
+/* .mem_start_addr*/ 0x6E,
+/* .prgm_start_h */ 0x70,
+/* .raw_compass */ 0x49,
+/* .yg_offs_tc */ 0x01,
+/* .s0_addr */ 0x25,
+/* .s0_reg */ 0x26,
+/* .s0_ctrl */ 0x27,
+/* .s1_addr */ 0x28,
+/* .s1_reg */ 0x29,
+/* .s1_ctrl */ 0x2A,
+/* .s4_ctrl */ 0x34,
+/* .s0_do */ 0x63,
+/* .s1_do */ 0x64,
+/* .i2c_delay_ctrl*/ 0x67
+};
+
+const struct hw_s hw = {
+/* .addr */ 0x68,
+/* .max_fifo */ 1024,
+/* .num_reg */ 118,
+/* .temp_sens */ 340,
+/* .temp_offset */ -521,
+/* .bank_size */ 256,
+/* .compass_fsr */ AK89xx_FSR
+
+};
+
+const struct test_s test = {
+/* .gyro_sens */ 32768/250,
+/* .accel_sens */ 32768/16,
+/* .reg_rate_div */ 0, /* 1kHz. */
+/* .reg_lpf */ 1, /* 188Hz. */
+/* .reg_gyro_fsr */ 0, /* 250dps. */
+/* .reg_accel_fsr */ 0x18, /* 16g. */
+/* .wait_ms */ 50,
+/* .packet_thresh */ 5, /* 5% */
+/* .min_dps */ 10.f,
+/* .max_dps */ 105.f,
+/* .max_gyro_var */ 0.14f,
+/* .min_g */ 0.3f,
+/* .max_g */ 0.95f,
+/* .max_accel_var */ 0.14f
+};
+
+static struct gyro_state_s st = {
+/* .reg */ ®,
+/* .hw */ &hw,
+/* .test */ &test
+
+};
+
+/**
+ * @brief Constructor
+ */
+AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() :
+ AP_InertialSensor(),
+ _accel_filter_x(800, 10),
+ _accel_filter_y(800, 10),
+ _accel_filter_z(800, 10),
+ _gyro_filter_x(800, 10),
+ _gyro_filter_y(800, 10),
+ _gyro_filter_z(800, 10)
+{
+
+}
+
+/*
+ set the filter frequency
+ */
+void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz)
+{
+ if (filter_hz == 0)
+ filter_hz = _default_filter_hz;
+
+ _accel_filter_x.set_cutoff_frequency(800, filter_hz);
+ _accel_filter_y.set_cutoff_frequency(800, filter_hz);
+ _accel_filter_z.set_cutoff_frequency(800, filter_hz);
+ _gyro_filter_x.set_cutoff_frequency(800, filter_hz);
+ _gyro_filter_y.set_cutoff_frequency(800, filter_hz);
+ _gyro_filter_z.set_cutoff_frequency(800, filter_hz);
+}
+
+/**
+ * @brief Init method
+ * @param[in] Sample_rate The sample rate, check the struct def.
+ * @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful.
+ */
+uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
+{
+ // Sensors pushed to the FIFO.
+ uint8_t sensors;
+
+ switch (sample_rate) {
+ case RATE_50HZ:
+ _default_filter_hz = 10;
+ _sample_period_usec = (1000*1000) / 50;
+ _gyro_samples_needed = 16;
+ break;
+ case RATE_100HZ:
+ _default_filter_hz = 20;
+ _sample_period_usec = (1000*1000) / 100;
+ _gyro_samples_needed = 8;
+ break;
+ case RATE_200HZ:
+ _default_filter_hz = 20;
+ _sample_period_usec = 5000;
+ _gyro_samples_needed = 4;
+ break;
+ case RATE_400HZ:
+ default:
+ _default_filter_hz = 20;
+ _sample_period_usec = 2500;
+ _gyro_samples_needed = 2;
+ break;
+ }
+
+ // get pointer to i2c bus semaphore
+ AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+
+ // take i2c bus sempahore
+ if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){
+ return -1;
+ }
+
+ // Init the sensor
+ // Reset the device
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->pwr_mgmt_1,
+ BIT_RESET);
+ hal.scheduler->delay(100);
+
+ // Wake up the chip
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->pwr_mgmt_1,
+ 0x00);
+
+ // Check product revision
+ // This registers are not documented in the register map.
+ uint8_t buff[6];
+ if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) {
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"));
+ goto failed;
+ }
+ uint8_t rev;
+ rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) |
+ (buff[1] & 0x01);
+
+ if (rev) {
+ if (rev == 1){
+ /* Congrats, these parts are better. */
+ ;
+ }
+ else if (rev == 2){
+ ;
+ }
+ else {
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: Unsupported software product rev.\n"));
+ goto failed;
+ }
+ } else {
+ hal.scheduler->panic(PSTR("Product ID read as 0 indicates device is either incompatible or an MPU3050.\n"));
+ goto failed;
+ }
+
+ // Set gyro full-scale range [250, 500, 1000, 2000]
+ if (mpu_set_gyro_fsr(2000)){
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"));
+ goto failed;
+ }
+ // Set the accel full-scale range
+ if (mpu_set_accel_fsr(2)){
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"));
+ goto failed;
+ }
+ // Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate)
+ if (mpu_set_lpf(_default_filter_hz)){
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"));
+ goto failed;
+ }
+ // Set sampling rate (value must be between 4Hz and 1KHz)
+ if (mpu_set_sample_rate(400)){
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"));
+ goto failed;
+ }
+ // Select which sensors are pushed to FIFO.
+ sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO;
+ if (mpu_configure_fifo(sensors)){
+ hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"));
+ goto failed;
+ }
+
+ // For now the compass is not used.
+ // TODO adjust the functions to the ArduPilot API
+
+ // setup_compass();
+ // if (mpu_set_compass_sample_rate(10, 400))
+ // return -1;
+
+ mpu_set_sensors(sensors);
+
+ // Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp)
+ _set_filter_frequency(_mpu6000_filter);
+
+ // give back i2c semaphore
+ i2c_sem->give();
+ // start the timer process to read samples
+ hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate));
+ return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE;
+
+ failed:
+ // give back i2c semaphore
+ i2c_sem->give();
+ return -1;
+}
+
+/**
+ * @brief Set the gyro full-scale range.
+ * @param[in] fsr Desired full-scale range.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_gyro_fsr(uint16_t fsr)
+{
+ uint8_t data;
+
+ switch (fsr) {
+ case 250:
+ data = INV_FSR_250DPS << 3;
+ break;
+ case 500:
+ data = INV_FSR_500DPS << 3;
+ break;
+ case 1000:
+ data = INV_FSR_1000DPS << 3;
+ break;
+ case 2000:
+ data = INV_FSR_2000DPS << 3;
+ break;
+ default:
+ return -1;
+ }
+
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->gyro_cfg,
+ data);
+ return 0;
+}
+
+/**
+ * @brief Set the accel full-scale range.
+ * @param[in] fsr Desired full-scale range.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_accel_fsr(uint8_t fsr)
+{
+ uint8_t data;
+
+ switch (fsr) {
+ case 2:
+ data = INV_FSR_2G << 3;
+ break;
+ case 4:
+ data = INV_FSR_4G << 3;
+ break;
+ case 8:
+ data = INV_FSR_8G << 3;
+ break;
+ case 16:
+ data = INV_FSR_16G << 3;
+ break;
+ default:
+ return -1;
+ }
+
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->accel_cfg,
+ data);
+ return 0;
+}
+
+/**
+ * @brief Set digital low pass filter.
+ * The following LPF settings are supported: 188, 98, 42, 20, 10, 5.
+ * @param[in] lpf Desired LPF setting.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_lpf(uint16_t lpf)
+{
+ uint8_t data;
+
+ if (lpf >= 188){
+ data = INV_FILTER_188HZ;
+ }
+ else if (lpf >= 98){
+ data = INV_FILTER_98HZ;
+ }
+ else if (lpf >= 42){
+ data = INV_FILTER_42HZ;
+ }
+ else if (lpf >= 20){
+ data = INV_FILTER_20HZ;
+ }
+ else if (lpf >= 10){
+ data = INV_FILTER_10HZ;
+ }
+ else {
+ data = INV_FILTER_5HZ;
+ }
+
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->lpf,
+ data);
+ return 0;
+}
+
+/**
+ * @brief Set sampling rate.
+ * Sampling rate must be between 4Hz and 1kHz.
+ * @param[in] rate Desired sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_sample_rate(uint16_t rate)
+{
+ uint8_t data;
+ // uint16_t sample_rate;
+
+ if (rate < 4){
+ rate = 4;
+ }
+ else if (rate > 1000){
+ rate = 1000;
+ }
+
+ data = 1000 / rate - 1;
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->rate_div,
+ data);
+
+ // sample_rate = 1000 / (1 + data);
+ // mpu_set_compass_sample_rate(min(sample_rate, MAX_COMPASS_SAMPLE_RATE), rate);
+
+ return 0;
+}
+
+/**
+ * @brief Set compass sampling rate.
+ * The compass on the auxiliary I2C bus is read by the MPU hardware at a
+ * maximum of 100Hz. The actual rate can be set to a fraction of the gyro
+ * sampling rate.
+ *
+ * \n WARNING: The new rate may be different than what was requested. Call
+ * mpu_get_compass_sample_rate to check the actual setting.
+ * @param[in] rate Desired compass sampling rate (Hz).
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_compass_sample_rate(uint16_t rate, uint16_t chip_sample_rate)
+{
+ uint8_t div;
+ if (!rate || rate > MAX_COMPASS_SAMPLE_RATE){
+ return -1;
+ }
+
+ div = chip_sample_rate / rate - 1;
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->s4_ctrl,
+ div);
+ return 0;
+}
+
+/**
+ * @brief Select which sensors are pushed to FIFO.
+ * @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * @param[in] sensors Mask of sensors to push to FIFO.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_configure_fifo(uint8_t sensors)
+{
+ int16_t result = 0;
+
+ /* Compass data isn't going into the FIFO. Stop trying. */
+ sensors &= ~INV_XYZ_COMPASS;
+
+ // Enable or disable the interrupts
+ // set_int_enable(1);
+ set_int_enable(0);
+ if (sensors) {
+ if (mpu_reset_fifo(sensors)) {
+ return -1;
+ }
+ }
+ return result;
+}
+
+/**
+ * @brief Enable/disable data ready interrupt.
+ * If the DMP is on, the DMP interrupt is enabled. Otherwise, the data ready
+ * interrupt is used.
+ * @param[in] enable 1 to enable interrupt.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::set_int_enable(uint8_t enable)
+{
+ uint8_t tmp;
+
+ if (enable){
+ tmp = BIT_DATA_RDY_EN;
+ }
+ else {
+ tmp = 0x00;
+ }
+ hal.i2c->writeRegister(st.hw->addr,
+ st.reg->int_enable,
+ tmp);
+ return 0;
+}
+
+/**
+ * @brief Reset FIFO read/write pointers.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_reset_fifo(uint8_t sensors)
+{
+ uint8_t data;
+
+ data = 0;
+ hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data);
+ hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, data);
+ hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
+ data = BIT_FIFO_RST;
+ hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
+
+ data = BIT_FIFO_EN;
+ // data = BIT_FIFO_EN | BIT_AUX_IF_EN;
+ hal.i2c->writeRegister(st.hw->addr,st.reg->user_ctrl, data);
+ hal.scheduler->delay(50);
+
+ // interrupts for the DMP
+ // data = BIT_DATA_RDY_EN;
+ data = 0;
+ hal.i2c->writeRegister(st.hw->addr,st.reg->int_enable, data);
+ // enable FIFO
+ hal.i2c->writeRegister(st.hw->addr,st.reg->fifo_en, sensors);
+ return 0;
+}
+
+
+#if 0
+/* This initialization is similar to the one in ak8975.c.
+ TODO: Use the ArduPilot APIs (write, read, ...), remove the st.chip_cfg cache vars
+*/
+static int AP_InertialSensor_MPU9150::setup_compass(void)
+{
+ uint8_t data[4], akm_addr;
+
+ mpu_set_bypass(1);
+
+ /* Find compass. Possible addresses range from 0x0C to 0x0F. */
+ for (akm_addr = 0x0C; akm_addr <= 0x0F; akm_addr++) {
+ int result;
+ result = i2c_read(akm_addr, AKM_REG_WHOAMI, 1, data);
+ if (!result && (data[0] == AKM_WHOAMI))
+ break;
+ }
+
+ if (akm_addr > 0x0F) {
+ /* TODO: Handle this case in all compass-related functions. */
+ log_e("Compass not found.\n");
+ return -1;
+ }
+
+ st.chip_cfg.compass_addr = akm_addr;
+
+ data[0] = AKM_POWER_DOWN;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ data[0] = AKM_FUSE_ROM_ACCESS;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ /* Get sensitivity adjustment data from fuse ROM. */
+ if (i2c_read(st.chip_cfg.compass_addr, AKM_REG_ASAX, 3, data))
+ return -1;
+ st.chip_cfg.mag_sens_adj[0] = (long)data[0] + 128;
+ st.chip_cfg.mag_sens_adj[1] = (long)data[1] + 128;
+ st.chip_cfg.mag_sens_adj[2] = (long)data[2] + 128;
+
+ data[0] = AKM_POWER_DOWN;
+ if (i2c_write(st.chip_cfg.compass_addr, AKM_REG_CNTL, 1, data))
+ return -1;
+ delay_ms(1);
+
+ mpu_set_bypass(0);
+
+ /* Set up master mode, master clock, and ES bit. */
+ data[0] = 0x40;
+ if (i2c_write(st.hw->addr, st.reg->i2c_mst, 1, data))
+ return -1;
+
+ /* Slave 0 reads from AKM data registers. */
+ data[0] = BIT_I2C_READ | st.chip_cfg.compass_addr;
+ if (i2c_write(st.hw->addr, st.reg->s0_addr, 1, data))
+ return -1;
+
+ /* Compass reads start at this register. */
+ data[0] = AKM_REG_ST1;
+ if (i2c_write(st.hw->addr, st.reg->s0_reg, 1, data))
+ return -1;
+
+ /* Enable slave 0, 8-byte reads. */
+ data[0] = BIT_SLAVE_EN | 8;
+ if (i2c_write(st.hw->addr, st.reg->s0_ctrl, 1, data))
+ return -1;
+
+ /* Slave 1 changes AKM measurement mode. */
+ data[0] = st.chip_cfg.compass_addr;
+ if (i2c_write(st.hw->addr, st.reg->s1_addr, 1, data))
+ return -1;
+
+ /* AKM measurement mode register. */
+ data[0] = AKM_REG_CNTL;
+ if (i2c_write(st.hw->addr, st.reg->s1_reg, 1, data))
+ return -1;
+
+ /* Enable slave 1, 1-byte writes. */
+ data[0] = BIT_SLAVE_EN | 1;
+ if (i2c_write(st.hw->addr, st.reg->s1_ctrl, 1, data))
+ return -1;
+
+ /* Set slave 1 data. */
+ data[0] = AKM_SINGLE_MEASUREMENT;
+ if (i2c_write(st.hw->addr, st.reg->s1_do, 1, data))
+ return -1;
+
+ /* Trigger slave 0 and slave 1 actions at each sample. */
+ data[0] = 0x03;
+ if (i2c_write(st.hw->addr, st.reg->i2c_delay_ctrl, 1, data))
+ return -1;
+
+ /* For the MPU9150, the auxiliary I2C bus needs to be set to VDD. */
+ data[0] = BIT_I2C_MST_VDDIO;
+ if (i2c_write(st.hw->addr, st.reg->yg_offs_tc, 1, data))
+ return -1;
+
+ return 0;
+}
+#endif
+
+/**
+ * @brief Turn specific sensors on/off.
+ * @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * \n INV_XYZ_COMPASS
+ * @param[in] sensors Mask of sensors to wake.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_sensors(uint8_t sensors)
+{
+ uint8_t data;
+ // uint8_t user_ctrl;
+
+ if (sensors & INV_XYZ_GYRO){
+ data = INV_CLK_PLL;
+ }
+ else if (sensors){
+ data = 0;
+ }
+ else {
+ data = BIT_SLEEP;
+ }
+ hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_1, data);
+ data = 0;
+ if (!(sensors & INV_X_GYRO)){
+ data |= BIT_STBY_XG;
+ }
+ if (!(sensors & INV_Y_GYRO)){
+ data |= BIT_STBY_YG;
+ }
+ if (!(sensors & INV_Z_GYRO)){
+ data |= BIT_STBY_ZG;
+ }
+ if (!(sensors & INV_XYZ_ACCEL)){
+ data |= BIT_STBY_XYZA;
+ }
+ hal.i2c->writeRegister(st.hw->addr,st.reg->pwr_mgmt_2, data);
+
+#if 0
+ if (sensors && (sensors != INV_XYZ_ACCEL)){
+ /* Latched interrupts only used in LP accel mode. */
+ mpu_set_int_latched(0);
+ }
+#endif
+
+// // handle the compass, not implemented for now
+// #ifdef AK89xx_SECONDARY
+// #ifdef AK89xx_BYPASS
+// if (sensors & INV_XYZ_COMPASS)
+// mpu_set_bypass(1);
+// else
+// mpu_set_bypass(0);
+// #else
+// if (i2c_read(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
+// return -1;
+// /* Handle AKM power management. */
+// if (sensors & INV_XYZ_COMPASS) {
+// data = AKM_SINGLE_MEASUREMENT;
+// user_ctrl |= BIT_AUX_IF_EN;
+// } else {
+// data = AKM_POWER_DOWN;
+// user_ctrl &= ~BIT_AUX_IF_EN;
+// }
+// if (st.chip_cfg.dmp_on)
+// user_ctrl |= BIT_DMP_EN;
+// else
+// user_ctrl &= ~BIT_DMP_EN;
+// if (i2c_write(st.hw->addr, st.reg->s1_do, 1, &data))
+// return -1;
+// /* Enable/disable I2C master mode. */
+// if (i2c_write(st.hw->addr, st.reg->user_ctrl, 1, &user_ctrl))
+// return -1;
+// #endif
+
+ hal.scheduler->delay(50);
+ return 0;
+}
+
+#if 0
+/**
+ * TODO: Remove the st.chip_cfg cache variables
+ *
+ * @brief Enable latched interrupts.
+ * Any MPU register will clear the interrupt.
+ * @param[in] enable 1 to enable, 0 to disable.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_set_int_latched(uint8_t enable)
+{
+ uint8_t tmp;
+ if (st.chip_cfg.latched_int == enable){
+ return 0;
+ }
+
+ if (enable){
+ tmp = BIT_LATCH_EN | BIT_ANY_RD_CLR;
+ }
+ else {
+ tmp = 0;
+ }
+ if (st.chip_cfg.bypass_mode){
+ tmp |= BIT_BYPASS_EN;
+ }
+ if (st.chip_cfg.active_low_int){
+ tmp |= BIT_ACTL;
+ }
+ hal.i2c->writeRegister(st.hw->addr,st.reg->int_pin_cfg, tmp);
+ st.chip_cfg.latched_int = enable;
+ return 0;
+}
+#endif
+
+
+
+/**
+ * @brief Get one packet from the FIFO.
+ * If @e sensors does not contain a particular sensor, disregard the data
+ * returned to that pointer.
+ * \n @e sensors can contain a combination of the following flags:
+ * \n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
+ * \n INV_XYZ_GYRO
+ * \n INV_XYZ_ACCEL
+ * \n If the FIFO has no new data, @e sensors will be zero.
+ * \n If the FIFO is disabled, @e sensors will be zero and this function will
+ * return a non-zero error code.
+ * @param[out] gyro Gyro data in hardware units.
+ * @param[out] accel Accel data in hardware units.
+ * @param[out] timestamp Timestamp in milliseconds.
+ * @param[out] sensors Mask of sensors read from FIFO.
+ * @param[out] more Number of remaining packets.
+ * @return 0 if successful.
+ */
+int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp,
+ uint8_t *sensors, uint8_t *more)
+{
+ /* Assumes maximum packet size is gyro (6) + accel (6). */
+ uint8_t data[12];
+ uint8_t packet_size = 0;
+ uint16_t fifo_count, index = 0;
+ uint8_t sensors_aux = INV_XYZ_ACCEL| INV_XYZ_GYRO;
+
+ sensors[0] = 0;
+ // We assume we want gyro and accel values
+ packet_size = 12;
+
+ // fifo_count_h register contains the number of samples in the FIFO
+ hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data);
+
+ fifo_count = (data[0] << 8) | data[1];
+ if (fifo_count < packet_size){
+ return 0;
+ }
+ // hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count));
+ if (fifo_count > (st.hw->max_fifo >> 1)) {
+ /* FIFO is 50% full, better check overflow bit. */
+ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data);
+ if (data[0] & BIT_FIFO_OVERFLOW) {
+ mpu_reset_fifo(sensors_aux);
+ return -2;
+ }
+ }
+
+ timestamp = hal.scheduler->millis();
+ // read the data
+ hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
+ more[0] = fifo_count / packet_size - 1;
+ sensors[0] = 0;
+
+ if (index != packet_size) {
+ accel[0] = (data[index+0] << 8) | data[index+1];
+ accel[1] = (data[index+2] << 8) | data[index+3];
+ accel[2] = (data[index+4] << 8) | data[index+5];
+ sensors[0] |= INV_XYZ_ACCEL;
+ index += 6;
+ }
+ if (index != packet_size) {
+ gyro[0] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_X_GYRO;
+ index += 2;
+ }
+ if (index != packet_size) {
+ gyro[1] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_Y_GYRO;
+ index += 2;
+ }
+ if (index != packet_size) {
+ gyro[2] = (data[index+0] << 8) | data[index+1];
+ sensors[0] |= INV_Z_GYRO;
+ index += 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Accumulate values from accels and gyros.
+ *
+ * This method is called periodically by the scheduler.
+ *
+ */
+void AP_InertialSensor_MPU9150::_accumulate(void){
+ // get pointer to i2c bus semaphore
+ AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
+
+ // take i2c bus sempahore
+ if (!i2c_sem->take_nonblocking()){
+ return;
+ }
+
+ // Read accelerometer FIFO to find out how many samples are available
+ /* Assumes maximum packet size is gyro (6) + accel (6). */
+ uint8_t data[12];
+ uint8_t packet_size = 12;
+ uint16_t fifo_count, index = 0;
+ int16_t accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
+
+ // fifo_count_h register contains the number of samples in the FIFO
+ hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_count_h, 2, data);
+ fifo_count = (data[0] << 8) | data[1];
+ if (fifo_count < packet_size){
+ // give back i2c semaphore
+ i2c_sem->give();
+ return;
+ }
+
+ // hal.console->printf_P(PTR("FIFO count: %hd\n", fifo_count));
+ if (fifo_count > (st.hw->max_fifo >> 1)) {
+ /* FIFO is 50% full, better check overflow bit. */
+ hal.i2c->readRegister(st.hw->addr, st.reg->int_status, data);
+ if (data[0] & BIT_FIFO_OVERFLOW) {
+ mpu_reset_fifo(INV_XYZ_ACCEL| INV_XYZ_GYRO);
+ i2c_sem->give();
+ return;
+ }
+ }
+
+ // read the samples
+ for (uint16_t i=0; i< fifo_count; i++) {
+ // read the data
+ // TODO check whether it's possible to read all the packages in a single call
+ hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
+ // TODO, remove all the checking since it's being configured this way.
+ if (index != packet_size) {
+ accel_x = (int16_t) (data[index+0] << 8) | data[index+1];
+ accel_y = (int16_t) (data[index+2] << 8) | data[index+3];
+ accel_z = (int16_t) (data[index+4] << 8) | data[index+5];
+ index += 6;
+ }
+ if (index != packet_size) {
+ gyro_x = (int16_t) (data[index+0] << 8) | data[index+1];
+ index += 2;
+ }
+ if (index != packet_size) {
+ gyro_y = (int16_t) (data[index+0] << 8) | data[index+1];
+ index += 2;
+ }
+ if (index != packet_size) {
+ gyro_z = (int16_t) (data[index+0] << 8) | data[index+1];
+ index += 2;
+ }
+ // reset the index
+ index = 0;
+
+ // TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this
+ // is because the sensor is placed in the bottom side of the board?
+ _accel_filtered = Vector3f(
+ _accel_filter_x.apply(accel_x),
+ _accel_filter_y.apply(accel_y),
+ _accel_filter_z.apply(accel_z));
+
+ _gyro_filtered = Vector3f(
+ _gyro_filter_x.apply(gyro_x),
+ _gyro_filter_y.apply(gyro_y),
+ _gyro_filter_z.apply(gyro_z));
+
+ _gyro_samples_available++;
+ }
+
+ // give back i2c semaphore
+ i2c_sem->give();
+}
+
+bool AP_InertialSensor_MPU9150::_sample_available(void)
+{
+ return (_gyro_samples_available >= _gyro_samples_needed);
+}
+
+bool AP_InertialSensor_MPU9150::wait_for_sample(uint16_t timeout_ms)
+{
+ if (_sample_available()) {
+ _last_sample_time = hal.scheduler->micros();
+ return true;
+ }
+ uint32_t start = hal.scheduler->millis();
+ while ((hal.scheduler->millis() - start) < timeout_ms) {
+ hal.scheduler->delay_microseconds(100);
+ _accumulate();
+ if (_sample_available()) {
+ _last_sample_time = hal.scheduler->micros();
+ return true;
+ }
+ }
+ return false;
+}
+
+bool AP_InertialSensor_MPU9150::update(void)
+{
+ if (!wait_for_sample(1000)) {
+ return false;
+ }
+ Vector3f accel_scale = _accel_scale[0].get();
+
+ _previous_accel[0] = _accel[0];
+
+ hal.scheduler->suspend_timer_procs();
+ _accel[0] = _accel_filtered;
+ _gyro[0] = _gyro_filtered;
+ hal.scheduler->resume_timer_procs();
+
+ // add offsets and rotation
+ _accel[0].rotate(_board_orientation);
+
+ // Adjust for chip scaling to get m/s/s
+ ////////////////////////////////////////////////
+ _accel[0] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available;
+
+ // Now the calibration scale factor
+ _accel[0].x *= accel_scale.x;
+ _accel[0].y *= accel_scale.y;
+ _accel[0].z *= accel_scale.z;
+ _accel[0] -= _accel_offset[0];
+
+ _gyro[0].rotate(_board_orientation);
+
+ // Adjust for chip scaling to get radians/sec
+ _gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available;
+ _gyro[0] -= _gyro_offset[0];
+ ////////////////////////////////////////////////
+
+ _gyro_samples_available = 0;
+
+ if (_last_filter_hz != _mpu6000_filter) {
+ _set_filter_frequency(_mpu6000_filter);
+ _last_filter_hz = _mpu6000_filter;
+ }
+ return true;
+}
+
+// TODO review to make sure it matches
+float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void)
+{
+ // 0.5 degrees/second/minute (a guess)
+ return ToRad(0.5/60);
+}
+
+// TODO review to make sure it matches
+float AP_InertialSensor_MPU9150::get_delta_time(void) const
+{
+ return _sample_period_usec * 1.0e-6f;
+}
+
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
new file mode 100644
index 0000000000..2a6e910eb8
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
@@ -0,0 +1,69 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_INERTIAL_SENSOR_MPU9150_H__
+#define __AP_INERTIAL_SENSOR_MPU9150_H__
+
+#include
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include
+#include "AP_InertialSensor.h"
+#include
+#include
+
+
+class AP_InertialSensor_MPU9150 : public AP_InertialSensor
+{
+public:
+
+ AP_InertialSensor_MPU9150();
+
+ /* Implementation of AP_InertialSensor functions: */
+ bool update();
+ float get_delta_time() const;
+ float get_gyro_drift_rate();
+ bool wait_for_sample(uint16_t timeout_ms);
+
+private:
+ uint16_t _init_sensor( Sample_rate sample_rate );
+ void _accumulate(void);
+ bool _sample_available();
+ // uint64_t _last_update_usec;
+ Vector3f _accel_filtered;
+ Vector3f _gyro_filtered;
+ uint32_t _sample_period_usec;
+ uint32_t _last_sample_time;
+ volatile uint32_t _gyro_samples_available;
+ volatile uint8_t _gyro_samples_needed;
+
+ // // support for updating filter at runtime
+ uint8_t _last_filter_hz;
+ uint8_t _default_filter_hz;
+
+ int16_t mpu_set_gyro_fsr(uint16_t fsr);
+ int16_t mpu_set_accel_fsr(uint8_t fsr);
+ int16_t mpu_set_lpf(uint16_t lpf);
+ int16_t mpu_set_sample_rate(uint16_t rate);
+ int16_t mpu_set_compass_sample_rate(uint16_t rate, uint16_t chip_sample_rate);
+ int16_t mpu_configure_fifo(uint8_t sensors);
+ int16_t set_int_enable(uint8_t enable);
+ int16_t mpu_reset_fifo(uint8_t sensors);
+ int16_t mpu_set_sensors(uint8_t sensors);
+ int16_t mpu_set_int_latched(uint8_t enable);
+ int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more);
+
+ // Filter (specify which one)
+ void _set_filter_frequency(uint8_t filter_hz);
+
+ // Low Pass filters for gyro and accel
+ LowPassFilter2p _accel_filter_x;
+ LowPassFilter2p _accel_filter_y;
+ LowPassFilter2p _accel_filter_z;
+ LowPassFilter2p _gyro_filter_x;
+ LowPassFilter2p _gyro_filter_y;
+ LowPassFilter2p _gyro_filter_z;
+
+
+};
+#endif
+#endif // __AP_INERTIAL_SENSOR_MPU9150_H__
diff --git a/libraries/AP_InertialSensor/examples/MPU9150/MPU9150.pde b/libraries/AP_InertialSensor/examples/MPU9150/MPU9150.pde
new file mode 100644
index 0000000000..5db65388fe
--- /dev/null
+++ b/libraries/AP_InertialSensor/examples/MPU9150/MPU9150.pde
@@ -0,0 +1,202 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+//
+// Simple test for the AP_InertialSensor driver.
+//
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+AP_InertialSensor_MPU9150 ins;
+
+void setup(void)
+{
+ hal.console->println("AP_InertialSensor startup...");
+
+ ins.init(AP_InertialSensor::COLD_START,
+ AP_InertialSensor::RATE_400HZ);
+
+ // display initial values
+ display_offsets_and_scaling();
+ hal.console->println("Complete. Reading:");
+}
+
+void loop(void)
+{
+ int16_t user_input;
+
+ hal.console->println();
+ hal.console->println_P(PSTR(
+ "Menu:\r\n"
+ " c) calibrate accelerometers\r\n"
+ " d) display offsets and scaling\r\n"
+ " l) level (capture offsets from level)\r\n"
+ " t) test\r\n"
+ " r) reboot"));
+
+ // wait for user input
+ while( !hal.console->available() ) {
+ hal.scheduler->delay(20);
+ }
+
+ // read in user input
+ while( hal.console->available() ) {
+ user_input = hal.console->read();
+
+ if( user_input == 'c' || user_input == 'C' ) {
+ run_calibration();
+ display_offsets_and_scaling();
+ }
+
+ if( user_input == 'd' || user_input == 'D' ) {
+ display_offsets_and_scaling();
+ }
+
+ if( user_input == 'l' || user_input == 'L' ) {
+ run_level();
+ display_offsets_and_scaling();
+ }
+
+ if( user_input == 't' || user_input == 'T' ) {
+ run_test();
+ }
+
+ if( user_input == 'r' || user_input == 'R' ) {
+ hal.scheduler->reboot(false);
+ }
+ }
+}
+
+void run_calibration()
+{
+ float roll_trim, pitch_trim;
+ // clear off any other characters (like line feeds,etc)
+ while( hal.console->available() ) {
+ hal.console->read();
+ }
+
+
+#if !defined( __AVR_ATmega1280__ )
+ AP_InertialSensor_UserInteractStream interact(hal.console);
+ ins.calibrate_accel(&interact, roll_trim, pitch_trim);
+#else
+ hal.console->println_P(PSTR("calibrate_accel not available on 1280"));
+#endif
+}
+
+void display_offsets_and_scaling()
+{
+ Vector3f accel_offsets = ins.get_accel_offsets();
+ Vector3f accel_scale = ins.get_accel_scale();
+ Vector3f gyro_offsets = ins.get_gyro_offsets();
+
+ // display results
+ hal.console->printf_P(
+ PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
+ accel_offsets.x,
+ accel_offsets.y,
+ accel_offsets.z);
+ hal.console->printf_P(
+ PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
+ accel_scale.x,
+ accel_scale.y,
+ accel_scale.z);
+ hal.console->printf_P(
+ PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
+ gyro_offsets.x,
+ gyro_offsets.y,
+ gyro_offsets.z);
+}
+
+void run_level()
+{
+ // clear off any input in the buffer
+ while( hal.console->available() ) {
+ hal.console->read();
+ }
+
+ // display message to user
+ hal.console->print("Place APM on a level surface and press any key..\n");
+
+ // wait for user input
+ while( !hal.console->available() ) {
+ hal.scheduler->delay(20);
+ }
+ while( hal.console->available() ) {
+ hal.console->read();
+ }
+
+ // run accel level
+ ins.init_accel();
+
+ // display results
+ display_offsets_and_scaling();
+}
+
+void run_test()
+{
+ Vector3f accel;
+ Vector3f gyro;
+ float length;
+ uint32_t counter = 0;
+
+ // flush any user input
+ while( hal.console->available() ) {
+ hal.console->read();
+ }
+
+ // clear out any existing samples from ins
+ ins.update();
+
+ // loop as long as user does not press a key
+ while( !hal.console->available() ) {
+
+ // wait until we have a sample
+ ins.wait_for_sample(1000);
+
+ // read samples from ins
+ ins.update();
+ accel = ins.get_accel();
+ gyro = ins.get_gyro();
+
+ length = accel.length();
+
+ if (counter++ % 50 == 0) {
+ // display results
+ hal.console->printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f\n"),
+ accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z);
+ }
+ }
+
+ // clear user input
+ while( hal.console->available() ) {
+ hal.console->read();
+ }
+}
+
+AP_HAL_MAIN();
diff --git a/libraries/AP_InertialSensor/examples/MPU9150/Makefile b/libraries/AP_InertialSensor/examples/MPU9150/Makefile
new file mode 100644
index 0000000000..f5daf25151
--- /dev/null
+++ b/libraries/AP_InertialSensor/examples/MPU9150/Makefile
@@ -0,0 +1 @@
+include ../../../../mk/apm.mk