librobotcontrol-sys 0.4.0

/**
 * <rc/mpu.h>
 *
 * @brief      A userspace C interface for the invensense MPU6050, MPU6500,
 * MPU9150, and MPU9250.
 *
 * This API allows the user to configure this IMU in two modes: NORMAL and DMP
 *
 * ##Normal Mode
 *
 * The accelerometer, gyroscope, magnetometer, and thermometer can be
 * read directly at any time. To use this mode, call rc_mpu_initialize() with
 * your imu_config and imu_data structs as arguments as defined below. You can
 * then call rc_mpu_read_accel, rc_mpu_read_gyro, rc_mpu_read_mag, or
 * rc_mpu_read_temp at any time to get the latest sensor values.
 *
 * ##DMP Mode
 *
 * Stands for Digital Motion Processor which is a feature of the MPU9250.
 * in this mode, the DMP will sample the sensors internally and fill a FIFO
 * buffer with the data at a fixed rate. Furthermore, the DMP will also
 * calculate a filtered orientation quaternion which is placed in the same
 * buffer. When new data is ready in the buffer, the IMU sends an interrupt to
 * the BeagleBone triggering the buffer read followed by the execution of a
 * function of your choosing set with the rc_mpu_set_dmp_callback() function.
 *
 * @author     James Strawson
 * @date       1/19/2018
 *
 * @addtogroup IMU_MPU
 * @{
 */

#ifndef RC_MPU_H
#define RC_MPU_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
#include <pthread.h>

#define RC_MPU_DEFAULT_I2C_ADDR	0x68 ///< default i2c address if AD0 is left low
#define RC_MPU_ALT_I2C_ADDR	0x69 ///< alternate i2c address if AD0 pin pulled high


// defines for index location within TaitBryan and quaternion vectors
#define TB_PITCH_X	0 ///< Index of the dmp_TaitBryan[] array corresponding to the Pitch (X) axis.
#define TB_ROLL_Y	1 ///< Index of the dmp_TaitBryan[] array corresponding to the Roll (Y) axis.
#define TB_YAW_Z	2 ///< Index of the dmp_TaitBryan[] array corresponding to the Yaw (Z) axis.
#define QUAT_W		0 ///< First index of the dmp_quat[] quaternion vector
#define QUAT_X		1 ///< Second index of the dmp_quat[] quaternion vector
#define QUAT_Y		2 ///< Third index of the dmp_quat[] quaternion vector
#define QUAT_Z		3 ///< Fourth index of the dmp_quat[] quaternion vector

#define DEG_TO_RAD	0.0174532925199	///< multiply to convert degrees to radians
#define RAD_TO_DEG	57.295779513	///< multiply to convert radians to degrees
#define MS2_TO_G	0.10197162129	///< multiply to convert m/s^2 to G
#define G_TO_MS2	9.80665		///< multiply to convert G to m/s^2, standard gravity definition

/**
 * @brief      accelerometer full scale range options
 *
 * The user may choose from 4 full scale ranges of the accelerometer and
 * gyroscope. They have units of gravity (G) and degrees per second (DPS) The
 * defaults values are A_FSR_2G and G_FSR_2000DPS respectively.
 */
typedef enum rc_mpu_accel_fsr_t{
	ACCEL_FSR_2G,
	ACCEL_FSR_4G,
	ACCEL_FSR_8G,
	ACCEL_FSR_16G
} rc_mpu_accel_fsr_t;

/**
 * @brief      gyroscope full scale range options
 *
 * The user may choose from 4 full scale ranges of the accelerometer and
 * gyroscope. They have units of gravity (G) and degrees per second (DPS) The
 * defaults values are A_FSR_2G and G_FSR_2000DPS respectively.
 */
typedef enum rc_mpu_gyro_fsr_t{
	GYRO_FSR_250DPS,
	GYRO_FSR_500DPS,
	GYRO_FSR_1000DPS,
	GYRO_FSR_2000DPS
} rc_mpu_gyro_fsr_t;

/**
 * @brief      accelerometer digital low-pass filter options
 *
 * The user may choose from 7 digital low pass filter constants for the
 * accelerometer and gyroscope. The filter runs at 1kz and helps to reduce
 * sensor noise when sampling more slowly. The default values are ACCEL_DLPF_184
 * GYRO_DLPF_184. Lower cut-off frequencies incur phase-loss in measurements.
 * Number is cutoff frequency in hz.
 */
typedef enum rc_mpu_accel_dlpf_t{
	ACCEL_DLPF_OFF,
	ACCEL_DLPF_460,
	ACCEL_DLPF_184,
	ACCEL_DLPF_92,
	ACCEL_DLPF_41,
	ACCEL_DLPF_20,
	ACCEL_DLPF_10,
	ACCEL_DLPF_5
} rc_mpu_accel_dlpf_t;

/**
 * @brief      gyroscope digital low-pass filter options
 *
 * The user may choose from 7 digital low pass filter constants for the
 * accelerometer and gyroscope. The filter runs at 1kz and helps to reduce
 * sensor noise when sampling more slowly. The default values are ACCEL_DLPF_184
 * GYRO_DLPF_184. Lower cut-off frequencies incur phase-loss in measurements.
 * Number is cutoff frequency in hz.
 */
typedef enum rc_mpu_gyro_dlpf_t{
	GYRO_DLPF_OFF,
	GYRO_DLPF_250,
	GYRO_DLPF_184,
	GYRO_DLPF_92,
	GYRO_DLPF_41,
	GYRO_DLPF_20,
	GYRO_DLPF_10,
	GYRO_DLPF_5
} rc_mpu_gyro_dlpf_t;


/**
 * @brief      Orientation of the sensor.
 *
 * This is only applicable when operating in DMP mode. This is the orientation
 * that the DMP consideres neutral, aka where roll/pitch/yaw are zero.
 */
typedef enum rc_mpu_orientation_t{
	ORIENTATION_Z_UP	= 136,
	ORIENTATION_Z_DOWN	= 396,
	ORIENTATION_X_UP	= 14,
	ORIENTATION_X_DOWN	= 266,
	ORIENTATION_Y_UP	= 112,
	ORIENTATION_Y_DOWN	= 336,
	ORIENTATION_X_FORWARD	= 133,
	ORIENTATION_X_BACK	= 161
} rc_mpu_orientation_t;

/**
 * @brief      configuration of the mpu sensor
 *
 * Configuration struct passed to rc_mpu_initialize and rc_mpu_initialize_dmp.
 * It is best to get the default config with rc_mpu_default_config() function
 * first and modify from there.
 */
typedef struct rc_mpu_config_t{
	/** @name physical connection configuration */
	///@{
	int gpio_interrupt_pin_chip;	///< gpio pin, default 3 on Robotics Cape and BB Blue
	int gpio_interrupt_pin;		///< gpio pin, default 21 on Robotics Cape and BB Blue
	int i2c_bus;			///< which bus to use, default 2 on Robotics Cape and BB Blue
	uint8_t i2c_addr;		///< default is 0x68, pull pin ad0 high to make it 0x69
	int show_warnings;		///< set to 1 to print i2c_bus warnings for debug
	///@}

	/** @name accelerometer, gyroscope, and magnetometer configuration */
	///@{
	rc_mpu_accel_fsr_t accel_fsr;	///< accelerometer full scale range, default ACCEL_FSR_8G
	rc_mpu_gyro_fsr_t gyro_fsr;	///< gyroscope full scale range, default GYRO_FSR_2000DPS
	rc_mpu_accel_dlpf_t accel_dlpf;	///< internal low pass filter cutoff, default ACCEL_DLPF_184
	rc_mpu_gyro_dlpf_t gyro_dlpf;	///< internal low pass filter cutoff, default GYRO_DLPF_184
	int enable_magnetometer;	///< magnetometer use is optional, set to 1 to enable, default 0 (off)
	///@}

	/** @name DMP settings, only used with DMP mode */
	///@{
	int dmp_sample_rate;		///< sample rate in hertz, 200,100,50,40,25,20,10,8,5,4
	int dmp_fetch_accel_gyro;	///< set to 1 to optionally raw accel/gyro when reading DMP quaternion, default: 0 (off)
	int dmp_auto_calibrate_gyro;	///< set to 1 to let DMP auto calibrate the gyro while in use, default: 0 (off)
	rc_mpu_orientation_t orient;	///< DMP orientation matrix, see rc_mpu_orientation_t
	double compass_time_constant;	///< time constant (seconds) for filtering compass with gyroscope yaw value, default 25
	int dmp_interrupt_sched_policy;	///< Scheduler policy for DMP interrupt handler and user callback, default SCHED_OTHER
	int dmp_interrupt_priority;	///< scheduler priority for DMP interrupt handler and user callback, default 0
	int read_mag_after_callback;	///< reads magnetometer after DMP callback function to improve latency, default 1 (true)
	int mag_sample_rate_div;	///< magnetometer_sample_rate = dmp_sample_rate/mag_sample_rate_div, default: 4
	int tap_threshold;		///< threshold impulse for triggering a tap in units of mg/ms
	///@}

} rc_mpu_config_t;


/**
 * @brief      data struct populated with new sensor data
 *
 * This is the container for holding the sensor data. The user is intended to
 * make their own instance of this struct and pass its pointer to imu read
 * functions. new data will then be written back into the user's instance of the
 * data struct.
 */
typedef struct rc_mpu_data_t{
	/** @name base sensor readings in real units */
	///@{
	double accel[3];	///< accelerometer (XYZ) in units of m/s^2
	double gyro[3];		///< gyroscope (XYZ) in units of degrees/s
	double mag[3];		///< magnetometer (XYZ) in units of uT
	double temp;		///< thermometer, in units of degrees Celsius
	///@}

	/** @name 16 bit raw adc readings and conversion rates*/
	///@{
	int16_t raw_gyro[3];	///< raw gyroscope (XYZ)from 16-bit ADC
	int16_t raw_accel[3];	///< raw accelerometer (XYZ) from 16-bit ADC
	double accel_to_ms2;	///< conversion rate from raw accelerometer to m/s^2
	double gyro_to_degs;	///< conversion rate from raw gyroscope to degrees/s
	///@}

	/** @name DMP data */
	///@{
	double dmp_quat[4];	///< normalized quaternion from DMP based on ONLY Accel/Gyro
	double dmp_TaitBryan[3];///< Tait-Bryan angles (roll pitch yaw) in radians from DMP based on ONLY Accel/Gyro
	int tap_detected;	///< set to 1 if there was a tap detect on the last dmp sample, reset to 0 on next sample
	int last_tap_direction;	///< direction of last tap, 1-6 corresponding to X+ X- Y+ Y- Z+ Z-
	int last_tap_count;	///< current counter of rapid consecutive taps
	///@}

	/** @name fused DMP data filtered with magnetometer */
	///@{
	double fused_quat[4];		///< fused and normalized quaternion
	double fused_TaitBryan[3];	///< fused Tait-Bryan angles (roll pitch yaw) in radians
	double compass_heading;		///< fused heading filtered with gyro and accel data, same as Tait-Bryan yaw
	double compass_heading_raw;	///< unfiltered heading from magnetometer
	///@}
} rc_mpu_data_t;


/** @name common functions */
///@{

/**
 * @brief      Returns an rc_mpu_config_t struct with default settings.
 *
 * Use this as a starting point and modify as you wish.
 *
 * @return     Returns an rc_mpu_config_t struct with default settings.
 */
rc_mpu_config_t rc_mpu_default_config(void);

/**
 * @brief      Resets a config struct to defaults.
 *
 * @param[out] conf  Pointer to config struct to be overwritten
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_set_config_to_default(rc_mpu_config_t* conf);

/**
 * @brief      Powers off the MPU
 *
 * Only call this after powering on the MPU with rc_mpu_initialize or
 * rc_mpu_initialize_dmp. This should geenrally be called at the end of your
 * main function to make sure the MPU is put to sleep.
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_power_off(void);
///@} end common functions


/** @name normal one-shot sampling functions */
///@{

/**
 * @brief      Sets up the MPU in normal one-shot sampling mode.
 *
 * First create an instance of the rc_mpu_data_t struct and pass its pointer to
 * rc_mpu_initialize which will then write to. Also pass an rc_mpu_config_t
 * struct with your configruation settings.
 *
 * This function will populate the  accel_to_ms2 and gyro_to_deg fields of the
 * rc_mpu_data_t struct appropriately based on the user-configured full scale
 * ranges.
 *
 * After this, you may read sensor data at any time with the functions
 * rc_mpu_read_accel(), rc_mpu_read_gyro(), and rc_mpu_read_temp(). The magentometer
 * can also be read with rc_mpu_read_mag() if using an MPU9150 or MPU9250 and the
 * enable_magnetometer field in the rc_mpu_config_t struct has been set to 1.
 *
 * Be sure to power off the MPU at the end of your program with
 * rc_mpu_power_off().
 *
 * @param      data  pointer to user's data struct
 * @param[in]  conf  user congiguration data
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_initialize(rc_mpu_data_t* data, rc_mpu_config_t conf);


/**
 * @brief      Reads accelerometer data from the MPU
 *
 * @param      data  Pointer to user's data struct where new data will be
 * written
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_read_accel(rc_mpu_data_t* data);


/**
 * @brief      Reads gyroscope data from the MPU
 *
 * @param      data  Pointer to user's data struct where new data will be
 * written
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_read_gyro(rc_mpu_data_t* data);


/**
 * @brief      Reads thermometer data from the MPU
 *
 * Note this is the internal termperature of the chip, not abient temperature.
 *
 * @param      data  Pointer to user's data struct where new data will be
 * written
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_read_temp(rc_mpu_data_t* data);


/**
 * @brief      Reads magnetometer data from the MPU
 *
 * Note this requires use of an MPU9150 or MPU9250, the MPU6050 and MPU6500 do
 * not have magnetometers. Additionally, the enable_magnetometer flag must has
 * been set in the user's rc_mpu_config_t when it was passed to
 * rc_mpu_initialize()
 *
 * @param      data  Pointer to user's data struct where new data will be
 * written
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_read_mag(rc_mpu_data_t* data);
///@} end normal one-shot sampling functions


/** @name interrupt-driven DMP mode functions */
///@{

/**
 * @brief      Initializes the MPU in DMP mode, see rc_test_dmp example
 *
 * After calling this the user does not need to call the normal read functions
 * rc_mpu_read_accel(), rc_mpu_read_gyro(), or rc_mpu_read mag(). Instead the
 * data will automatically be read into the user's data struct at the
 * dmp_sample_rate set in the config struct.
 *
 * @param      data  Pointer to user's data struct where new data will be
 * written
 * @param[in]  conf  User's configuration struct
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_initialize_dmp(rc_mpu_data_t* data, rc_mpu_config_t conf);


/**
 * @brief      Sets the callback function that will be triggered when new DMP
 * data is ready.
 *
 * @param[in]  func  user's callback function
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_set_dmp_callback(void (*func)(void));


/**
 * @brief      blocking function that returns once new DMP data is available
 *
 * @return     Returns 0 once new data is available, 1 if the MPU is shutting
 * down due to rc_mpu_power_off, or -1 on error.
 */
int rc_mpu_block_until_dmp_data(void);


/**
 * @brief      calculates number of nanoseconds since the last DMP interrupt
 *
 * @return     nanoseconds since last interrupt, or -1 if no interrupt received
 * yet.
 */
int64_t rc_mpu_nanos_since_last_dmp_interrupt(void);


/**
 * @brief      sets the callback function triggered when a tap is detected
 *
 * @param[in]  func  user's callback function
 *
 * @return     0 on success or -1 on failure.
 */
int rc_mpu_set_tap_callback(void (*func)(int direction, int counter));


/**
 * @brief      blocking function that returns when a tap is detected
 *
 * @return     Returns 0 once a tap is detected, 1 if the MPU is shutting down
 * due to rc_mpu_power_off(), or -1 on error.
 */
int rc_mpu_block_until_tap(void);


/**
 * @brief      calculates nanoseconds since last tap was detected
 *
 * @return     nanoseconds since last tap, or -1 if no tap has been detected
 * yet.
 */
int64_t rc_mpu_nanos_since_last_tap(void);
///@} end interrupt-driven DMP mode functions



/** @name calibration functions */
///@{

/**
 * @brief      Runs gyroscope calibration routine
 *
 * This should generally not be used by the user unless they absolutely want to
 * calibrate the gyroscope inside their own program. Instead call the
 * rc_calibrate_gyro example program.
 *
 * @param[in]  conf  Config struct, only used to configure i2c bus and address.
 *
 * @return     0 on success, -1 on failure
 */
int rc_mpu_calibrate_gyro_routine(rc_mpu_config_t conf);


/**
 * @brief      Runs magnetometer calibration routine
 *
 * This should generally not be used by the user unless they absolutely want to
 * calibrate the magnetometer inside their own program. Instead call the
 * rc_calibrate_mag example program.
 *
 * @param[in]  conf  Config struct, only used to configure i2c bus and address.
 *
 * @return     0 on success, -1 on failure
 */
int rc_mpu_calibrate_mag_routine(rc_mpu_config_t conf);


/**
 * @brief      Runs accelerometer calibration routine
 *
 * This should generally not be used by the user unless they absolutely want to
 * calibrate the accelerometer inside their own program. Instead call the
 * rc_calibrate_accel example program.
 *
 * @param[in]  conf  Config struct, only used to configure i2c bus and address.
 *
 * @return     0 on success, -1 on failure
 */
int rc_mpu_calibrate_accel_routine(rc_mpu_config_t conf);


/**
 * @brief      Checks if a gyro calibration file is saved to disk
 *
 * generally used to warn the user that they are running a program without
 * calibration. Can also be used to decide if calibration should be done at the
 * beginning of user's program.
 *
 * @return     1 if calibrated, 0 if not
 */
int rc_mpu_is_gyro_calibrated(void);


/**
 * @brief      Checks if a magnetometer calibration file is saved to disk
 *
 * generally used to warn the user that they are running a program without
 * calibration. Can also be used to decide if calibration should be done at the
 * beginning of user's program.
 *
 * @return     1 if calibrated, 0 if not
 */
int rc_mpu_is_mag_calibrated(void);


/**
 * @brief      Checks if an accelerometer calibration file is saved to disk
 *
 * generally used to warn the user that they are running a program without
 * calibration. Can also be used to decide if calibration should be done at the
 * beginning of user's program.
 *
 * @return     1 if calibrated, 0 if not
 */
int rc_mpu_is_accel_calibrated(void);



///@} end calibration functions

#ifdef __cplusplus
}
#endif

#endif // RC_MPU_H

/** @}  end group IMU_MPU*/