mpu.h

 
#ifndef RC_MPU_MPU_H
#define RC_MPU_MPU_H
 
#ifdef __cplusplus
extern "C" {
#endif
 
 
#include <stdio.h>
#include <stdint.h>
 
#include "dmp_firmware.h"
#include "mpu_defs.h"
#include "dmpKey.h"
#include "dmpmap.h"
#include <pico/stdlib.h>
#include <pico/binary_info.h>
#include <hardware/i2c.h>
#include <hardware/gpio.h>
#include <hardware/flash.h>
 
 
// macros
#ifndef ARRAY_SIZE
#define ARRAY_SIZE(array) sizeof(array)/sizeof(array[0])
#endif
 
#ifndef min
#define min(a, b)   ((a < b) ? a : b)
#endif
 
#ifndef __unused
#define __unused    __attribute__ ((unused))
#endif
 
#ifndef unlikely
#define unlikely(x) __builtin_expect (!!(x), 0)
#endif
 
#ifndef likely
#define likely(x)   __builtin_expect (!!(x), 1)
#endif
 
// Flash-based address of the last sector
#define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE)
 
// there should be 28 or 35 bytes in the FIFO if the magnetometer is disabled
// or enabled.
#define FIFO_LEN_QUAT_TAP 20 // 16 for quat, 4 for tap
#define FIFO_LEN_QUAT_ACCEL_GYRO_TAP 32 // 16 quat, 6 accel, 6 gyro, 4 tap
#define MAX_FIFO_BUFFER (FIFO_LEN_QUAT_ACCEL_GYRO_TAP*5)
 
// error threshold checks
#define QUAT_ERROR_THRESH   (1L<<16) // very precise threshold
#define QUAT_MAG_SQ_NORMALIZED  (1L<<28)
#define QUAT_MAG_SQ_MIN     (QUAT_MAG_SQ_NORMALIZED - QUAT_ERROR_THRESH)
#define QUAT_MAG_SQ_MAX     (QUAT_MAG_SQ_NORMALIZED + QUAT_ERROR_THRESH)
#define GYRO_CAL_THRESH     50  // std dev below which to consider still
#define ACCEL_CAL_THRESH    100 // std dev below which to consider still
#define GYRO_OFFSET_THRESH  500
 
#define DMP_AUTO_CAL_GYRO 0
#define DMP_FETCH_ACCEL_GYRO 0
#define DMP_INIT_ORIENTATION ORIENTATION_Z_UP
#define DMP_TAP_THRESHOLD 210
#define DMP_CFG_SAMPLE_RATE 100
#define DMP_ENABLE_MAGNETOMETER 0
#define DEBUG_OUTPUT 0
 
#define rc_MPU_DEFAULT_I2C_ADDR 0x68 
#define rc_MPU_ALT_I2C_ADDR 0x69 
#define rc_MPU_INTERRUPT_GPIO 22
#define rc_MPU_I2C_BAUDRATE 400000 
 
// defines for index location within TaitBryan and quaternion vectors
#define TB_PITCH_X  0 
#define TB_ROLL_Y   1 
#define TB_YAW_Z    2 
#define QUAT_W      0 
#define QUAT_X      1 
#define QUAT_Y      2 
#define QUAT_Z      3 
 
#define DEG_TO_RAD  0.0174532925199 
#define RAD_TO_DEG  57.295779513    
#define MS2_TO_G    0.10197162129   
#define G_TO_MS2    9.80665     
#define PI      M_PI
#define TWO_PI      (2.0 * M_PI)
#define GYRO_TO_DEG 2000.0/32768.0;
 
#define HAS_MAG
 
 
//static i2c_inst_t* callback_i2c;
 
// enums credit from the RC Control Library
// Copyright (c) 2016 Strawson Design
// Copyright (c) 2020 BeagleBoard.org Foundation
 
// used in accordance with their MIT license
 
typedef enum rc_mpu_accel_fsr_t{
    ACCEL_FSR_2G,
    ACCEL_FSR_4G,
    ACCEL_FSR_8G,
    ACCEL_FSR_16G
} rc_mpu_accel_fsr_t;
 
typedef enum rc_mpu_gyro_fsr_t{
    GYRO_FSR_250DPS,
    GYRO_FSR_500DPS,
    GYRO_FSR_1000DPS,
    GYRO_FSR_2000DPS
} rc_mpu_gyro_fsr_t;
 
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;
 
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;
 
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;
 
typedef enum rc_mpu_cfg_offset_t{
    MPU_GYRO_CONFIG_ADDR = 0,
    MPU_ACCEL_CONFIG_ADDR = 6,
    MPU_MAG_CONFIG_ADDR = 54
} rc_mpu_cfg_offset_t;
 
typedef struct rc_mpu_config_t{
    int gpio_interrupt_pin;     
    i2c_inst_t* i2c_bus;    
    uint8_t i2c_addr;       
    int show_warnings;      
 
    rc_mpu_accel_fsr_t accel_fsr;   
    rc_mpu_gyro_fsr_t gyro_fsr; 
    rc_mpu_accel_dlpf_t accel_dlpf; 
    rc_mpu_gyro_dlpf_t gyro_dlpf;   
    int enable_magnetometer;    
 
    int dmp_sample_rate;        
    int dmp_fetch_accel_gyro;   
    int dmp_auto_calibrate_gyro;    
    rc_mpu_orientation_t orient;    
    double compass_time_constant;   
    int read_mag_after_callback;    
    int mag_sample_rate_div;    
    int tap_threshold;      
 
} rc_mpu_config_t;
 
 
typedef struct rc_mpu_data_t{
    double accel[3];    
    double gyro[3];     
    double mag[3];      
    double temp;        
 
    int16_t raw_gyro[3];    
    int16_t raw_accel[3];   
    double accel_to_ms2;    
    double gyro_to_degs;    
 
    double dmp_quat[4]; 
    double dmp_TaitBryan[3];
    int tap_detected;   
    int last_tap_direction; 
    int last_tap_count; 
 
    double fused_quat[4];       
    double fused_TaitBryan[3];  
    double compass_heading;     
    double compass_heading_raw; 
} rc_mpu_data_t;
 
void rc_dmp_callback(uint gpio, uint32_t events);
 
void mpu9250_read_raw(i2c_inst_t* i2c, int16_t accel[3], int16_t gyro[3], int16_t mag[3], int16_t *temp);
 
 
rc_mpu_config_t rc_mpu_default_config(void);
 
int rc_mpu_set_config_to_default(rc_mpu_config_t* conf);
 
int rc_mpu_power_off(void);
 
 
 
 
int rc_mpu_initialize(rc_mpu_data_t* data, rc_mpu_config_t conf);
 
 
int rc_mpu_read_accel(rc_mpu_data_t* data);
 
 
int rc_mpu_read_gyro(rc_mpu_data_t* data);
 
 
int rc_mpu_read_temp(rc_mpu_data_t* data);
 
 
int rc_mpu_read_mag(rc_mpu_data_t* data);
 
 
 
int rc_mpu_initialize_dmp(rc_mpu_data_t* data, rc_mpu_config_t conf);
 
 
//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);
 
 
//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);
 
 
 
 
int rc_mpu_calibrate_gyro_routine(rc_mpu_config_t conf);
 
 
int rc_mpu_calibrate_mag_routine(rc_mpu_config_t conf);
 
 
int rc_mpu_calibrate_accel_routine(rc_mpu_config_t conf);
 
int rc_mpu_reset_accel_cal(i2c_inst_t* i2c);
 
int rc_mpu_is_gyro_calibrated(void);
 
 
int rc_mpu_is_mag_calibrated(void);
 
 
int rc_mpu_is_accel_calibrated(void);
 
 
int rc_print_gyro_calibration(i2c_inst_t* i2c);
 
 
 
#ifdef __cplusplus
}
#endif
 
#endif // rc_MPU_MPU_H