int16_t MPU6050_getAccel_z_raw(){
int16_t tmp;
uint8_t state;
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU6050_ADRESS;
I2CMasterBuffer[1] = MPU6050_RA_ACCEL_ZOUT_H;
I2CMasterBuffer[2] = MPU6050_ADRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return 1;
tmp = (I2CSlaveBuffer[0] << 8) ;
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU6050_ADRESS;
I2CMasterBuffer[1] = MPU6050_RA_ACCEL_ZOUT_L;
I2CMasterBuffer[2] = MPU6050_ADRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return 1;
tmp |= (I2CSlaveBuffer[0]) ;
return tmp - zero_acc_z; /* TBD */
}
int16_t MPU6050_getGyroRoll_raw(){
int16_t tmp;
uint8_t state;
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU6050_ADRESS;
I2CMasterBuffer[1] = MPU6050_RA_GYRO_YOUT_H;
I2CMasterBuffer[2] = MPU6050_ADRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return 1;
tmp = (I2CSlaveBuffer[0] << 8) ;
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU6050_ADRESS;
I2CMasterBuffer[1] = MPU6050_RA_GYRO_YOUT_L;
I2CMasterBuffer[2] = MPU6050_ADRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return 1;
tmp |= (I2CSlaveBuffer[0]) ;
return tmp - zero_gyro_roll;
}
uint8_t MPU6050_init() {
uint8_t state;
clearBuffer();
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_PWR_MGMT_1;
I2CMasterBuffer[2] = 0x01; // reset device y-axis gyro as clk;
state = I2CEngine();
if(state != I2C_OK) return 1;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_CONFIG;
I2CMasterBuffer[2] = 0x00; // Lowpass
//I2CMasterBuffer[2] = 0b00000011; // Lowpass
state = I2CEngine();
if(state != I2C_OK) return 1;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_PWR_MGMT_1;
I2CMasterBuffer[2] = 0x04; // wakeup
state = I2CEngine();
if(state != I2C_OK) return 1;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_GYRO_CONFIG;
I2CMasterBuffer[2] = 0x08; // gyro range 600°/s
state = I2CEngine();
if(state != I2C_OK) return 1;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_ACCEL_CONFIG;
I2CMasterBuffer[2] = 0x18; // 16G range
state = I2CEngine();
if(state != I2C_OK) return 1;
return 0;
}
/**
* AK8963(MPU9250の地磁気センサ)のデータ(MagXYZ) <br>
* をdest_arrayに次のように格納 <br>
* Asa[]: 工場出荷時にAK8963に書きこまれた補正値 <br>
* AK8963の初期化の際にAsa[]に格納 <br>
* byte <br>
* 0 MagX 下位 <br>
* 1 MagX 上位 <br>
* 2 MagY 下位 <br>
* 3 MagY 上位 <br>
* 4 MagZ 下位 <br>
* 5 MagZ 上位 <br>
* さらにセンサの読み取り値を物理量に変換しmag_vec[i]に格納する.
*
* @param *dest_array データの格納先
* @return void
*/
void getDataFromAK8963 (uint8_t *dest_array)
{
volatile int32_t mag[3];
volatile uint32_t i;
volatile int16_t Mag[3];
I2CWriteLength = 2;
I2CReadLength = 6;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x03; //Address 6byte (MagXYZ)
I2CMasterBuffer[2] = AK8963_R;
I2CEngine();
for ( i = 0; i < 3; i++ )
{
Mag[i] = (uint16_t)I2CSlaveBuffer[i<<1] | (((uint16_t)I2CSlaveBuffer[(i<<1)+1])<<8);
Mag[i] = Mag[i]*(uint16_t)((((uint8_t)Asa[i] - 128)/256) + 1);
}
dest_array[0] = (uint8_t)Mag[1];
dest_array[1] = (uint8_t)(Mag[1]>>8);
dest_array[2] = (uint8_t)Mag[0];
dest_array[3] = (uint8_t)(Mag[0]>>8);
Mag[2] = (~Mag[2]) + 1; //地磁気センサのZ軸の取り方を下向きから上向きに変更
dest_array[4] = (uint8_t)Mag[2];
dest_array[5] = (uint8_t)(Mag[2]>>8);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0A; //Address start MagXYZ ADC (need 7.2ms)
I2CMasterBuffer[2] = 0x11;
I2CEngine();
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
//センサ値を地磁気ベクトルに変換
for( i=0;i<3;i++)
{
mag[i] = dest_array[i] + (dest_array[i+1]<<8);
if(mag[i] >= 32767) mag[i] -= 65535;
mag_vec[i] = (float)mag[i]*0.15;
}
}
/**
* MPU9250の初期化(MPU9250のジャイロ,加速度センサ)
*
* @param void
* @return void
*/
void initMPU9250 (void)
{
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x6B; //Address start MPU9250
I2CMasterBuffer[2] = 0x00;
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x37; //Address auxiliary I2C
I2CMasterBuffer[2] = 0x02;
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1A; //Address digital low pass filter
I2CMasterBuffer[2] = 0x02; //100Hz
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1B; //range ang
I2CMasterBuffer[2] = 0x18; //2000deg/s
I2CEngine();
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x1C; //range acc
I2CMasterBuffer[2] = 0x18; //16g
I2CEngine();
wait1msec(10);
}
int16_t MPU6050_getAccel_x_raw(){
int16_t tmp;
uint8_t state;
I2CWriteLength = 2;
I2CReadLength = 2; //1;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_ACCEL_YOUT_H;
I2CMasterBuffer[2] = MPU6050_ADDRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return 1;
tmp = (I2CSlaveBuffer[0]<<8) | I2CSlaveBuffer[1];
// tmp = (I2CSlaveBuffer[0] << 8) ;
//
// I2CWriteLength = 2;
// I2CReadLength = 1;
// I2CMasterBuffer[0] = MPU6050_ADDRESS;
// I2CMasterBuffer[1] = MPU6050_RA_ACCEL_YOUT_L;
// I2CMasterBuffer[2] = MPU6050_ADDRESS | RD_BIT;
//
// state = I2CEngine();
// if(state != I2C_OK) return 1;
//
// tmp |= (I2CSlaveBuffer[0]) ;
return tmp - zero_acc_x ;
}
void writeRegister(uint8_t DeviceAddress, uint8_t regAddress, uint8_t data) {
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = DeviceAddress;
I2CMasterBuffer[1] = regAddress;
I2CMasterBuffer[2] = data;
I2CEngine();
}
/**
* Asa[]に感度調整値を格納.
*
* @param void
* @return void
*/
void initAK8963 (void)
{
volatile uint32_t i;
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a; //フューズROMアクセスモードに移行
I2CMasterBuffer[2] = 0x0f; //
I2CEngine();
wait1msec(10);
I2CWriteLength = 2;
I2CReadLength = 3;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x10; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = AK8963_R;
I2CEngine();
for(i = 0;i < 3;i ++)
{
Asa[i] = I2CSlaveBuffer[i];
}
wait1msec(10);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a;
I2CMasterBuffer[2] = 0x00; //パワーダウンモードに変更
I2CEngine();
wait1msec(100);
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x0a;
I2CMasterBuffer[2] = 0x10; //16bitの分解能に変更
I2CEngine();
wait1msec(10);
}
//=====================
// RTC Write Register
//=====================
void RTC_Write_Reg(uint32_t addr, uint32_t data)
{
I2CWriteLength = 3;
I2CReadLength = 0;
I2CMasterBuffer[0] = RTC_DEV_ADDR;
I2CMasterBuffer[1] = addr;
I2CMasterBuffer[2] = data;
I2CEngine();
}
void whoAmI(void)
{
uint16_t i;
setSendDataEnable(0);
while(1)
{
myPrintfUSB("//////////////// \n");
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = AK8963_W;
I2CMasterBuffer[1] = 0x00; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = AK8963_R;
I2CEngine();
myPrintfUSB("Who Am I AK8963 : %d \n", I2CSlaveBuffer[0]);
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
wait1msec(1000);
myPrintfUSB("//////////////// \n");
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x75; //Address 3byte (adjust MagXYZ)
I2CMasterBuffer[2] = MPU9250_R;
I2CEngine();
myPrintfUSB("Who Am I MPU9250 : %d \n", I2CSlaveBuffer[0]);
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
wait1msec(1000);
}
}
//====================
// RTC Read Register
//====================
uint32_t RTC_Read_Reg(uint32_t addr)
{
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = RTC_DEV_ADDR;
I2CMasterBuffer[1] = addr;
I2CMasterBuffer[2] = RTC_DEV_ADDR | RD_BIT ;
I2CEngine();
//
return I2CSlaveBuffer[0];
}
/*===============================================================================
Function : L3G4200D_ReadReg();
Parameters : Addr: 1 byte address from where data will be read
Length: Length of data to be read
Description : Reads n number of bytes from L3G4200D module
Preconditions : uncomment GYROSCOPE definition in Hardwareprofile.h
===============================================================================*/
uint32_t L3G4200D_ReadReg(uint8_t Addr, uint8_t Length)
{
uint32_t Status;
I2CWriteLength[L3G4200D_PORT] = 2;
I2CReadLength[L3G4200D_PORT] = Length;
I2CMasterBuffer[L3G4200D_PORT][0] = L3G4200D_ADDR | L3G4200D_WRITE;
I2CMasterBuffer[L3G4200D_PORT][1] = Addr; /* address */
I2CMasterBuffer[L3G4200D_PORT][2] = L3G4200D_ADDR | L3G4200D_READ;
Status = I2CEngine( L3G4200D_PORT );
return (Status);
}
void data_send(char *buffer, int len) // low level routine
{
int i;
// form up and send the request
I2CWriteLength[0] = len;
I2CReadLength[0] = 0;
for (i=0; i<len; i++) I2CMasterBuffer[0][i] = buffer[i];
I2CEngine( 0 );
}
/*===============================================================================
Function : LSM303DLHC_Magnetometer_ReadReg();
Parameters : Addr: 1 byte address from where data will be read
Length: Length of data to be read
Description : Reads n number of bytes from LSM303DLHC magnetometer module
Preconditions : uncomment MAGNETOMETER definition in Hardwareprofile.h
===============================================================================*/
uint32_t LSM303DLHC_Magnetometer_ReadReg(uint8_t Addr, uint8_t Length)
{
uint32_t Status;
I2CWriteLength[LSM303DLHC_PORT] = 2;
I2CReadLength[LSM303DLHC_PORT] = Length;
I2CMasterBuffer[LSM303DLHC_PORT][0] = LSM303DLHC_MAG_ADDR | LSM303DLHC_WRITE;
I2CMasterBuffer[LSM303DLHC_PORT][1] = Addr; /* address */
I2CMasterBuffer[LSM303DLHC_PORT][2] = LSM303DLHC_MAG_ADDR | LSM303DLHC_READ;
Status = I2CEngine( LSM303DLHC_PORT );
return (Status);
}
/*===============================================================================
Function : L3G4200D_WriteReg();
Parameters : Addr: 1 byte address where data will be written
Data: Data to be written
Description : Sends 1 byte of data to L3G4200D module
Preconditions : uncomment GYROSCOPE definition in Hardwareprofile.h
===============================================================================*/
uint32_t L3G4200D_WriteReg(uint8_t Addr, uint8_t Data)
{
uint32_t Status;
I2CWriteLength[L3G4200D_PORT] = 3;
I2CReadLength[L3G4200D_PORT] = 0;
I2CMasterBuffer[L3G4200D_PORT][0] = L3G4200D_ADDR | L3G4200D_WRITE;
I2CMasterBuffer[L3G4200D_PORT][1] = Addr; /* address */
I2CMasterBuffer[L3G4200D_PORT][2] = Data; /* Data 0 */
Status = I2CEngine( L3G4200D_PORT );
return (Status);
}
/*===============================================================================
Function : LSM303DLHC_Magnetometer_WriteReg;
Parameters : Addr: 1 byte address where data will be written
Data: Data to be written
Description : Sends 1 byte of data to LSM303DLHC magnetometer module
Preconditions : uncomment MAGNETOMETER definition in Hardwareprofile.h
===============================================================================*/
uint32_t LSM303DLHC_Magnetometer_WriteReg(uint8_t Addr, uint8_t Data)
{
uint32_t Status;
I2CWriteLength[LSM303DLHC_PORT] = 3;
I2CReadLength[LSM303DLHC_PORT] = 0;
I2CMasterBuffer[LSM303DLHC_PORT][0] = LSM303DLHC_MAG_ADDR | LSM303DLHC_WRITE;
I2CMasterBuffer[LSM303DLHC_PORT][1] = Addr; /* address */
I2CMasterBuffer[LSM303DLHC_PORT][2] = Data; /* Data 0 */
Status = I2CEngine( LSM303DLHC_PORT );
return (Status);
}
uint32_t eeprom_set(uint16_t addr, uint8_t val)
{
i2c_clearbuffers();
I2CWriteLength = 4;
I2CReadLength = 0;
I2CMasterBuffer[0] = EEPROM_ADDR; /* EEPROM address */
I2CMasterBuffer[1] = (addr & 0xF0); /* key */
I2CMasterBuffer[2] = (addr & 0x0F); /* key */
I2CMasterBuffer[3] = val; /* value */
return I2CEngine();
}
/**
* @brief The main function for the charger.
* @details This function is entered automatically at reset. There is no exit
* from this function, it is declared as _int_ to avoid compiler warning.
*/
int main()
{
uint32_t i;
FastVoltage = RawVoltage = RawCurrent = 0;
PWM_Stop(); // Disable PWM output (set it low)
ADC_Init(); // Initialize the A/D converters
if (BUTTON1_PRESSED) // If button pressed at reset, calibrate only
State = CALIBRATE;
else // else wait for the START button
State = WAIT4BUTTON;
//
// Wait at least 100 ms for the LCD to wake up
// Assume that the loop takes 4 clocks, wait 125 ms
//
for (i = 0; i < (SystemCoreClock / 32); i++) {
}
LCD_Init();
//
// Set up the System Tick
//
FLAG1_PORT->DIR |= FLAG1_Msk;
FLAG1_PORT->DATA &= ~FLAG1_Msk;
Ticks = 0;
SysTick_Config((SystemCoreClock / TICKS_PER_SEC) - 1);
//
// Set up the I2C interface for the temperature sensor
//
I2CInit((uint32_t) I2CMASTER);
for (;;) {
if (0 == Ticks) {
for (i = 0; i < BUFSIZE; i++) {
I2CSlaveBuffer[i] = 0x00;
}
I2CWriteLength = 0;
I2CReadLength = 2;
I2CMasterBuffer[0] = LM75_ADDR | RD_BIT;
I2CEngine();
Temperature = I2CSlaveBuffer[0];
// Wait until Ticks becomes non-zero to read the sensor again
while (0 == Ticks) {
}
}
}
}
/******************************************************************************
** Function name: I2C_Write
**
** Description: Saves a word to EEPROM
**
** Parameters: 1. Address to save to
** 2. Data to save
** Returned value: None
**
******************************************************************************/
void I2C_Write (uint16_t _EEadd, uint8_t data0, uint8_t data1, uint8_t data2, uint8_t data3)
{
I2CWriteLength[PORT_USED] = 7;
I2CReadLength[PORT_USED] = 0;
I2CMasterBuffer[PORT_USED][0] = _24LC256_ADDR;
I2CMasterBuffer[PORT_USED][1] = 0x00; // address
I2CMasterBuffer[PORT_USED][2] = _EEadd; // address
I2CMasterBuffer[PORT_USED][3] = data0;
I2CMasterBuffer[PORT_USED][4] = data1;
I2CMasterBuffer[PORT_USED][5] = data2;
I2CMasterBuffer[PORT_USED][6] = data3;
I2CEngine( PORT_USED );
delayMs(1,2);
}
void data_request(char *buffer, uint8_t len)
{
int i;
// form up and send the request
I2CWriteLength[0] = 0;
I2CReadLength[0] = len;
// for (i=0; i<len; i++) I2CSlaveBuffer[0][i] = 0;
I2CMasterBuffer[0][0] = 0x5; // address 4 + read bit
I2CEngine( 0 );
for (i=0; i<len; i++) {
buffer[i] = I2CSlaveBuffer[0][i];
//I2CSlaveBuffer[0][i] = 0;
}
}
uint8_t eeprom_get(uint16_t addr)
{
i2c_clearbuffers();
I2CWriteLength = 3;
I2CReadLength = 1;
I2CMasterBuffer[0] = EEPROM_ADDR; /* EEPROM address */
I2CMasterBuffer[1] = (addr & 0xF0); /* key */
I2CMasterBuffer[2] = (addr & 0x0F); /* key */
I2CMasterBuffer[3] = EEPROM_ADDR | RD_BIT;
while (I2CEngine() == I2CSTATE_SLA_NACK);
return I2CSlaveBuffer[0];
}
/******************************************************************************
** Function name: I2C_Seq_Read
**
** Description: Reads a byte from EEPROM
**
** Parameters: 1. Address to read from
** 2. Byte length of read
** Returned value: None
**
******************************************************************************/
void I2C_Seq_Read (uint16_t _EEadd, int read_len)
{
int i;
for ( i = 0; i < BUFSIZE; i++ ) // clear buffer
{
I2CSlaveBuffer[PORT_USED][i] = 0;
}
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = read_len;
I2CMasterBuffer[PORT_USED][0] = _24LC256_ADDR;
I2CMasterBuffer[PORT_USED][1] = (_EEadd & 0x0f00) >> 8; // address
I2CMasterBuffer[PORT_USED][2] = _EEadd & 0x00ff; // address
I2CMasterBuffer[PORT_USED][3] = _24LC256_ADDR | RD_BIT;
I2CEngine( PORT_USED );
I2CStop(PORT_USED);
}
uint8_t MPU6050_whoami(){
uint8_t state;
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = MPU6050_ADDRESS;
I2CMasterBuffer[1] = MPU6050_RA_WHO_AM_I;
I2CMasterBuffer[2] = MPU6050_ADDRESS | RD_BIT;
state = I2CEngine();
if(state != I2C_OK) return state;
if(I2CSlaveBuffer[0] != 0x68) return 1;
return 0;
}
/******************************************************************************
** Function name: I2C_Read
**
** Description: Reads a byte from EEPROM
**
** Parameters: Address to read from
** Returned value: Data at address
**
******************************************************************************/
uint32_t I2C_Read (uint16_t _EEadd)
{
int i;
for ( i = 0; i < BUFSIZE; i++ ) // clear buffer
{
I2CMasterBuffer[PORT_USED][i] = 0;
}
I2CWriteLength[PORT_USED] = 3;
I2CReadLength[PORT_USED] = 1;
I2CMasterBuffer[PORT_USED][0] = _24LC256_ADDR;
I2CMasterBuffer[PORT_USED][1] = 0x00; // address
I2CMasterBuffer[PORT_USED][2] = _EEadd; // address
I2CMasterBuffer[PORT_USED][3] = _24LC256_ADDR | RD_BIT;
I2CEngine( PORT_USED );
I2CStop(PORT_USED);
return (uint32_t)I2CSlaveBuffer[PORT_USED][0];
}
uint8_t readRegister(uint8_t DeviceAddress, uint8_t regAddress) {
I2CWriteLength = 2;
I2CReadLength = 1;
I2CMasterBuffer[0] = DeviceAddress;
I2CMasterBuffer[1] = regAddress;
I2CMasterBuffer[2] = DeviceAddress | RD_BIT;
I2CEngine();
/* printf("I2CMasterBuffer[0]: %d ", I2CMasterBuffer[0]);
printf("I2CMasterBuffer[1]: %d ", I2CMasterBuffer[1]);
printf("I2CMasterBuffer[2]: %d ", I2CMasterBuffer[2]);
printf("I2CMasterBuffer[3]: %d ", I2CMasterBuffer[3]);
printf("I2CMasterBuffer[4]: %d ", I2CMasterBuffer[4]);
printf("I2CMasterBuffer[5]: %d \n", I2CMasterBuffer[5]);
printf("I2CSlaveBuffer[0]: %d ", I2CSlaveBuffer[0]);
printf("I2CSlaveBuffer[1]: %d ", I2CSlaveBuffer[1]);
printf("I2CSlaveBuffer[2]: %d ", I2CSlaveBuffer[2]);
printf("I2CSlaveBuffer[3]: %d ", I2CSlaveBuffer[3]);
printf("I2CSlaveBuffer[4]: %d ", I2CSlaveBuffer[4]);
printf("I2CSlaveBuffer[5]: %d \n", I2CSlaveBuffer[5]);*/
return I2CSlaveBuffer[0];
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
uint32_t i;
SystemInit();
if ( I2CInit( (uint32_t)I2CMASTER ) == FALSE ) /* initialize I2c */
{
while ( 1 ); /* Fatal error */
}
/* In order to start the I2CEngine, the all the parameters
must be set in advance, including I2CWriteLength, I2CReadLength,
I2CCmd, and the I2cMasterBuffer which contains the stream
command/data to the I2c slave device.
(1) If it's a I2C write only, the number of bytes to be written is
I2CWriteLength, I2CReadLength is zero, the content will be filled
in the I2CMasterBuffer.
(2) If it's a I2C read only, the number of bytes to be read is
I2CReadLength, I2CWriteLength is 0, the read value will be filled
in the I2CMasterBuffer.
(3) If it's a I2C Write/Read with repeated start, specify the
I2CWriteLength, fill the content of bytes to be written in
I2CMasterBuffer, specify the I2CReadLength, after the repeated
start and the device address with RD bit set, the content of the
reading will be filled in I2CMasterBuffer index at
I2CMasterBuffer[I2CWriteLength+2].
e.g. Start, DevAddr(W), WRByte1...WRByteN, Repeated-Start, DevAddr(R),
RDByte1...RDByteN Stop. The content of the reading will be filled
after (I2CWriteLength + two devaddr) bytes. */
/* Write SLA(W), address and one data byte */
I2CWriteLength = 6;
I2CReadLength = 0;
I2CMasterBuffer[0] = PCF8594_ADDR;
I2CMasterBuffer[1] = 0x00; /* address */
I2CMasterBuffer[2] = 0x55; /* Data0 */
I2CMasterBuffer[3] = 0xAA; /* Data1 */
I2CMasterBuffer[4] = 0x12; /* Data0 */
I2CMasterBuffer[5] = 0x34; /* Data1 */
I2CEngine();
/* Be careful with below fixed delay. From device to device, or
even same device with different write length, or various I2C clock,
below delay length may need to be changed accordingly. Having
a break point before Write/Read start will be helpful to isolate
the problem. */
for ( i = 0; i < 0x200000; i++ ); /* Delay after write */
for ( i = 0; i < BUFSIZE; i++ )
{
I2CSlaveBuffer[i] = 0x00;
}
/* Write SLA(W), address, SLA(R), and read one byte back. */
I2CWriteLength = 2;
I2CReadLength = 4;
I2CMasterBuffer[0] = PCF8594_ADDR;
I2CMasterBuffer[1] = 0x00; /* address */
I2CMasterBuffer[2] = PCF8594_ADDR | RD_BIT;
I2CEngine();
return 0;
}
/**
* MPU9250のデータ (AccXYZ + GyroXYZ + temperature) <br>
* dest_arrayに次のように格納 <br>
* byte <br>
* 0 accX 下位 <br>
* 1 accX 上位 <br>
* 2 accY 下位 <br>
* 3 accY 上位 <br>
* 4 accZ 下位 <br>
* 5 accZ 上位 <br>
* 6 temp 下位 <br>
* 7 temp 上位 <br>
* 8 gyroX 下位 <br>
* 9 gyroX 上位 <br>
* 10 gyroY 下位 <br>
* 11 gyroY 上位 <br>
* 12 gyroZ 下位 <br>
* 13 gyroZ 上位 <br>
* さらに, センサの読み取りデータを物理量に変換し, acc_vec[i] <br>
* omega_vec[i]に格納する.
*
* @param *dest_array データの格納先
* @return void
*/
void getDataFromMPU9250 (uint8_t *dest_array )
{
volatile uint32_t i;
volatile int32_t acc[3];
volatile uint16_t acc_temp[3]; //加速度の値計算の中間値
volatile int32_t omega[3];
volatile int32_t temp_; //温度
I2CWriteLength = 2;
I2CReadLength = 14;
I2CMasterBuffer[0] = MPU9250_W;
I2CMasterBuffer[1] = 0x3B; //Address 14byte (AccXYZ + GyroXYZ + temp)
I2CMasterBuffer[2] = MPU9250_R;
I2CEngine();
acc_temp[0] = (I2CSlaveBuffer[0]<<8) + I2CSlaveBuffer[1];
acc_temp[1] = (I2CSlaveBuffer[2]<<8) + I2CSlaveBuffer[3];
acc_temp[2] = (I2CSlaveBuffer[4]<<8) + I2CSlaveBuffer[5];
acc_temp[0] = (~acc_temp[0]) + 1;
acc_temp[1] = (~acc_temp[1]) + 1;
acc_temp[2] = (~acc_temp[2]) + 1;
//Acc
dest_array[0] = 0x00ff & acc_temp[0];
dest_array[1] = (0xff00 & acc_temp[0])>>8;
dest_array[2] = 0x00ff & acc_temp[1];
dest_array[3] = (0xff00 & acc_temp[1])>>8;
dest_array[4] = 0x00ff & acc_temp[2];
dest_array[5] = (0xff00 & acc_temp[2])>>8;
//temparature
dest_array[6] = I2CSlaveBuffer[7];
dest_array[7] = I2CSlaveBuffer[6];
//gyro
dest_array[8] = I2CSlaveBuffer[9];
dest_array[9] = I2CSlaveBuffer[8];
dest_array[10] = I2CSlaveBuffer[11];
dest_array[11] = I2CSlaveBuffer[10];
dest_array[12] = I2CSlaveBuffer[13];
dest_array[13] = I2CSlaveBuffer[12];
for ( i = 0; i < BUFSIZE; i++ ) //clear I2CSlaveBuffer
{
I2CSlaveBuffer[i] = 0x00;
}
//センサ値を物理量に変換
acc[0] = dest_array[0] + (dest_array[1]<<8);
if(acc[0] >= 32767) acc[0] -= 65536;
acc_vec[0] = ((float)acc[0])/2048.0;
acc[1] = dest_array[2] + (dest_array[3]<<8);
if(acc[1] >= 32767) acc[1] -= 65536;
acc_vec[1] = ((float)acc[1])/2048.0;
acc[2] = dest_array[4] + (dest_array[5]<<8);
if(acc[2] >= 32767) acc[2] -= 65536;
acc_vec[2] = ((float)acc[2])/2048.0;
omega[0] = dest_array[8] + (dest_array[9]<<8);
if(omega[0] >= 32767) omega[0] -= 65536;
omega_vec[0] = DEG2RAD( ((omega[0]) - (int32_t)(omega_ref_vec[0] + 0.5 ) )/16.4 );
omega[1] = dest_array[10] + (dest_array[11]<<8);
if(omega[1] >= 32767) omega[1] -= 65536;
omega_vec[1] = DEG2RAD( ((omega[1]) - (int32_t)(omega_ref_vec[1] + 0.5 ) )/16.4 );
omega[2] = dest_array[12] + (dest_array[13]<<8);
if(omega[2] >= 32767) omega[2] -= 65536;
omega_vec[2] = DEG2RAD( ((omega[2]) - (int32_t)(omega_ref_vec[2] + 0.5 ) )/16.4 );
temp_ = dest_array[6] + (dest_array[7]<<8);
if(temp_ >= 32767) temp_ -= 65536;
temperature = (float)temp_/333.87 + 21.0;
//加速度センサ値をLPFにかける
acc_LPF_vec[0] = gain_LPF * acc_LPF_vec[0] + (1.0-gain_LPF) * acc_vec[0];
acc_LPF_vec[1] = gain_LPF * acc_LPF_vec[1] + (1.0-gain_LPF) * acc_vec[1];
acc_LPF_vec[2] = gain_LPF * acc_LPF_vec[2] + (1.0-gain_LPF) * acc_vec[2];
//角速度をLPFにかける
omega_LPF_vec[0] = gain_LPF * omega_LPF_vec[0] + (1.0-gain_LPF) * omega_vec[0];
omega_LPF_vec[1] = gain_LPF * omega_LPF_vec[1] + (1.0-gain_LPF) * omega_vec[1];
omega_LPF_vec[2] = gain_LPF * omega_LPF_vec[2] + (1.0-gain_LPF) * omega_vec[2];
//加速度センサ値をLPFにかける
acc_str_LPF_vec[0] = gain_str_LPF * acc_LPF_vec[0] + (1.0-gain_str_LPF) * acc_vec[0];
acc_str_LPF_vec[1] = gain_str_LPF * acc_LPF_vec[1] + (1.0-gain_str_LPF) * acc_vec[1];
acc_str_LPF_vec[2] = gain_str_LPF * acc_LPF_vec[2] + (1.0-gain_str_LPF) * acc_vec[2];
//角速度をLPFにかける
omega_str_LPF_vec[0] = gain_str_LPF * omega_str_LPF_vec[0] + (1.0-gain_str_LPF) * omega_vec[0];
omega_str_LPF_vec[1] = gain_str_LPF * omega_str_LPF_vec[1] + (1.0-gain_str_LPF) * omega_vec[1];
omega_str_LPF_vec[2] = gain_str_LPF * omega_str_LPF_vec[2] + (1.0-gain_str_LPF) * omega_vec[2];
}
