Baseflight MPU 6050 - FabLabSeoul/WingProject GitHub Wiki

typedef struct sensor_t {
    sensorInitFuncPtr init;               // initialize function
    sensorReadFuncPtr read;               // read 3 axis data function
    sensorReadFuncPtr temperature;        // read temperature if available
    float scale;           // scalefactor (currently used for gyro only, todo for accel)
} sensor_t;

typedef void (*sensorInitFuncPtr)(sensor_align_e align);   // sensor init prototype
typedef void (*sensorReadFuncPtr)(int16_t *data);          // sensor read and align prototype

typedef enum {
    ALIGN_DEFAULT = 0,                                      // driver-provided alignment
    CW0_DEG = 1,
    CW90_DEG = 2,
    CW180_DEG = 3,
    CW270_DEG = 4,
    CW0_DEG_FLIP = 5,
    CW90_DEG_FLIP = 6,
    CW180_DEG_FLIP = 7,
    CW270_DEG_FLIP = 8
} sensor_align_e;

// 16.4 dps/lsb scalefactor
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

actual angle[ axis] = gyro->angle[ axis] * gyro->scale * deltaT;

extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;

bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
{
    bool ack;
    uint8_t sig, rev;
    uint8_t tmp[6];

    delay(35);                  // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe

    ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
    if (!ack)
        return false;

    // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
    // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0뭩 7-bit I2C address.
    // The least significant bit of the MPU-60X0뭩 I2C address is determined by the value of the AD0 pin. (we know that already).
    // But here's the best part: The value of the AD0 pin is not reflected in this register.
	// 정상적으로 작동한다면 sig 값은 1101000(b) = 0x68 값을 가진다. 
    if (sig != (MPU6050_ADDRESS & 0x7e))
        return false;

    // determine product ID and accel revision
    i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp);
    rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01);
    if (rev) {
        /* Congrats, these parts are better. */
        if (rev == 1) {
            mpuAccelHalf = 1;
        } else if (rev == 2) {
            mpuAccelHalf = 0;
        } else {
            failureMode(5);
        }
    } else {
        i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &sig);
        rev = sig & 0x0F;
        if (!rev) {
            failureMode(5);
        } else if (rev == 4) {
            mpuAccelHalf = 1;
        } else {
            mpuAccelHalf = 0;
        }
    }

    acc->init = mpu6050AccInit;
    acc->read = mpu6050AccRead;
    gyro->init = mpu6050GyroInit;
    gyro->read = mpu6050GyroRead;

    // 16.4 dps/lsb scalefactor
    gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;

    // give halfacc (old revision) back to system
    if (scale)
        *scale = mpuAccelHalf;

    // default lpf is 42Hz
    if (lpf >= 188)
        mpuLowPassFilter = INV_FILTER_188HZ;
    else if (lpf >= 98)
        mpuLowPassFilter = INV_FILTER_98HZ;
    else if (lpf >= 42)
        mpuLowPassFilter = INV_FILTER_42HZ;
    else if (lpf >= 20)
        mpuLowPassFilter = INV_FILTER_20HZ;
    else if (lpf >= 10)
        mpuLowPassFilter = INV_FILTER_10HZ;
    else
        mpuLowPassFilter = INV_FILTER_5HZ;

    return true;
}

// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

static void mpu6050AccInit(sensor_align_e align)
{
    if (mpuAccelHalf)
        acc_1G = 255 * 8;
    else
        acc_1G = 512 * 8;

    if (align > 0)
        accAlign = align;
}

static void mpu6050GyroInit(sensor_align_e align)
{
    gpio_config_t gpio;

    gpio.pin = Pin_13;
    gpio.speed = Speed_2MHz;
    gpio.mode = Mode_IN_FLOATING;
    gpioInit(GPIOC, &gpio);

    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
    delay(100);
    i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00);      //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03);      //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
    i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter);  //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);

    // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
    // Accel scale 8g (4096 LSB/g)
    i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

    if (align > 0)
        gyroAlign = align;
}

int main()
{
	if (!setup())
		return 0;

	calibrate_sensors();
	set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);

	while(1)
	{
		int16_t accelgyro[6];	
		acc.read(accelgyro);
		gyro.read(accelgyro+3);
		
		SendSerialAccelGryro(accelgyro);
		
		delay(10);
	}
}