forked from Archive/PX4-Autopilot
mpu9250: improve AK8963 init (fixes raspberry pi support)
This commit is contained in:
parent
16bd1088fa
commit
5f5a2e12fb
|
@ -53,6 +53,10 @@
|
|||
|
||||
#define PX4_NUMBER_I2C_BUSES 2
|
||||
|
||||
// SPI
|
||||
#define PX4_SPI_BUS_SENSORS 0
|
||||
#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0)
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 0
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL 1
|
||||
|
||||
|
|
|
@ -158,32 +158,17 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
|
|||
}
|
||||
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_REG, reg);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_REG, reg);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
|
||||
{
|
||||
_parent->_interface->read(reg, val, count);
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
|
||||
{
|
||||
set_passthrough(reg, size);
|
||||
px4_usleep(25 + 25 * size); // wait for the value to be read from slave
|
||||
read_block(MPUREG_EXT_SENS_DATA_00, buf, size);
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
|
||||
}
|
||||
|
||||
uint8_t
|
||||
MPU9250_mag::read_reg(unsigned int reg)
|
||||
{
|
||||
uint8_t buf{};
|
||||
|
||||
if (_interface == nullptr) {
|
||||
passthrough_read(reg, &buf, 0x01);
|
||||
read_reg_through_mpu9250(reg, &buf);
|
||||
|
||||
} else {
|
||||
_interface->read(reg, &buf, 1);
|
||||
|
@ -200,23 +185,12 @@ MPU9250_mag::ak8963_check_id(uint8_t &deviceid)
|
|||
return (AK8963_DEVICE_ID == deviceid);
|
||||
}
|
||||
|
||||
/*
|
||||
* 400kHz I2C bus speed = 2.5us per bit = 25us per byte
|
||||
*/
|
||||
void
|
||||
MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
set_passthrough(reg, 1, &val);
|
||||
px4_usleep(50); // wait for the value to be written to slave
|
||||
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250_mag::write_reg(unsigned reg, uint8_t value)
|
||||
{
|
||||
// general register transfer at low clock speed
|
||||
if (_interface == nullptr) {
|
||||
passthrough_write(reg, value);
|
||||
write_reg_through_mpu9250(reg, value);
|
||||
|
||||
} else {
|
||||
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
|
||||
|
@ -246,17 +220,19 @@ MPU9250_mag::ak8963_read_adjustments()
|
|||
uint8_t response[3];
|
||||
float ak8963_ASA[3];
|
||||
|
||||
write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
|
||||
px4_usleep(50);
|
||||
write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
|
||||
px4_usleep(200);
|
||||
|
||||
if (_interface != nullptr) {
|
||||
_interface->read(AK8963REG_ASAX, response, 3);
|
||||
|
||||
} else {
|
||||
passthrough_read(AK8963REG_ASAX, response, 3);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
read_reg_through_mpu9250(AK8963REG_ASAX + i, response + i);
|
||||
}
|
||||
}
|
||||
|
||||
write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
|
||||
write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (0 != response[i] && 0xff != response[i]) {
|
||||
|
@ -291,6 +267,7 @@ MPU9250_mag::ak8963_setup_master_i2c()
|
|||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
MPU9250_mag::ak8963_setup()
|
||||
{
|
||||
|
@ -299,6 +276,7 @@ MPU9250_mag::ak8963_setup()
|
|||
do {
|
||||
ak8963_setup_master_i2c();
|
||||
write_reg(AK8963REG_CNTL2, AK8963_RESET);
|
||||
px4_usleep(100);
|
||||
|
||||
uint8_t id = 0;
|
||||
|
||||
|
@ -347,3 +325,107 @@ MPU9250_mag::ak8963_setup()
|
|||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void MPU9250_mag::write_imu_reg_verified(int reg, uint8_t val, uint8_t mask)
|
||||
{
|
||||
uint8_t b;
|
||||
int retry = 5;
|
||||
|
||||
while (retry) { // should not reach any retries in normal condition
|
||||
--retry;
|
||||
_parent->write_reg(reg, val);
|
||||
|
||||
b = _parent->read_reg(reg);
|
||||
|
||||
if ((b & mask) != val) {
|
||||
PX4_DEBUG("MPU9250_mag::write_imu_reg_verified failed. retrying...");
|
||||
continue;
|
||||
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MPU9250_mag::read_reg_through_mpu9250(uint8_t reg, uint8_t *val)
|
||||
{
|
||||
uint8_t b = 0;
|
||||
|
||||
// Read operation on the mag using the slave 4 registers.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR,
|
||||
AK8963_I2C_ADDR | BIT_I2C_READ_FLAG, 0xff);
|
||||
|
||||
// Set the mag register to read from.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
|
||||
|
||||
// Read the existing value of the SLV4 control register.
|
||||
b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
|
||||
|
||||
// Set the I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
|
||||
// bits. Enable data transfer, a read transfer as configured above.
|
||||
b |= 0x80;
|
||||
// Trigger the data transfer
|
||||
_parent->write_reg(MPUREG_I2C_SLV4_CTRL, b);
|
||||
|
||||
// Continuously check I2C_MST_STATUS register value for the completion
|
||||
// of I2C transfer until timeout
|
||||
|
||||
int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion
|
||||
|
||||
do {
|
||||
px4_usleep(1000);
|
||||
b = _parent->read_reg(MPUREG_I2C_MST_STATUS);
|
||||
} while (((b & 0x40) == 0x00) && (--loop_ctrl));
|
||||
|
||||
if (loop_ctrl == 0) {
|
||||
PX4_ERR("I2C transfer timed out");
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("mpu9250 SPI2IIC read delay: %dms", loop_ctrl);
|
||||
}
|
||||
|
||||
// Read the value received from the mag, and copy to the caller's out parameter.
|
||||
*val = _parent->read_reg(MPUREG_I2C_SLV4_DI);
|
||||
}
|
||||
|
||||
void MPU9250_mag::write_reg_through_mpu9250(uint8_t reg, uint8_t val)
|
||||
{
|
||||
uint8_t b = 0;
|
||||
|
||||
// Configure a write operation to the mag using Slave 4.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR,
|
||||
AK8963_I2C_ADDR, 0xff);
|
||||
|
||||
// Set the mag register address to write to using Slave 4.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff);
|
||||
|
||||
// Set the value to write in the I2C_SLV4_DO register.
|
||||
write_imu_reg_verified(MPUREG_I2C_SLV4_DO, val, 0xff);
|
||||
|
||||
// Read the current value of the Slave 4 control register.
|
||||
b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL);
|
||||
|
||||
// Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other
|
||||
// bits.
|
||||
b |= 0x80;
|
||||
// Trigger the data transfer from the byte now stored in the SLV4_DO register.
|
||||
_parent->write_reg(MPUREG_I2C_SLV4_CTRL, b);
|
||||
|
||||
// Continuously check I2C_MST_STATUS regsiter value for the completion
|
||||
// of I2C transfer until timeout.
|
||||
|
||||
int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion
|
||||
|
||||
do {
|
||||
px4_usleep(1000);
|
||||
b = _parent->read_reg(MPUREG_I2C_MST_STATUS);
|
||||
|
||||
} while (((b & 0x40) == 0x00) && (--loop_ctrl));
|
||||
|
||||
if (loop_ctrl == 0) {
|
||||
PX4_ERR("I2C transfer to mag timed out");
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("mpu9250 SPI2IIC write delay: %dms", loop_ctrl);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -115,10 +115,7 @@ public:
|
|||
MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation);
|
||||
~MPU9250_mag();
|
||||
|
||||
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL);
|
||||
void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size);
|
||||
void passthrough_write(uint8_t reg, uint8_t val);
|
||||
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
|
||||
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
|
||||
|
||||
int ak8963_reset();
|
||||
int ak8963_setup();
|
||||
|
@ -138,6 +135,9 @@ protected:
|
|||
|
||||
uint8_t read_reg(unsigned reg);
|
||||
void write_reg(unsigned reg, uint8_t value);
|
||||
void write_imu_reg_verified(int reg, uint8_t val, uint8_t mask);
|
||||
void read_reg_through_mpu9250(uint8_t reg, uint8_t *val);
|
||||
void write_reg_through_mpu9250(uint8_t reg, uint8_t val);
|
||||
|
||||
bool is_passthrough() { return _interface == nullptr; }
|
||||
|
||||
|
|
|
@ -398,17 +398,6 @@ MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16
|
|||
return _interface->read(MPU9250_SET_SPEED(start_reg, speed), buf, count);
|
||||
}
|
||||
|
||||
uint16_t
|
||||
MPU9250::read_reg16(unsigned reg)
|
||||
{
|
||||
uint8_t buf[2] {};
|
||||
|
||||
// general register transfer at low clock speed
|
||||
_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
|
||||
|
||||
return (uint16_t)(buf[0] << 8) | buf[1];
|
||||
}
|
||||
|
||||
void
|
||||
MPU9250::write_reg(unsigned reg, uint8_t value)
|
||||
{
|
||||
|
|
|
@ -316,7 +316,6 @@ private:
|
|||
* @return The value that was read.
|
||||
*/
|
||||
uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED);
|
||||
uint16_t read_reg16(unsigned reg);
|
||||
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue