void read_mag()
{
//mag
I2C_start(I2C1, LSM303DL_M_ADDRESS, I2C_Direction_Transmitter);
I2C_write(I2C1,0x03); //LSM303_OUT_X_H_M 0x03
I2C_stop(I2C1);
I2C_start(I2C1, LSM303DL_M_ADDRESS, I2C_Direction_Receiver);
xhm = I2C_read_ack(I2C1);
xlm = I2C_read_ack(I2C1);
yhm = I2C_read_ack(I2C1);
ylm = I2C_read_ack(I2C1);
zhm = I2C_read_ack(I2C1);
zlm = I2C_read_nack(I2C1);
I2C_stop(I2C1);
char buffer_out[100];
mx=0,my=0,mz=0;
mx = (int16_t)(xhm << 8 | xlm);
sprintf(buffer_out,"mx%i\r\n",mx);
usb_cdc_printf(buffer_out);
my = (int16_t)(yhm << 8 | ylm);
buffer_out[0]='\0';
sprintf(buffer_out,"my%i\r\n",my);
usb_cdc_printf(buffer_out);
mz = (int16_t)(zhm << 8 | zlm);
buffer_out[0]='\0';
sprintf(buffer_out,"mz%i\r\n",mz);
usb_cdc_printf(buffer_out);
//mag
}
void read_gyro()
{
//gyro
I2C_start(I2C1, LSM303DL_G_ADDRESS, I2C_Direction_Transmitter);
I2C_write(I2C1, 0x28| (1 << 7) ); //L3G_OUT_X_L 0x28
I2C_stop(I2C1);
I2C_start(I2C1, LSM303DL_G_ADDRESS, I2C_Direction_Receiver);
xlg = I2C_read_ack(I2C1);
xhg = I2C_read_ack(I2C1);
ylg = I2C_read_ack(I2C1);
yhg = I2C_read_ack(I2C1);
zlg = I2C_read_ack(I2C1);
zhg = I2C_read_nack(I2C1);
I2C_stop(I2C1);
char buffer_out[100];
gx=0,gy=0,gz=0;
gx = (int16_t)(xhg << 8 | xlg);
sprintf(buffer_out,"gx%i\r\n",gx);
usb_cdc_printf(buffer_out);
gy = (int16_t)(yhg << 8 | ylg);
buffer_out[0]='\0';
sprintf(buffer_out,"gy%i\r\n",gy);
usb_cdc_printf(buffer_out);
gz = (int16_t)(zhg << 8 | zlg);
buffer_out[0]='\0';
sprintf(buffer_out,"gz%i\r\n",gz);
usb_cdc_printf(buffer_out);
//gyro
}
void read_acc()
{
//acc
I2C_start(I2C1, LSM303DL_A_ADDRESS, I2C_Direction_Transmitter);
I2C_write(I2C1,0x28| (1 << 7));
I2C_stop(I2C1);
I2C_start(I2C1, LSM303DL_A_ADDRESS, I2C_Direction_Receiver);
xla = I2C_read_ack(I2C1);
xha = I2C_read_ack(I2C1);
yla = I2C_read_ack(I2C1);
yha = I2C_read_ack(I2C1);
zla = I2C_read_ack(I2C1);
zha = I2C_read_nack(I2C1);
I2C_stop(I2C1); // stop the transmission
char buffer_out[100];
ax=0,ay=0,az=0;
ax = ((int16_t)(xha << 8 | xla)) >> 4;
sprintf(buffer_out,"ax%i\r\n",ax);
usb_cdc_printf(buffer_out);
ay = ((int16_t)(yha << 8 | yla)) >> 4;
buffer_out[0]='\0';
sprintf(buffer_out,"ay%i\r\n",ay);
usb_cdc_printf(buffer_out);
az = ((int16_t)(zha << 8 | zla)) >> 4;
buffer_out[0]='\0';
sprintf(buffer_out,"az%i\r\n",az);
usb_cdc_printf(buffer_out);
//acc
}
void AK8963_Init(void)
{
int i = 0;
Delay(500000);
// First extract the factory calibration for each magnetometer axis
uint8_t buffer[3]; // x/y/z gyro calibration data stored here
AK8963_WriteRegister(AK8963_CNTL, MODE0_POWER_DOWN); // Power down magnetometer
AK8963_WriteRegister(AK8963_CNTL, MODE6_FUSE_ROM_ACCESS); // Enter Fuse ROM access mode
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Transmitter);
I2C_write(I2C1, AK8963_ASAX);
I2C_stop(I2C1); // stop the transmission
// start a transmission in Master receiver mode
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Receiver);
// read one byte and request another byte
for(i = 0; i < 3-1; i++){
buffer[i] = I2C_read_ack(I2C1);
}
// read one byte and don't request another byte, stop transmission
buffer[3-1] = I2C_read_nack(I2C1);
mag_calibration[0] = (float)(buffer[0] - 128)/256.0 + 1.0; // Return x-axis sensitivity adjustment values, etc.
mag_calibration[1] = (float)(buffer[1] - 128)/256.0 + 1.0;
mag_calibration[2] = (float)(buffer[2] - 128)/256.0 + 1.0;
AK8963_WriteRegister(AK8963_CNTL, MODE0_POWER_DOWN); // Power down magnetometer
AK8963_WriteRegister(AK8963_CNTL, RESOLUTION_16_BIT << 4 | MODE3_CONTINUOUS_100_HZ); // Set magnetometer data resolution and sample ODR
Delay(500000);
}
/*----------------------------------------------------------------------------
//Reads I2C ADC
//Returns the 12-bit reading
*----------------------------------------------------------------------------*/
uint16_t I2C_ADC_Read(void){
uint16_t reading = 0x0000;
I2C_start(I2C3, ADC_ADDRESS, I2C_Direction_Receiver);//Open RX comms with the I2C ADC
reading |= I2C_read_ack(I2C3)<<8; // read first byte
reading |= I2C_read_nack(I2C3); //read second byte
I2C_stop(I2C3); // stop the transmission
return reading;
}
void I2C_Read_Multi_Reg(I2C_TypeDef* I2Cx, uint8_t Device, uint8_t Register, uint8_t* buf, uint8_t count)
{ uint8_t i = 0;
I2C_start(I2Cx, Device <<1, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
I2C_write(I2Cx, Register); // Select the register you want to start your read at.
I2C_stop(I2Cx); // stop the transmission
I2C_start(I2Cx, Device <<1, I2C_Direction_Receiver); // start a transmission in Master receiver mode
for(i =0; i < count; i++)
{
if (i < (count - 1))
{
buf[i] = I2C_read_ack(I2Cx); // Read one byte and send an ack to the slave.
}
else
{
buf[i] = I2C_read_nack(I2Cx); // Read the last register and send a Nack.
I2C_stop(I2Cx);
}
}
}
/*----------------------------------------------------------------------------
//Reads temperature registers from temp sensor
//Returns the temperature, in Celsius
*----------------------------------------------------------------------------*/
double I2C_Temp_Read(void){
double temperature;
uint16_t recievedData[2] = {0,0};
//Indicate we'd like to read the temperature register
I2C_start(I2C3, TEMP_ADDRESS, I2C_Direction_Transmitter);
I2C_write(I2C3, TEMP_READ_REG);
I2C_stop(I2C3);
//Then open in RX mode and get the data
I2C_start(I2C3, TEMP_ADDRESS, I2C_Direction_Receiver);
recievedData[0] = I2C_read_ack(I2C3); //read first byte
recievedData[1] = I2C_read_nack(I2C3); //read second byte
I2C_stop(I2C3); // stop the transmission
recievedData[1] |= recievedData[0]<<8;
temperature = pow(2,-8) * (double)recievedData[1];
return temperature;
}
void AK8963_ReadMag(IMU_DATA_t *mag)
{
int i = 0;
uint8_t buffer[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
uint16_t rmag[3];
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Transmitter);
I2C_write(I2C1, AK8963_ST1);
I2C_stop(I2C1); // stop the transmission
// start a transmission in Master receiver mode
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Receiver);
if(I2C_read_nack(I2C1) & 0x01){
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Transmitter);
I2C_write(I2C1, AK8963_XOUT_L);
I2C_stop(I2C1); // stop the transmission
I2C_start(I2C1, AK8963_ADDRESS<<1, I2C_Direction_Receiver);
for(i = 0; i < 7-1; i++){
buffer[i] = I2C_read_ack(I2C1);
}
// read one byte and don't request another byte, stop transmission
buffer[7-1] = I2C_read_nack(I2C1);
if(!(buffer[6] & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
// maybe change to 1, 0, 3, 2, 5, 4
rmag[0] = (((int16_t) buffer[0]) << 8) | buffer[1]; // X axis (internal sensor y axis)
rmag[1] = (((int16_t) buffer[2]) << 8) | buffer[3]; // Y axis (internal sensor x axis)
rmag[2] = (((int16_t) buffer[4]) << 8) | buffer[5]; // Z axis (internal sensor z axis)
// maybe it should look lik this:
// (float)buffer[0]*MAG_SCALE*mag_calibration[0] - MAG_X_OFFSET;
mag->Roll = (float)(rmag[0] - MAG_X_OFFSET) * MAG_SCALE * mag_calibration[0];
mag->Pitch = (float)(rmag[1] - MAG_Y_OFFSET) * MAG_SCALE * mag_calibration[1];
mag->Yaw = (float)(rmag[2] - MAG_Z_OFFSET) * MAG_SCALE * mag_calibration[2];
}
}
}
int main (void)
{
initI2C1 ();
initUsart ();
#if 0
/*
* +----------------+
* | Initialization |
* +----------------+
*/
accelgyro.initialize();
setClockSource(MPU6050_CLOCK_PLL_XGYRO/*0x01*/);
/*
* Excerpt from domcumentation : Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
* However, it is highly recommended that the device be configured to use one of the gyroscopes. Below is the code
* which does it:
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_CLKSEL_BIT/*2*/, MPU6050_PWR1_CLKSEL_LENGTH/*3*/, source/*MPU6050_CLOCK_PLL_XGYRO 0x01*/);
setFullScaleGyroRange(MPU6050_GYRO_FS_250/*0x00*/);
/*
* 0x1b register is used to trigger gyroscope self-test and configure the gyroscopes’ full scale range. Below
* we set ful scale to be +/- 250 units (seconds?)
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG/*0x1B*/, MPU6050_GCONFIG_FS_SEL_BIT/*4*/, MPU6050_GCONFIG_FS_SEL_LENGTH/*2*/, range/*0x00*/);
setFullScaleAccelRange(MPU6050_ACCEL_FS_2/*0x00*/);
/*
* Set accelerometer full scale to be +/- 2g.
*/
I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG/*0x1C*/, MPU6050_ACONFIG_AFS_SEL_BIT/*4*/, MPU6050_ACONFIG_AFS_SEL_LENGTH/*2*/, range/*0*/);
setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
/*
* By default MPU6050 is in sleep mode after powering up. Below we are waking it back on. This
* is done using the same register as in first line,
*/
I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_SLEEP_BIT/*6*/, enabled/*false*/);
accelgyro.testConnection()
getDeviceID() == 0x34;
/*
* This register is used to verify the identity of the device. The contents of WHO_AM_I are
* the upper 6 bits of the MPU-60X0’s 7-bit I C address. The Power-On-Reset value of Bit6:Bit1 is 0b110100 == 0x34.
*/
I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I/*0x75*/, MPU6050_WHO_AM_I_BIT/*6*/, MPU6050_WHO_AM_I_LENGTH/*6*/, buffer);
return buffer[0];
/*
* +----------------+
* | Main loop |
* +----------------+
*/
int16_t ax, ay, az;
int16_t gx, gy, gz;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/*
* In MPU-6000 and MPU-6050 Product Specification Rev 3.3 on pages 36 and 37 we read, that I²C reads and writes
* can be performed with single byte or multiple bytes. In single byte mode, we issue (after sending slave
* address ofcourse) a register address, and send or receive one byte of data. Multiple byte reads and writes, at the
* other hand consist of slave address, regiser address and multiple consecutive bytes od data. Slave puts or gets
* first byte from the register with the address we've just sent, and increases this addres by 1 after each byte.
*
* This is very useful in case of accelerometer and gyroscope because manufacturer has set up the apropriate registers
* cnsecutively, so one can read accel, internal temp and gyro data in one read command. Below is the code which does
* exactly this:
*/
I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H/*0x3B*/, 14, buffer);
*ax = (((int16_t)buffer[0]) << 8) | buffer[1];
*ay = (((int16_t)buffer[2]) << 8) | buffer[3];
*az = (((int16_t)buffer[4]) << 8) | buffer[5];
*gx = (((int16_t)buffer[8]) << 8) | buffer[9];
*gy = (((int16_t)buffer[10]) << 8) | buffer[11];
*gz = (((int16_t)buffer[12]) << 8) | buffer[13];
#endif
// Configuration:
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode
I2C_write_slow (I2C1, MPU6050_RA_PWR_MGMT_1); // Register address
I2C_write (I2C1, MPU6050_CLOCK_PLL_XGYRO); // Register value = 0x01. Which means, that DEVICE_RESET, SLEEP, CYCLE and TEMP_DIS are all 0.
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_GYRO_CONFIG);
I2C_write (I2C1, MPU6050_GYRO_FS_250); // All bits set to zero.
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_ACCEL_CONFIG);
I2C_write (I2C1, MPU6050_ACCEL_FS_2); // All bits set to zero.
I2C_stop (I2C1);
// Simple test if communication is working
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_WHO_AM_I);
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver);
uint8_t whoAmI = I2C_read_nack (I2C1); // read one byte and don't request another byte
I2C_stop (I2C1);
if (whoAmI == 0x34) {
usartSendString (USART1, "Accelerometer has been found!\r\n");
}
else {
usartSendString (USART1, "*NO* Accelerometer has been found!\r\n");
}
while (1) {
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter);
I2C_write (I2C1, MPU6050_RA_ACCEL_XOUT_H);
I2C_stop (I2C1);
I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver);
uint16_t ax = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t ay = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t az = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t temp = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gx = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gy = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1);
uint16_t gz = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_nack (I2C1);
I2C_stop (I2C1);
printf ("Accel : (%d, %d, %d), temperature : %d, gyro : (%d, %d, %d)\r\n", ax, ay, az, temp, gx, gy, gz);
}
}
