From 98a52f1ccdda71acb434f3fbc014a2a4ef2a96d3 Mon Sep 17 00:00:00 2001 From: IOsetting Date: Thu, 20 Jan 2022 22:55:44 +0800 Subject: [PATCH] feat: mpu6050 demo, i2c --- demo/i2c/mpu6050/main.c | 82 +++++++++++++++++++ demo/i2c/mpu6050/mpu6050.c | 106 +++++++++++++++++++++++++ demo/i2c/mpu6050/mpu6050.h | 158 +++++++++++++++++++++++++++++++++++++ include/fw_i2c.h | 14 ++-- src/fw_i2c.c | 43 +++++++--- 5 files changed, 385 insertions(+), 18 deletions(-) create mode 100644 demo/i2c/mpu6050/main.c create mode 100644 demo/i2c/mpu6050/mpu6050.c create mode 100644 demo/i2c/mpu6050/mpu6050.h diff --git a/demo/i2c/mpu6050/main.c b/demo/i2c/mpu6050/main.c new file mode 100644 index 0000000..f5e0023 --- /dev/null +++ b/demo/i2c/mpu6050/main.c @@ -0,0 +1,82 @@ +// Copyright 2021 IOsetting +// +// 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. + +/*** + * Demo: MPU6050, 3-axis gyroscope + 3-axis accelerometer + Digital Motion Processor™ (DMP™) + * Board: STC8H8K64U + * + * P32 -> SCL + * P33 -> SDA + * GND -> GND + * 3.3V -> VCC + */ +#include "fw_hal.h" +#include "mpu6050.h" +#include + +void I2C_Init(void) +{ + // Master mode + I2C_SetWorkMode(I2C_WorkMode_Master); + /** + * I2C clock = SYSCLK / 2 / (__prescaler__ * 2 + 4) + * MPU6050 works with i2c clock up to 400KHz + * + * 44.2368 / 2 / (26 * 2 + 4) = 0.39 MHz + */ + I2C_SetClockPrescaler(0x1A); + // Switch alternative port + I2C_SetPort(I2C_AlterPort_P32_P33); + // Start I2C + I2C_SetEnabled(HAL_State_ON); +} + +void GPIO_Init(void) +{ + // SDA + GPIO_P3_SetMode(GPIO_Pin_3, GPIO_Mode_InOut_QBD); + // SCL + GPIO_P3_SetMode(GPIO_Pin_2, GPIO_Mode_Output_PP); +} + +int main(void) +{ + uint8_t i; + uint16_t buf[7]; + + SYS_SetClock(); + GPIO_Init(); + UART1_Config8bitUart(UART1_BaudSource_Timer1, HAL_State_ON, 115200); + I2C_Init(); + MPU6050_Init(); + + while(1) + { + for (i = 0; i < 100; i++) + { + if (i == 0) + { + MPU6050_EnableLowPowerMode(MPU6050_Wakeup_Freq_1p25Hz); + } + else if (i == 50) + { + MPU6050_DisableLowPowerMode(); + } + MPU6050_ReadAll(buf); + printf("ax:%6d, ay:%6d, az:%6d, tp:%6d, gx:%6d, gy:%6d, gz:%6d\r\n", + buf[0], buf[1], buf[2], (int16_t)buf[3] / 34 + 365, buf[4], buf[5], buf[6]); + SYS_Delay(100); + } + } +} \ No newline at end of file diff --git a/demo/i2c/mpu6050/mpu6050.c b/demo/i2c/mpu6050/mpu6050.c new file mode 100644 index 0000000..5be7ed4 --- /dev/null +++ b/demo/i2c/mpu6050/mpu6050.c @@ -0,0 +1,106 @@ +// Copyright 2021 IOsetting +// +// 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); +} diff --git a/demo/i2c/mpu6050/mpu6050.h b/demo/i2c/mpu6050/mpu6050.h new file mode 100644 index 0000000..f9a9dfa --- /dev/null +++ b/demo/i2c/mpu6050/mpu6050.h @@ -0,0 +1,158 @@ +// Copyright 2021 IOsetting +// +// 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. + +#ifndef __FW_MPU6050__ +#define __FW_MPU6050__ + +#include "fw_hal.h" + + +#define MPU6050_ADDR 0xD0 // MPU6050 address for write +/** + * Register Address +*/ +#define MPU6050_REG_SELF_TEST_X 0x0D // RW +#define MPU6050_REG_SELF_TEST_Y 0x0E // RW +#define MPU6050_REG_SELF_TEST_Z 0x0F // RW +#define MPU6050_REG_SELF_TEST_A 0x10 // RW +#define MPU6050_REG_SMPLRT_DIV 0x19 // RW, Sample Rate Divider +#define MPU6050_REG_CONFIG 0x1A // RW, Configuration +#define MPU6050_REG_GYRO_CONFIG 0x1B // RW, Gyroscope Configuration +#define MPU6050_REG_ACCEL_CONFIG 0x1C // RW, Accelerometer Configuration +#define MPU6050_REG_FIFO_EN 0x23 // RW +#define MPU6050_REG_I2C_MST_CTRL 0x24 // RW +#define MPU6050_REG_I2C_SLV0_ADDR 0x25 // RW +#define MPU6050_REG_I2C_SLV0_REG 0x26 // RW +#define MPU6050_REG_I2C_SLV0_CTRL 0x27 // RW +#define MPU6050_REG_I2C_SLV1_ADDR 0x28 // RW +#define MPU6050_REG_I2C_SLV1_REG 0x29 // RW +#define MPU6050_REG_I2C_SLV1_CTRL 0x2A // RW +#define MPU6050_REG_I2C_SLV2_ADDR 0x2B // RW +#define MPU6050_REG_I2C_SLV2_REG 0x2C // RW +#define MPU6050_REG_I2C_SLV2_CTRL 0x2D // RW +#define MPU6050_REG_I2C_SLV3_ADDR 0x2E // RW +#define MPU6050_REG_I2C_SLV3_REG 0x2F // RW +#define MPU6050_REG_I2C_SLV3_CTRL 0x30 // RW +#define MPU6050_REG_I2C_SLV4_ADDR 0x31 // RW +#define MPU6050_REG_I2C_SLV4_REG 0x32 // RW +#define MPU6050_REG_I2C_SLV4_DO 0x33 // RW +#define MPU6050_REG_I2C_SLV4_CTRL 0x34 // RW +#define MPU6050_REG_I2C_SLV4_DI 0x35 // R +#define MPU6050_REG_I2C_MST_STATUS 0x36 // R +#define MPU6050_REG_INT_PIN_CFG 0x37 // RW +#define MPU6050_REG_INT_ENABLE 0x38 // RW +#define MPU6050_REG_INT_STATUS 0x3A // R +#define MPU6050_REG_ACCEL_XOUT_H 0x3B // R +#define MPU6050_REG_ACCEL_XOUT_L 0x3C // R +#define MPU6050_REG_ACCEL_YOUT_H 0x3D // R +#define MPU6050_REG_ACCEL_YOUT_L 0x3E // R +#define MPU6050_REG_ACCEL_ZOUT_H 0x3F // R +#define MPU6050_REG_ACCEL_ZOUT_L 0x40 // R +#define MPU6050_REG_TEMP_OUT_H 0x41 // R +#define MPU6050_REG_TEMP_OUT_L 0x42 // R +#define MPU6050_REG_GYRO_XOUT_H 0x43 // R +#define MPU6050_REG_GYRO_XOUT_L 0x44 // R +#define MPU6050_REG_GYRO_YOUT_H 0x45 // R +#define MPU6050_REG_GYRO_YOUT_L 0x46 // R +#define MPU6050_REG_GYRO_ZOUT_H 0x47 // R +#define MPU6050_REG_GYRO_ZOUT_L 0x48 // R +#define MPU6050_REG_EXT_SENS_DATA_00 0x49 // R +#define MPU6050_REG_EXT_SENS_DATA_01 0x4A // R +#define MPU6050_REG_EXT_SENS_DATA_02 0x4B // R +#define MPU6050_REG_EXT_SENS_DATA_03 0x4C // R +#define MPU6050_REG_EXT_SENS_DATA_04 0x4D // R +#define MPU6050_REG_EXT_SENS_DATA_05 0x4E // R +#define MPU6050_REG_EXT_SENS_DATA_06 0x4F // R +#define MPU6050_REG_EXT_SENS_DATA_07 0x50 // R +#define MPU6050_REG_EXT_SENS_DATA_08 0x51 // R +#define MPU6050_REG_EXT_SENS_DATA_09 0x52 // R +#define MPU6050_REG_EXT_SENS_DATA_10 0x53 // R +#define MPU6050_REG_EXT_SENS_DATA_11 0x54 // R +#define MPU6050_REG_EXT_SENS_DATA_12 0x55 // R +#define MPU6050_REG_EXT_SENS_DATA_13 0x56 // R +#define MPU6050_REG_EXT_SENS_DATA_14 0x57 // R +#define MPU6050_REG_EXT_SENS_DATA_15 0x58 // R +#define MPU6050_REG_EXT_SENS_DATA_16 0x59 // R +#define MPU6050_REG_EXT_SENS_DATA_17 0x5A // R +#define MPU6050_REG_EXT_SENS_DATA_18 0x5B // R +#define MPU6050_REG_EXT_SENS_DATA_19 0x5C // R +#define MPU6050_REG_EXT_SENS_DATA_20 0x5D // R +#define MPU6050_REG_EXT_SENS_DATA_21 0x5E // R +#define MPU6050_REG_EXT_SENS_DATA_22 0x5F // R +#define MPU6050_REG_EXT_SENS_DATA_23 0x60 // R +#define MPU6050_REG_I2C_SLV0_DO 0x63 // RW +#define MPU6050_REG_I2C_SLV1_DO 0x64 // RW +#define MPU6050_REG_I2C_SLV2_DO 0x65 // RW +#define MPU6050_REG_I2C_SLV3_DO 0x66 // RW +#define MPU6050_REG_I2C_MST_DELAY_CTRL 0x67 // RW +#define MPU6050_REG_SIGNAL_PATH_RESET 0x68 // RW +#define MPU6050_REG_USER_CTRL 0x6A // RW +#define MPU6050_REG_PWR_MGMT_1 0x6B // RW +#define MPU6050_REG_PWR_MGMT_2 0x6C // RW +#define MPU6050_REG_FIFO_COUNTH 0x72 // RW +#define MPU6050_REG_FIFO_COUNTL 0x73 // RW +#define MPU6050_REG_FIFO_R_W 0x74 // RW +#define MPU6050_REG_WHO_AM_I 0x75 // R #define + +typedef enum +{ + MPU6050_Wakeup_Freq_1p25Hz = 0x00, + MPU6050_Wakeup_Freq_5Hz = 0x01, + MPU6050_Wakeup_Freq_20Hz = 0x02, + MPU6050_Wakeup_Freq_40Hz = 0x03, +} MPU6050_Wakeup_Freq_t; + +typedef enum +{ + MPU6050_DLPF_Delay0ms = 0x00, + MPU6050_DLPF_Delay2ms = 0x01, + MPU6050_DLPF_Delay3ms = 0x02, + MPU6050_DLPF_Delay5ms = 0x03, + MPU6050_DLPF_Delay8ms = 0x04, + MPU6050_DLPF_Delay13ms = 0x05, + MPU6050_DLPF_Delay19ms = 0x06, +} MPU6050_DLPF_t; + +typedef enum +{ + MPU6050_Gyro_FullScaleRange_250dps = 0x00, + MPU6050_Gyro_FullScaleRange_500dps = 0x01, + MPU6050_Gyro_FullScaleRange_1000dps = 0x02, + MPU6050_Gyro_FullScaleRange_2000dps = 0x03, +} MPU6050_Gyro_FullScaleRange_t; + +typedef enum +{ + MPU6050_Acc_FullScaleRange_2g = 0x00, + MPU6050_Acc_FullScaleRange_4g = 0x01, + MPU6050_Acc_FullScaleRange_8g = 0x02, + MPU6050_Acc_FullScaleRange_16g = 0x03, +} MPU6050_Acc_FullScaleRange_t; + +uint8_t MPU6050_Read(uint8_t addr); +uint16_t MPU6050_ReadInt(uint8_t addr); +void MPU6050_ReadAll(uint16_t *buf); +void MPU6050_Init(void); +void MPU6050_Reset(void); +void MPU6050_EnterSleepMode(void); +void MPU6050_DisableTemperature(HAL_State_t state); +void MPU6050_EnableLowPowerMode(MPU6050_Wakeup_Freq_t freq); +void MPU6050_DisableLowPowerMode(void); +void MPU6050_SetSampleRateDiv(uint8_t div); +void MPU6050_SetDLPF(MPU6050_DLPF_t filter); +void MPU6050_SetGyroFullScaleRange(MPU6050_Gyro_FullScaleRange_t range); +void MPU6050_SetAccFullScaleRange(MPU6050_Acc_FullScaleRange_t range); + + +#endif // __FW_MPU6050__ diff --git a/include/fw_i2c.h b/include/fw_i2c.h index 5e356f2..7307462 100644 --- a/include/fw_i2c.h +++ b/include/fw_i2c.h @@ -67,12 +67,12 @@ typedef enum I2CMSST &= ~0x40; \ } -#define I2C_MasterStart() do{SFRX_ON();I2C_SendMasterCmd(I2C_MasterCmd_Start);SFRX_OFF();}while(0) -#define I2C_MasterSendData(__DATA__) do{SFRX_ON();I2CTXD = (__DATA__); I2C_SendMasterCmd(I2C_MasterCmd_Send);SFRX_OFF();}while(0) -#define I2C_MasterRxAck() do{SFRX_ON();I2C_SendMasterCmd(I2C_MasterCmd_RxAck);SFRX_OFF();}while(0) -#define I2C_MasterAck() do{SFRX_ON();I2CMSST &= ~(0x01); I2C_SendMasterCmd(I2C_MasterCmd_TxAck);SFRX_OFF();}while(0) -#define I2C_MasterNAck() do{SFRX_ON();I2CMSST |= 0x01; I2C_SendMasterCmd(I2C_MasterCmd_TxAck);SFRX_OFF();}while(0) -#define I2C_MasterStop() do{SFRX_ON();I2C_SendMasterCmd(I2C_MasterCmd_Stop);SFRX_OFF();}while(0) +#define I2C_MasterStart() I2C_SendMasterCmd(I2C_MasterCmd_Start) +#define I2C_MasterSendData(__DATA__) do{I2CTXD = (__DATA__); I2C_SendMasterCmd(I2C_MasterCmd_Send);}while(0) +#define I2C_MasterRxAck() I2C_SendMasterCmd(I2C_MasterCmd_RxAck) +#define I2C_MasterAck() do{I2CMSST &= ~(0x01); I2C_SendMasterCmd(I2C_MasterCmd_TxAck);}while(0) +#define I2C_MasterNAck() do{I2CMSST |= 0x01; I2C_SendMasterCmd(I2C_MasterCmd_TxAck);}while(0) +#define I2C_MasterStop() I2C_SendMasterCmd(I2C_MasterCmd_Stop) /** * If enabled, `Send Data`+`RxAck` will be executed automatically after write operation on I2CTXD @@ -101,7 +101,7 @@ typedef enum #define I2C_SetPort(__ALTER_PORT__) (P_SW2 = P_SW2 & ~(0x03 << 4) | ((__ALTER_PORT__) << 4)) -uint8_t I2C_MasterRecv(void); uint8_t I2C_Write(uint8_t devAddr, uint8_t memAddr, uint8_t *dat, uint16_t size); +uint8_t I2C_Read(uint8_t devAddr, uint8_t memAddr, uint8_t *buf, uint16_t size); #endif diff --git a/src/fw_i2c.c b/src/fw_i2c.c index 64fcc23..4431682 100644 --- a/src/fw_i2c.c +++ b/src/fw_i2c.c @@ -13,20 +13,11 @@ // limitations under the License. #include "fw_i2c.h" -#include "fw_tim.h" -#include "fw_sys.h" -uint8_t I2C_MasterRecv(void) -{ - P_SW2 = 0x80; - I2C_SendMasterCmd(I2C_MasterCmd_Recv); - P_SW2 = 0x00; - return I2CRXD; -} - uint8_t I2C_Write(uint8_t devAddr, uint8_t memAddr, uint8_t *dat, uint16_t size) { + SFRX_ON(); I2C_MasterStart(); I2C_MasterSendData(devAddr & 0xFE); I2C_MasterRxAck(); @@ -38,5 +29,35 @@ uint8_t I2C_Write(uint8_t devAddr, uint8_t memAddr, uint8_t *dat, uint16_t size) I2C_MasterRxAck(); } I2C_MasterStop(); - return 0; + SFRX_OFF(); + return HAL_OK; +} + +uint8_t I2C_Read(uint8_t devAddr, uint8_t memAddr, uint8_t *buf, uint16_t size) +{ + SFRX_ON(); + I2C_MasterStart(); + I2C_MasterSendData(devAddr & 0xFE); + I2C_MasterRxAck(); + I2C_MasterSendData(memAddr); + I2C_MasterRxAck(); + I2C_MasterStart(); + I2C_MasterSendData(devAddr | 0x01); + I2C_MasterRxAck(); + while(size--) + { + I2C_SendMasterCmd(I2C_MasterCmd_Recv); + *buf++ = I2CRXD; + if (size == 0) + { + I2C_MasterNAck(); + } + else + { + I2C_MasterAck(); + } + } + I2C_MasterStop(); + SFRX_OFF(); + return HAL_OK; }