I am attempting to bring code into the Ch32 chips.
I am currently trying to port Mpu6060 to Ch32.
But, I am slow on porting Wire cmds to Stm32 struct cmds.
Am I converting this correctly ?
So far I have :
#include "debug.h"
//
void IIC_Init(u32 bound, u16 address)
{
GPIO_InitTypeDef GPIO_InitStructure={0};
I2C_InitTypeDef I2C_InitTSturcture={0};
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE );
RCC_APB1PeriphClockCmd( RCC_APB1Periph_I2C1, ENABLE );
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init( GPIOC, &GPIO_InitStructure );
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init( GPIOC, &GPIO_InitStructure );
I2C_InitTSturcture.I2C_ClockSpeed = bound;
I2C_InitTSturcture.I2C_Mode = I2C_Mode_I2C;
I2C_InitTSturcture.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitTSturcture.I2C_OwnAddress1 = address;
I2C_InitTSturcture.I2C_Ack = I2C_Ack_Enable;
I2C_InitTSturcture.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_Init( I2C1, &I2C_InitTSturcture );
I2C_Cmd( I2C1, ENABLE );
I2C_AcknowledgeConfig( I2C1, ENABLE );
}
void AT24CXX_Init(void)
{
IIC_Init( 100000, 0xA0);
}
u8 AT24CXX_ReadOneByte(u16 ReadAddr)
{
u8 temp=0;
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0XA0, I2C_Direction_Transmitter );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED ) ) {}
#if (Address_Lenth == Address_8bit)
I2C_SendData( I2C1, (u8)(ReadAddr&0x00FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#elif (Address_Lenth == Address_16bit)
I2C_SendData( I2C1, (u8)(ReadAddr>>8) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)(ReadAddr&0x00FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#elif (Address_Lenth == Address_24bit)
I2C_SendData( I2C1, (u8)(ReadAddr>>16) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)((ReadAddr>>8) & 0x0000FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)(ReadAddr & 0x0000FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#endif
I2C_GenerateSTART( I2C1, ENABLE );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) );
I2C_Send7bitAddress( I2C1, 0XA0, I2C_Direction_Receiver );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED ) );
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_RXNE ) == RESET )
I2C_AcknowledgeConfig( I2C1, DISABLE );
temp = I2C_ReceiveData( I2C1 );
I2C_GenerateSTOP( I2C1, ENABLE );
return temp;
}
/*
Single Byte Write Command
*/
void AT24CXX_WriteOneByte(u16 WriteAddr, u8 DataToWrite)
{
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET );
I2C_GenerateSTART( I2C1, ENABLE );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) );
I2C_Send7bitAddress( I2C1, 0XA0, I2C_Direction_Transmitter );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED ) );
#if (Address_Lenth == Address_8bit)
I2C_SendData( I2C1, (u8)(WriteAddr&0x00FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#elif (Address_Lenth == Address_16bit)
I2C_SendData( I2C1, (u8)(WriteAddr>>8) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)(WriteAddr&0x00FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#elif (Address_Lenth == Address_24bit)
I2C_SendData( I2C1, (u8)(ReadAddr>>16) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)((ReadAddr>>8) & 0x0000FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_SendData( I2C1, (u8)(ReadAddr & 0x0000FF) );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
#endif
if( I2C_GetFlagStatus( I2C1, I2C_FLAG_TXE ) != RESET )
{
I2C_SendData( I2C1, DataToWrite );
}
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, ENABLE );
}
/*********************************************************************
* @fn AT24CXX_Read
*
* @brief Read multiple data from EEPROM.
*
* @param ReadAddr - Read frist address. (AT24c02: 0~255)
* pBuffer - Read data.
* NumToRead - Data number.
*
* @return none
*/
void AT24CXX_Read(u16 ReadAddr, u8 *pBuffer, u16 NumToRead)
{
while(NumToRead)
{
*pBuffer++=AT24CXX_ReadOneByte(ReadAddr++);
NumToRead--;
}
}
/*********************************************************************
* @fn AT24CXX_Write
*
* @brief Write multiple data to EEPROM.
*
* @param WriteAddr - Write frist address. (AT24c02: 0~255)
* pBuffer - Write data.
* NumToWrite - Data number.
*
* @return none
*/
void AT24CXX_Write(u16 WriteAddr, u8 *pBuffer, u16 NumToWrite)
{
while(NumToWrite--)
{
AT24CXX_WriteOneByte(WriteAddr,*pBuffer);
WriteAddr++;
pBuffer++;
Delay_Ms(2);
}
}
//
/*
#include <Wire.h>
*/
float PI = 3.14159265359 ;
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup()
{
/*
Serial.begin(19200);
*/
/*
Wire.begin(); // Initialize comunication
*/
/*
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
*/
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
;
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0X6B, I2C_Direction_Transmitter ); // Talk to the register 6B
;
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED ) );
I2C_SendData( I2C1, (u8)(0x00) );
;
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, ENABLE );
/*
// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Talk to the ACCEL_CONFIG register (1C hex)
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(true);
*/
/*
// Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); // Set the register bits as 00010000 (1000deg/s full scale)
Wire.endTransmission(true);
Delay_Ms(20);
*/
// Call this function if you need to get the IMU error values for your module
calculate_IMU_error();
Delay_Ms(20);
}
void loop()
{
// === Read acceleromter data === //
/*
Wire.beginTransmission(MPU);
*/
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
/*
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0X3B, I2C_Direction_Transmitter ); // Talk to the register 3B
/*
Wire.endTransmission(false);
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, DISABLE );
/*
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTART( I2C1, ENABLE );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) );
I2C_Send7bitAddress( I2C1, 0X6B, I2C_Direction_Receiver );
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED ) );
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_RXNE ) == RESET )
I2C_AcknowledgeConfig( I2C1, DISABLE );
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
/*
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
*/
AccX = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
/*
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
*/
AccY = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
/*
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
*/
AccZ = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
// Calculating Roll and Pitch from the accelerometer data
// accAngleX = (atan( AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleX = (atan( AccY / sqrt( (AccX * AccX) + (AccZ * AccZ))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
// accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
accAngleY = (atan(-1 * AccX / sqrt( (AccY * AccY) + (AccZ * AccZ))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
// === Read gyroscope data === //
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
/*
Wire.beginTransmission(MPU);
*/
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
/*
Wire.write(0x43); // Gyro data first register address 0x43
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0X43, I2C_Direction_Transmitter ); // Talk to the register 3B
/*
Wire.endTransmission(false);
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, DISABLE );
/*
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTART( I2C1, ENABLE );
/*
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
*/
GyroX = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 132.0 ;
/*
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
*/
GyroY = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 132.0 ;
/*
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
*/
GyroZ = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 131.0 ;
// Correct the outputs with the calculated error values
GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
GyroY = GyroY - 2; // GyroErrorY ~(2)
GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime;
// Complementary filter - combine acceleromter and gyro angle values
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
// Print the values on the serial monitor
/*
Serial.print(roll);
Serial.print("/");
Serial.print(pitch);
Serial.print("/");
Serial.println(yaw);
*/
printf("- roll=%f , pitch=%f , yaw=%f \r\n" , roll , pitch , yaw );
}
void calculate_IMU_error()
{
// We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
// Read accelerometer values 200 times
while (c < 200)
{
/*
Wire.beginTransmission(MPU);
*/
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
/*
Wire.write(0x43); // Gyro data first register address 0x43
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0X43, I2C_Direction_Transmitter ); // Talk to the register 3B
/*
Wire.endTransmission(false);
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, DISABLE );
/*
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTART( I2C1, ENABLE );
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
AccX = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
AccY = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
AccZ = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 16384.0 ;
// Sum all readings
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}
//Divide the sum by 200 to get the error value
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;
// Read gyro values 200 times
while (c < 200)
{
/*
Wire.beginTransmission(MPU);
*/
while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET ) {}
I2C_GenerateSTART( I2C1, ENABLE );
/*
Wire.write(0x43); // Gyro data first register address 0x43
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) ) {}
I2C_Send7bitAddress( I2C1, 0X43, I2C_Direction_Transmitter ); // Talk to the register 3B
/*
Wire.endTransmission(false);
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTOP( I2C1, DISABLE );
/*
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
*/
while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ) );
I2C_GenerateSTART( I2C1, ENABLE );
GyroX = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 1.0 ;
GyroY = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 1.0 ;
GyroZ = ( (I2C_ReceiveData( I2C1 ) << 8) | (I2C_ReceiveData( I2C1 ) ) ) / 1.0 ;
// Sum all readings
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}
//Divide the sum by 200 to get the error value
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Print the error values on the Serial Monitor
printf("AccErrorX=%f , AccErrorY=%f , GyroErrorX=%f , GyroErrorY=%f , GyroErrorZ=%f \r\n" , AccErrorX , AccErrorY , GyroErrorX , GyroErrorY , GyroErrorZ );
}
//
int main(void)
{
SystemCoreClockUpdate();
Delay_Init();
setup();
while(1)
{
loop();
}
}
– Any help would be appreciated !