Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #2008 from paparazzi/auto_imu_scaling

Browse Source 
MPU based IMUs: default scaling based on selected range
This commit is contained in:
Gautier Hattenberger
2017-02-13 09:47:52 +01:00
committed by GitHub
parent 184cdc9b7d 6ca62208ff
commit feedf244e8
33 changed files with 565 additions and 470 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/apogee/imu_apogee.c
-7
View File
@@ -56,14 +56,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz")
PRINT_CONFIG_VAR(APOGEE_SMPLRT_DIV)
PRINT_CONFIG_VAR(APOGEE_LOWPASS_FILTER)
#ifndef APOGEE_GYRO_RANGE
#define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE)
#ifndef APOGEE_ACCEL_RANGE
#define APOGEE_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE)
#if APOGEE_USE_MPU9150
sw/airborne/boards/apogee/imu_apogee.h
+27 -29
View File
@@ -51,22 +51,24 @@
#define IMU_ACCEL_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
I*/
#ifndef APOGEE_GYRO_RANGE
#define APOGEE_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
#ifndef APOGEE_ACCEL_RANGE
#define APOGEE_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[APOGEE_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[APOGEE_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[APOGEE_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE][1]
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
@@ -75,21 +77,17 @@
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[APOGEE_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE][1]
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
sw/airborne/boards/krooz/imu_krooz.c
-7
View File
@@ -48,14 +48,7 @@
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#endif
PRINT_CONFIG_VAR(KROOZ_ACCEL_RANGE)
struct ImuKrooz imu_krooz;
sw/airborne/boards/krooz/imu_krooz.h
+28 -55
View File
@@ -37,69 +37,42 @@
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 250 deg/s has 131.072 LSB/(deg/s)
* sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/131.072 * pi/180 * 4096 = 0.5454
I*/
#ifndef KROOZ_ACCEL_RANGE
#define KROOZ_ACCEL_RANGE MPU60X0_ACCEL_RANGE_2G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 0.5454
#define IMU_GYRO_P_SENS_NUM 2727
#define IMU_GYRO_P_SENS_DEN 5000
#define IMU_GYRO_Q_SENS 0.5454
#define IMU_GYRO_Q_SENS_NUM 2727
#define IMU_GYRO_Q_SENS_DEN 5000
#define IMU_GYRO_R_SENS 0.5454
#define IMU_GYRO_R_SENS_NUM 2727
#define IMU_GYRO_R_SENS_DEN 5000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#endif
/** default accel sensitivy from the datasheet
* MPU with 2g has 16384 LSB/g
* sens = 9.81 [m/s^2] / 16384 [LSB/g] * 2^INT32_ACCEL_FRAC = 0.6131
*/
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 0.6131
#define IMU_ACCEL_X_SENS_NUM 6131
#define IMU_ACCEL_X_SENS_DEN 10000
#define IMU_ACCEL_Y_SENS 0.6131
#define IMU_ACCEL_Y_SENS_NUM 6131
#define IMU_ACCEL_Y_SENS_DEN 10000
#define IMU_ACCEL_Z_SENS 0.6131
#define IMU_ACCEL_Z_SENS_NUM 6131
#define IMU_ACCEL_Z_SENS_DEN 10000
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 0
#define IMU_ACCEL_Y_NEUTRAL 0
#define IMU_ACCEL_Z_NEUTRAL 0
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[KROOZ_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[KROOZ_ACCEL_RANGE][1]
#endif
#ifndef IMU_KROOZ_ACCEL_AVG_FILTER
#define IMU_KROOZ_ACCEL_AVG_FILTER 15
#endif
sw/airborne/boards/krooz/imu_krooz_memsic.c
-3
View File
@@ -45,9 +45,6 @@
PRINT_CONFIG_VAR(KROOZ_SMPLRT_DIV)
PRINT_CONFIG_VAR(KROOZ_LOWPASS_FILTER)
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
PRINT_CONFIG_VAR(KROOZ_GYRO_RANGE)
#ifndef KROOZ_ACCEL_RANGE
sw/airborne/boards/krooz/imu_krooz_memsic.h
+12 -35
View File
@@ -40,44 +40,21 @@
#include "peripherals/hmc58xx.h"
#include "mcu_periph/spi.h"
// Default configuration
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#ifndef KROOZ_GYRO_RANGE
#define KROOZ_GYRO_RANGE MPU60X0_GYRO_RANGE_250
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 250 deg/s has 131.072 LSB/(deg/s)
* sens = 1/131.072 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/131.072 * pi/180 * 4096 = 0.5454
I*/
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 0.5454
#define IMU_GYRO_P_SENS_NUM 2727
#define IMU_GYRO_P_SENS_DEN 5000
#define IMU_GYRO_Q_SENS 0.5454
#define IMU_GYRO_Q_SENS_NUM 2727
#define IMU_GYRO_Q_SENS_DEN 5000
#define IMU_GYRO_R_SENS 0.5454
#define IMU_GYRO_R_SENS_NUM 2727
#define IMU_GYRO_R_SENS_DEN 5000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[KROOZ_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[KROOZ_GYRO_RANGE][1]
#endif
sw/airborne/peripherals/mpu60x0.c
+29 -1
View File
@@ -24,11 +24,39 @@
*
* MPU-60X0 driver common functions (I2C and SPI).
*
* Still needs the either the I2C or SPI specific implementation.
* Still needs the either I2C or SPI specific implementation.
*/
#include "peripherals/mpu60x0.h"
const float MPU60X0_GYRO_SENS[4] = {
MPU60X0_GYRO_SENS_250,
MPU60X0_GYRO_SENS_500,
MPU60X0_GYRO_SENS_1000,
MPU60X0_GYRO_SENS_2000
};
const int32_t MPU60X0_GYRO_SENS_FRAC[4][2] = {
{ MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_DEN },
{ MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_DEN },
{ MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_DEN },
{ MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_DEN }
};
const float MPU60X0_ACCEL_SENS[4] = {
MPU60X0_ACCEL_SENS_2G,
MPU60X0_ACCEL_SENS_4G,
MPU60X0_ACCEL_SENS_8G,
MPU60X0_ACCEL_SENS_16G
};
const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2] = {
{ MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_DEN },
{ MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_DEN },
{ MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_DEN },
{ MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_DEN }
};
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
{
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
sw/airborne/peripherals/mpu60x0.h
+46
View File
@@ -51,6 +51,52 @@
#define MPU60X0_I2C_NB_SLAVES 5
#endif
/** default gyro sensitivy from the datasheet
* sens = 1/ [LSB/(deg/s)] * pi/180 * 2^INT32_RATE_FRAC
* ex: MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#define MPU60X0_GYRO_SENS_250 0.544883
#define MPU60X0_GYRO_SENS_250_NUM 19327
#define MPU60X0_GYRO_SENS_250_DEN 35470
#define MPU60X0_GYRO_SENS_500 1.08977
#define MPU60X0_GYRO_SENS_500_NUM 57663
#define MPU60X0_GYRO_SENS_500_DEN 52913
#define MPU60X0_GYRO_SENS_1000 2.17953
#define MPU60X0_GYRO_SENS_1000_NUM 18271
#define MPU60X0_GYRO_SENS_1000_DEN 8383
#define MPU60X0_GYRO_SENS_2000 4.35906
#define MPU60X0_GYRO_SENS_2000_NUM 36542
#define MPU60X0_GYRO_SENS_2000_DEN 8383
// Get default sensitivity from a table
extern const float MPU60X0_GYRO_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t MPU60X0_GYRO_SENS_FRAC[4][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
* ex: MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#define MPU60X0_ACCEL_SENS_2G 0.613125
#define MPU60X0_ACCEL_SENS_2G_NUM 981
#define MPU60X0_ACCEL_SENS_2G_DEN 1600
#define MPU60X0_ACCEL_SENS_4G 1.22625
#define MPU60X0_ACCEL_SENS_4G_NUM 981
#define MPU60X0_ACCEL_SENS_4G_DEN 800
#define MPU60X0_ACCEL_SENS_8G 2.4525
#define MPU60X0_ACCEL_SENS_8G_NUM 981
#define MPU60X0_ACCEL_SENS_8G_DEN 400
#define MPU60X0_ACCEL_SENS_16G 4.905
#define MPU60X0_ACCEL_SENS_16G_NUM 981
#define MPU60X0_ACCEL_SENS_16G_DEN 200
// Get default sensitivity from a table
extern const float MPU60X0_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2];
enum Mpu60x0ConfStatus {
MPU60X0_CONF_UNINIT,
MPU60X0_CONF_RESET,
sw/airborne/peripherals/mpu9250.c
+29 -1
View File
@@ -24,11 +24,39 @@
*
* MPU-9250 driver common functions (I2C and SPI).
*
* Still needs the either the I2C or SPI specific implementation.
* Still needs the either I2C or SPI specific implementation.
*/
#include "peripherals/mpu9250.h"
const float MPU9250_GYRO_SENS[4] = {
MPU9250_GYRO_SENS_250,
MPU9250_GYRO_SENS_500,
MPU9250_GYRO_SENS_1000,
MPU9250_GYRO_SENS_2000
};
const int32_t MPU9250_GYRO_SENS_FRAC[4][2] = {
{ MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_DEN },
{ MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_DEN },
{ MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_DEN },
{ MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_DEN }
};
const float MPU9250_ACCEL_SENS[4] = {
MPU9250_ACCEL_SENS_2G,
MPU9250_ACCEL_SENS_4G,
MPU9250_ACCEL_SENS_8G,
MPU9250_ACCEL_SENS_16G
};
const int32_t MPU9250_ACCEL_SENS_FRAC[4][2] = {
{ MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_DEN },
{ MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_DEN },
{ MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_DEN },
{ MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_DEN }
};
void mpu9250_set_default_config(struct Mpu9250Config *c)
{
c->clk_sel = MPU9250_DEFAULT_CLK_SEL;
sw/airborne/peripherals/mpu9250.h
+46
View File
@@ -52,6 +52,52 @@
#define MPU9250_I2C_NB_SLAVES 5
#endif
/** default gyro sensitivy from the datasheet
* sens = 1/ [LSB/(deg/s)] * pi/180 * 2^INT32_RATE_FRAC
* ex: MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#define MPU9250_GYRO_SENS_250 0.544883
#define MPU9250_GYRO_SENS_250_NUM 19327
#define MPU9250_GYRO_SENS_250_DEN 35470
#define MPU9250_GYRO_SENS_500 1.08977
#define MPU9250_GYRO_SENS_500_NUM 57663
#define MPU9250_GYRO_SENS_500_DEN 52913
#define MPU9250_GYRO_SENS_1000 2.17953
#define MPU9250_GYRO_SENS_1000_NUM 18271
#define MPU9250_GYRO_SENS_1000_DEN 8383
#define MPU9250_GYRO_SENS_2000 4.35906
#define MPU9250_GYRO_SENS_2000_NUM 36542
#define MPU9250_GYRO_SENS_2000_DEN 8383
// Get default sensitivity from a table
extern const float MPU9250_GYRO_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t MPU9250_GYRO_SENS_FRAC[4][2];
/** default accel sensitivy from the datasheet
* sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC
* ex: MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#define MPU9250_ACCEL_SENS_2G 0.613125
#define MPU9250_ACCEL_SENS_2G_NUM 981
#define MPU9250_ACCEL_SENS_2G_DEN 1600
#define MPU9250_ACCEL_SENS_4G 1.22625
#define MPU9250_ACCEL_SENS_4G_NUM 981
#define MPU9250_ACCEL_SENS_4G_DEN 800
#define MPU9250_ACCEL_SENS_8G 2.4525
#define MPU9250_ACCEL_SENS_8G_NUM 981
#define MPU9250_ACCEL_SENS_8G_DEN 400
#define MPU9250_ACCEL_SENS_16G 4.905
#define MPU9250_ACCEL_SENS_16G_NUM 981
#define MPU9250_ACCEL_SENS_16G_DEN 200
// Get default sensitivity from a table
extern const float MPU9250_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t MPU9250_ACCEL_SENS_FRAC[4][2];
enum Mpu9250ConfStatus {
MPU9250_CONF_UNINIT,
MPU9250_CONF_RESET,
sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
-7
View File
@@ -64,14 +64,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
PRINT_CONFIG_VAR(ASPIRIN_2_LOWPASS_FILTER)
PRINT_CONFIG_VAR(ASPIRIN_2_SMPLRT_DIV)
#ifndef ASPIRIN_2_GYRO_RANGE
#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_GYRO_RANGE)
#ifndef ASPIRIN_2_ACCEL_RANGE
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE)
sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
+34 -2
View File
@@ -31,10 +31,42 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
/* default sensitivitiy */
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#ifndef ASPIRIN_2_GYRO_RANGE
#define ASPIRIN_2_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
#ifndef ASPIRIN_2_ACCEL_RANGE
#define ASPIRIN_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[ASPIRIN_2_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[ASPIRIN_2_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE][1]
#endif
struct ImuAspirin2Spi {
struct Mpu60x0_Spi mpu;
sw/airborne/subsystems/imu/imu_bebop.c
-7
View File
@@ -60,14 +60,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
PRINT_CONFIG_VAR(BEBOP_SMPLRT_DIV)
PRINT_CONFIG_VAR(BEBOP_LOWPASS_FILTER)
#ifndef BEBOP_GYRO_RANGE
#define BEBOP_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(BEBOP_GYRO_RANGE)
#ifndef BEBOP_ACCEL_RANGE
#define BEBOP_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(BEBOP_ACCEL_RANGE)
struct OrientationReps imu_to_mag_bebop; ///< IMU to magneto rotation
sw/airborne/subsystems/imu/imu_bebop.h
+31 -31
View File
@@ -34,41 +34,41 @@
#include "peripherals/ak8963.h"
#include "peripherals/mpu60x0_i2c.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#ifndef BEBOP_GYRO_RANGE
#define BEBOP_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#ifndef BEBOP_ACCEL_RANGE
#define BEBOP_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[BEBOP_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[BEBOP_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE][1]
#endif
/** Everything that is in the bebop IMU */
struct ImuBebop {
struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device
sw/airborne/subsystems/imu/imu_disco.c
-7
View File
@@ -49,14 +49,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
PRINT_CONFIG_VAR(DISCO_SMPLRT_DIV)
PRINT_CONFIG_VAR(DISCO_LOWPASS_FILTER)
#ifndef DISCO_GYRO_RANGE
#define DISCO_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(DISCO_GYRO_RANGE)
#ifndef DISCO_ACCEL_RANGE
#define DISCO_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(DISCO_ACCEL_RANGE)
/** Disco IMU data */
sw/airborne/subsystems/imu/imu_disco.h
+29 -30
View File
@@ -34,39 +34,38 @@
#include "peripherals/ak8963.h"
#include "peripherals/mpu60x0_i2c.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#ifndef DISCO_GYRO_RANGE
#define DISCO_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#ifndef DISCO_ACCEL_RANGE
#define DISCO_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[DISCO_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[DISCO_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE][1]
#endif
/** Everything that is in the disco IMU */
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
-7
View File
@@ -56,14 +56,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
PRINT_CONFIG_VAR(DROTEK_2_SMPLRT_DIV)
PRINT_CONFIG_VAR(DROTEK_2_LOWPASS_FILTER)
#ifndef DROTEK_2_GYRO_RANGE
#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(DROTEK_2_GYRO_RANGE)
#ifndef DROTEK_2_ACCEL_RANGE
#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(DROTEK_2_ACCEL_RANGE)
#ifndef DROTEK_2_MPU_I2C_ADDR
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
+28 -29
View File
@@ -36,43 +36,42 @@
#include "peripherals/mpu60x0_i2c.h"
#include "peripherals/hmc58xx.h"
#ifndef DROTEK_2_GYRO_RANGE
#define DROTEK_2_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
I*/
#ifndef DROTEK_2_ACCEL_RANGE
#define DROTEK_2_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[DROTEK_2_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[DROTEK_2_GYRO_RANGE][1]
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[DROTEK_2_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[DROTEK_2_ACCEL_RANGE][1]
#endif
struct ImuDrotek2 {
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
sw/airborne/subsystems/imu/imu_mpu6000.c
+1 -8
View File
@@ -50,20 +50,13 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#define IMU_MPU_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#else
#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV.
#error Non-default PERIODIC_FREQUENCY: please define IMU_MPU_LOWPASS_FILTER and IMU_MPU_SMPLRT_DIV.
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
#ifndef IMU_MPU_ACCEL_RANGE
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
sw/airborne/subsystems/imu/imu_mpu6000.h
+34 -1
View File
@@ -31,9 +31,42 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
#ifndef IMU_MPU_ACCEL_RANGE
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#endif
struct ImuMpu6000 {
struct Mpu60x0_Spi mpu;
sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.c
+1 -8
View File
@@ -53,20 +53,13 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#define IMU_MPU_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#else
#error Non-default PERIODIC_FREQUENCY: please define MPU_HMC_LOWPASS_FILTER and MPU_HMC_SMPLRT_DIV.
#error Non-default PERIODIC_FREQUENCY: please define IMU_MPU_LOWPASS_FILTER and IMU_MPU_SMPLRT_DIV.
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
#ifndef IMU_MPU_ACCEL_RANGE
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
// Default channels order
sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h
+34 -1
View File
@@ -32,10 +32,43 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#include "peripherals/hmc58xx.h"
#ifndef IMU_MPU_GYRO_RANGE
#define IMU_MPU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
#ifndef IMU_MPU_ACCEL_RANGE
#define IMU_MPU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE][1]
#endif
struct ImuMpu6000Hmc5883 {
struct Mpu60x0_Spi mpu;
sw/airborne/subsystems/imu/imu_mpu60x0_defaults.h
-66
View File
@@ -1,66 +0,0 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi 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 2, or (at your option)
* any later version.
*
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/imu/imu_mpu60x0_defaults.h
* Default sensitivity definitions for an IMU using the MPU60x0.
*/
#ifndef IMU_MPU60X0_DEFAULTS_H
#define IMU_MPU60X0_DEFAULTS_H
#include "generated/airframe.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU60X0 has 16.4 LSB/(deg/s) at 2000deg/s range
* sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/16.4 * pi/180 * 4096 = 4.359066229
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 4.359
#define IMU_GYRO_P_SENS_NUM 4359
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.359
#define IMU_GYRO_Q_SENS_NUM 4359
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.359
#define IMU_GYRO_R_SENS_NUM 4359
#define IMU_GYRO_R_SENS_DEN 1000
#endif
/** default accel sensitivy from the datasheet
* MPU60X0 has 2048 LSB/g
* fixed point sens: 9.81 [m/s^2] / 2048 [LSB/g] * 2^INT32_ACCEL_FRAC
* sens = 9.81 / 2048 * 1024 = 4.905
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS 4.905
#define IMU_ACCEL_X_SENS_NUM 4905
#define IMU_ACCEL_X_SENS_DEN 1000
#define IMU_ACCEL_Y_SENS 4.905
#define IMU_ACCEL_Y_SENS_NUM 4905
#define IMU_ACCEL_Y_SENS_DEN 1000
#define IMU_ACCEL_Z_SENS 4.905
#define IMU_ACCEL_Z_SENS_NUM 4905
#define IMU_ACCEL_Z_SENS_DEN 1000
#endif
#endif /* IMU_MPU60X0_DEFAULTS_H */
sw/airborne/subsystems/imu/imu_mpu60x0_i2c.c
+1 -8
View File
@@ -47,20 +47,13 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#define IMU_MPU60X0_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#else
#error Non-default PERIODIC_FREQUENCY: please define MPU60X0_HMC_LOWPASS_FILTER and MPU60X0_HMC_SMPLRT_DIV.
#error Non-default PERIODIC_FREQUENCY: please define IMU_MPU60X0_LOWPASS_FILTER and IMU_MPU60X0_SMPLRT_DIV.
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU60X0_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU60X0_SMPLRT_DIV)
#ifndef IMU_MPU60X0_GYRO_RANGE
#define IMU_MPU60X0_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(IMU_MPU60X0_GYRO_RANGE)
#ifndef IMU_MPU60X0_ACCEL_RANGE
#define IMU_MPU60X0_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(IMU_MPU60X0_ACCEL_RANGE)
#ifndef IMU_MPU60X0_I2C_ADDR
sw/airborne/subsystems/imu/imu_mpu60x0_i2c.h
+33 -1
View File
@@ -31,9 +31,41 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_i2c.h"
#ifndef IMU_MPU60X0_GYRO_RANGE
#define IMU_MPU60X0_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
#ifndef IMU_MPU60X0_ACCEL_RANGE
#define IMU_MPU60X0_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[IMU_MPU60X0_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[IMU_MPU60X0_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE][1]
#endif
struct ImuMpu60x0 {
struct Mpu60x0_I2c mpu;
sw/airborne/subsystems/imu/imu_mpu9250_i2c.c
-7
View File
@@ -61,14 +61,7 @@ PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV)
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_LOWPASS_FILTER)
#ifndef IMU_MPU9250_GYRO_RANGE
#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_RANGE)
#ifndef IMU_MPU9250_ACCEL_RANGE
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_RANGE)
#ifndef IMU_MPU9250_I2C_ADDR
sw/airborne/subsystems/imu/imu_mpu9250_i2c.h
+29 -30
View File
@@ -35,39 +35,38 @@
#include "peripherals/mpu9250_i2c.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
I*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#ifndef IMU_MPU9250_GYRO_RANGE
#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#ifndef IMU_MPU9250_ACCEL_RANGE
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#endif
sw/airborne/subsystems/imu/imu_mpu9250_spi.c
-7
View File
@@ -67,14 +67,7 @@ PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV)
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_LOWPASS_FILTER)
#ifndef IMU_MPU9250_GYRO_RANGE
#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_RANGE)
#ifndef IMU_MPU9250_ACCEL_RANGE
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_RANGE)
// Default channels order
sw/airborne/subsystems/imu/imu_mpu9250_spi.h
+29 -30
View File
@@ -35,39 +35,38 @@
#include "peripherals/mpu9250_spi.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
I*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#ifndef IMU_MPU9250_GYRO_RANGE
#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#ifndef IMU_MPU9250_ACCEL_RANGE
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU9250_GYRO_SENS[IMU_MPU9250_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU9250_ACCEL_SENS[IMU_MPU9250_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE][1]
#endif
sw/airborne/subsystems/imu/imu_navstik.c
-7
View File
@@ -60,14 +60,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
PRINT_CONFIG_VAR(NAVSTIK_SMPLRT_DIV)
PRINT_CONFIG_VAR(NAVSTIK_LOWPASS_FILTER)
#ifndef NAVSTIK_GYRO_RANGE
#define NAVSTIK_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(NAVSTIK_GYRO_RANGE)
#ifndef NAVSTIK_ACCEL_RANGE
#define NAVSTIK_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(NAVSTIK_ACCEL_RANGE)
/** Basic Navstik IMU data */
sw/airborne/subsystems/imu/imu_navstik.h
+29 -30
View File
@@ -34,39 +34,38 @@
#include "peripherals/hmc58xx.h"
#include "peripherals/mpu60x0_i2c.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#ifndef NAVSTIK_GYRO_RANGE
#define NAVSTIK_GYRO_RANGE MPU60X0_GYRO_RANGE_1000
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#ifndef NAVSTIK_ACCEL_RANGE
#define NAVSTIK_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[NAVSTIK_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[NAVSTIK_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[NAVSTIK_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[NAVSTIK_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[NAVSTIK_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[NAVSTIK_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[NAVSTIK_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[NAVSTIK_ACCEL_RANGE][1]
#endif
sw/airborne/subsystems/imu/imu_px4fmu.c
-7
View File
@@ -53,14 +53,7 @@ PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
PRINT_CONFIG_VAR(PX4FMU_LOWPASS_FILTER)
PRINT_CONFIG_VAR(PX4FMU_SMPLRT_DIV)
#ifndef PX4FMU_GYRO_RANGE
#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE)
#ifndef PX4FMU_ACCEL_RANGE
#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
PRINT_CONFIG_VAR(PX4FMU_ACCEL_RANGE)
struct ImuPx4fmu imu_px4fmu;
sw/airborne/subsystems/imu/imu_px4fmu.h
+35 -1
View File
@@ -31,10 +31,44 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/imu/imu_mpu60x0_defaults.h"
#include "peripherals/mpu60x0_spi.h"
#include "peripherals/hmc58xx.h"
#ifndef PX4FMU_GYRO_RANGE
#define PX4FMU_GYRO_RANGE MPU60X0_GYRO_RANGE_2000
#endif
PRINT_CONFIG_VAR(PX4FMU_GYRO_RANGE)
#ifndef PX4FMU_ACCEL_RANGE
#define PX4FMU_ACCEL_RANGE MPU60X0_ACCEL_RANGE_16G
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
#define IMU_GYRO_P_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
#define IMU_GYRO_P_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
#define IMU_GYRO_Q_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
#define IMU_GYRO_Q_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
#define IMU_GYRO_Q_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
#define IMU_GYRO_R_SENS MPU60X0_GYRO_SENS[PX4FMU_GYRO_RANGE]
#define IMU_GYRO_R_SENS_NUM MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][0]
#define IMU_GYRO_R_SENS_DEN MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE][1]
#endif
// Set default sensitivity based on range if needed
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
#define IMU_ACCEL_X_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
#define IMU_ACCEL_X_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
#define IMU_ACCEL_Y_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
#define IMU_ACCEL_Y_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
#define IMU_ACCEL_Y_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
#define IMU_ACCEL_Z_SENS MPU60X0_ACCEL_SENS[PX4FMU_ACCEL_RANGE]
#define IMU_ACCEL_Z_SENS_NUM MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][0]
#define IMU_ACCEL_Z_SENS_DEN MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE][1]
#endif
struct ImuPx4fmu {
struct Mpu60x0_Spi mpu;
struct Hmc58xx hmc;