void MPU9250::magReadAdjustValues() {
magSetMode(AK8963_MODE_POWERDOWN);
magSetMode(AK8963_MODE_FUSEROM);
uint8_t buff[3];
I2Cread(MAG_ADDRESS, AK8963_RA_ASAX, 3, buff);
magXAdjust = buff[0];
magYAdjust = buff[1];
magZAdjust = buff[2];
}
void readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *b )
{
I2CbeginTransmission(devAddr);
I2Cwrite(regAddr);
I2CendTransmission();
msDelay(2);
I2CrequestFrom(devAddr, 1); // read a single byte
*b = (uint8_t)I2Cread();
}
// exhibit the status of the HMC5843
void test_HMC5843()
{
printf( "rega %u \n\r",I2Cread(0x03C,0));
printf( "regb %u \n\r",I2Cread(0x03C,1));
printf( "mode %u \n\r",I2Cread(0x03C,2));
printf( "stat %u \n\r",I2Cread(0x03C,9));
printf( "id %1c%1c%1c \n\r",I2Cread(0x03C,10),I2Cread(0x03C,11),I2Cread(0x03C,12));
microcontroller_delay_ms(100);
}
void readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
{
int i;
I2CbeginTransmission(devAddr);
I2Cwrite(regAddr);
msDelay(2);
I2CendTransmission();
msDelay(2);
I2CrequestFrom(devAddr, 14);
msDelay(5);
for (i = 0; i < length; i++)
{
data[i] = I2Cread();
}
}
// Read HMC5843 data registers
// This can be called at 100ms intervals to get new vector
void HMC5843(struct intvector *magdata)
{
// To clock out the new data, send:
// 0x3D, and clock out DXRA, DXRB, DYRA, DYRB, DZRA, DZRB located in registers 3 through 8.
// The HMC5843 will automatically re-point back to register 3 for the next 0x3D query,
// expected 100 milli-seconds or later
// note Mitch - automatic indexing is flakey - use direct register access and read all 6 bytes
magdata->x.b2.hbyte = I2Cread(0x03C, 3);
magdata->x.b2.lbyte = I2Cread(0x03C, 4);
magdata->y.b2.hbyte = I2Cread(0x03C, 5);
magdata->y.b2.lbyte = I2Cread(0x03C, 6);
magdata->z.b2.hbyte = I2Cread(0x03C, 7);
magdata->z.b2.lbyte = I2Cread(0x03C, 8);
}
/***
* read all 9 sensors from the IMU and convert values into metric units
* note: these values will be reported in the coordinate system of the sensor,
* which may be different for gyro, accelerometer, and magnetometer
*/
void Imu::read() {
// all measurements are converted to 16 bits by the IMU-internal ADC
double max16BitValue = 32767.0;
uint8_t Buf[14];
this->I2Cread(MPU9250_ADDRESS, 0x3B, 14, Buf);
/* Read accelerometer */
/* 16 bit accelerometer data */
int16_t ax = Buf[0] << 8 | Buf[1];
int16_t ay = Buf[2] << 8 | Buf[3];
int16_t az = Buf[4] << 8 | Buf[5];
/* scale to get metric data in m/s^2 */
// float maxAccRange = 2.0; // in g
double maxAccRange = 16.0; // max range (in g)
// as set in setup()
// function
double g2ms2 = 9.80665;
double accScale = g2ms2 * maxAccRange / max16BitValue; // convert 16 bit to
// float
/* convert 16 bit raw measurement to metric float */
accX = - double(ax) * accScale;
accY = double(ay) * accScale;
accZ = - double(az) * accScale;
/* Read gyroscope */
/* 16 bit gyroscope raw data */
int16_t gx = Buf[8] << 8 | Buf[9];
int16_t gy = Buf[10] << 8 | Buf[11];
int16_t gz = Buf[12] << 8 | Buf[13];
double maxGyrRange = 2000.0; // max range (in deg per sec)
// as set in setup() function
// float maxGyrRange = 500.0;
double gyrScale = maxGyrRange / max16BitValue; // convert 16 bit to float
/* convert 16 bit raw measurement to metric float */
gyrX = - double(gx) * gyrScale;
gyrY = double(gy) * gyrScale;
gyrZ = - double(gz) * gyrScale;
/* Read magnetometer */
uint8_t ST1;
I2Cread(MAG_ADDRESS, 0x02, 1, &ST1);
/* new measurement available (otherwise just move on) */
if (ST1 & 0x01) {
/* Read magnetometer data */
uint8_t m[6];
I2Cread(MAG_ADDRESS, 0x03, 6, m);
/***
* see datatsheet:
* - byte order is reverse from other sensors
* - x and y are flipped
* - z axis is reverse
*/
int16_t mmy = m[1] << 8 | m[0];
int16_t mmx = m[3] << 8 | m[2];
int16_t mmz = -m[5] << 8 | m[4];
/* convert 16 bit raw measurement to metric float */
double magScale = 4912.0 / max16BitValue;
this->magX = double(mmx) * magScale * this->_magnetometerAdjustmentScaleX;
this->magY = double(mmy) * magScale * this->_magnetometerAdjustmentScaleY;
this->magZ = double(mmz) * magScale * this->_magnetometerAdjustmentScaleZ;
/* request next reading on magnetometer */
I2CwriteByte(MAG_ADDRESS, 0x0A, B00010001);
}
}
void MPU9250::accelUpdate() {
I2Cread(address, 0x3B, 14, accelBuf);
}
void MPU9250::magUpdate() {
I2Cread(MAG_ADDRESS, AK8963_RA_HXL, 7, magBuf);
}
void I2CreceiveEvent(int l) {
#ifdef DEBUGG
Serial.println("[listening]");
#endif
I2Cread();
}
