107 lines
2.9 KiB
C
107 lines
2.9 KiB
C
// Copyright 2021 IOsetting <iosetting(at)outlook.com>
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
|
|
#include "mpu6050.h"
|
|
|
|
uint16_t swap(uint16_t num)
|
|
{
|
|
return (num >> 8) | (num << 8);
|
|
}
|
|
|
|
void MPU6050_Write(uint8_t addr, uint8_t dat)
|
|
{
|
|
I2C_Write(MPU6050_ADDR, addr, &dat, 1);
|
|
}
|
|
|
|
uint8_t MPU6050_Read(uint8_t addr)
|
|
{
|
|
uint8_t ret;
|
|
I2C_Read(MPU6050_ADDR, addr, &ret, 1);
|
|
return ret;
|
|
}
|
|
|
|
uint16_t MPU6050_ReadInt(uint8_t addr)
|
|
{
|
|
uint16_t ret;
|
|
I2C_Read(MPU6050_ADDR, addr, (uint8_t *)&ret, 2);
|
|
return swap(ret); // swap high/low bits for correct order
|
|
}
|
|
|
|
void MPU6050_ReadAll(uint16_t *buf)
|
|
{
|
|
uint8_t i;
|
|
I2C_Read(MPU6050_ADDR, MPU6050_REG_ACCEL_XOUT_H, (uint8_t *)buf, 14);
|
|
for (i = 0; i < 7; i++)
|
|
{
|
|
*(buf + i) = swap(*(buf + i));
|
|
}
|
|
}
|
|
|
|
void MPU6050_Init(void) {
|
|
MPU6050_DisableLowPowerMode();
|
|
MPU6050_SetSampleRateDiv(0x07);
|
|
MPU6050_SetDLPF(MPU6050_DLPF_Delay2ms);
|
|
MPU6050_SetGyroFullScaleRange(MPU6050_Gyro_FullScaleRange_500dps);
|
|
MPU6050_SetAccFullScaleRange(MPU6050_Acc_FullScaleRange_4g);
|
|
}
|
|
|
|
void MPU6050_Reset(void) {
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_1, 0x80);
|
|
}
|
|
|
|
void MPU6050_EnterSleepMode(void) {
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_1, 0x40);
|
|
}
|
|
|
|
void MPU6050_DisableTemperature(HAL_State_t state) {
|
|
uint8_t reg = MPU6050_Read(MPU6050_REG_PWR_MGMT_1);
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_1, reg & ~0x08 | (state << 3));
|
|
}
|
|
|
|
void MPU6050_EnableLowPowerMode(MPU6050_Wakeup_Freq_t freq) {
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_1, 0x28); // 0010,1000 sleep:0, cycle:1, dis_temp:1
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_2, freq << 6 | 0x03); // STBY_XG, STBY_YG, STBY_ZG -> 1
|
|
}
|
|
|
|
void MPU6050_DisableLowPowerMode(void) {
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_1, 0x00);
|
|
MPU6050_Write(MPU6050_REG_PWR_MGMT_2, 0x00);
|
|
}
|
|
|
|
/**
|
|
* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
|
* where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or 7), and 1kHz
|
|
* when the DLPF is enabled
|
|
*/
|
|
void MPU6050_SetSampleRateDiv(uint8_t div)
|
|
{
|
|
MPU6050_Write(MPU6050_REG_SMPLRT_DIV, div);
|
|
}
|
|
|
|
void MPU6050_SetDLPF(MPU6050_DLPF_t filter)
|
|
{
|
|
MPU6050_Write(MPU6050_REG_CONFIG, filter);
|
|
}
|
|
|
|
void MPU6050_SetGyroFullScaleRange(MPU6050_Gyro_FullScaleRange_t range)
|
|
{
|
|
MPU6050_Write(MPU6050_REG_GYRO_CONFIG, range << 3);
|
|
}
|
|
|
|
void MPU6050_SetAccFullScaleRange(MPU6050_Acc_FullScaleRange_t range)
|
|
{
|
|
MPU6050_Write(MPU6050_REG_ACCEL_CONFIG, range << 3);
|
|
}
|