mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
140 lines
3.9 KiB
C
Executable file
140 lines
3.9 KiB
C
Executable file
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight 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.
|
|
*
|
|
* Foobar 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#include "common/maths.h"
|
|
|
|
#include "system.h"
|
|
#include "bus_i2c.h"
|
|
|
|
#include "accgyro.h"
|
|
#include "accgyro_mpu3050.h"
|
|
|
|
|
|
|
|
// MPU3050, Standard address 0x68
|
|
#define MPU3050_ADDRESS 0x68
|
|
|
|
// Registers
|
|
#define MPU3050_SMPLRT_DIV 0x15
|
|
#define MPU3050_DLPF_FS_SYNC 0x16
|
|
#define MPU3050_INT_CFG 0x17
|
|
#define MPU3050_TEMP_OUT 0x1B
|
|
#define MPU3050_GYRO_OUT 0x1D
|
|
#define MPU3050_USER_CTRL 0x3D
|
|
#define MPU3050_PWR_MGM 0x3E
|
|
|
|
// Bits
|
|
#define MPU3050_FS_SEL_2000DPS 0x18
|
|
#define MPU3050_DLPF_10HZ 0x05
|
|
#define MPU3050_DLPF_20HZ 0x04
|
|
#define MPU3050_DLPF_42HZ 0x03
|
|
#define MPU3050_DLPF_98HZ 0x02
|
|
#define MPU3050_DLPF_188HZ 0x01
|
|
#define MPU3050_DLPF_256HZ 0x00
|
|
|
|
#define MPU3050_USER_RESET 0x01
|
|
#define MPU3050_CLK_SEL_PLL_GX 0x01
|
|
|
|
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
|
|
|
|
static void mpu3050Init(void);
|
|
static void mpu3050Read(int16_t *gyroData);
|
|
static void mpu3050ReadTemp(int16_t *tempData);
|
|
|
|
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
|
|
{
|
|
bool ack;
|
|
|
|
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
|
|
|
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
|
|
if (!ack)
|
|
return false;
|
|
|
|
gyro->init = mpu3050Init;
|
|
gyro->read = mpu3050Read;
|
|
gyro->temperature = mpu3050ReadTemp;
|
|
|
|
// 16.4 dps/lsb scalefactor
|
|
gyro->scale = 1.0f / 16.4f;
|
|
|
|
// default lpf is 42Hz
|
|
switch (lpf) {
|
|
case 256:
|
|
mpuLowPassFilter = MPU3050_DLPF_256HZ;
|
|
break;
|
|
case 188:
|
|
mpuLowPassFilter = MPU3050_DLPF_188HZ;
|
|
break;
|
|
case 98:
|
|
mpuLowPassFilter = MPU3050_DLPF_98HZ;
|
|
break;
|
|
default:
|
|
case 42:
|
|
mpuLowPassFilter = MPU3050_DLPF_42HZ;
|
|
break;
|
|
case 20:
|
|
mpuLowPassFilter = MPU3050_DLPF_20HZ;
|
|
break;
|
|
case 10:
|
|
mpuLowPassFilter = MPU3050_DLPF_10HZ;
|
|
break;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static void mpu3050Init(void)
|
|
{
|
|
bool ack;
|
|
|
|
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
|
|
|
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
|
|
if (!ack)
|
|
failureMode(3);
|
|
|
|
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
|
|
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
|
|
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
|
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
|
}
|
|
|
|
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
|
static void mpu3050Read(int16_t *gyroData)
|
|
{
|
|
uint8_t buf[6];
|
|
|
|
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
|
|
|
|
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
|
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
|
gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
|
}
|
|
|
|
static void mpu3050ReadTemp(int16_t *tempData)
|
|
{
|
|
uint8_t buf[2];
|
|
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
|
|
|
|
*tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
|
|
}
|