rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

Merged in mindnever/librepilot/LP-475_Fix_PIOS_MPU9250_Mag_Sensitivity (pull request #382)

Browse Source 
LP-475 Fix for PIOS_MPU9250_Mag_Sensitivity() function.
This commit is contained in:
Philippe Renon 2017-01-27 19:56:04 +00:00
commit aeddb98ea6
1 changed files with 21 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
flight/pios/common/pios_mpu9250.c
View File

@ -117,7 +117,7 @@ typedef union {
} data; } data;
} __attribute__((__packed__)) mpu9250_data_t; } __attribute__((__packed__)) mpu9250_data_t;
#define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l) #define GET_SENSOR_DATA(mpudataptr, sensor) ((int16_t)((mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l)))
static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0; static PIOS_SENSORS_3Axis_SensorsWithTemp *queue_data = 0;
static PIOS_SENSORS_3Axis_SensorsWithTemp *mag_data = 0; static PIOS_SENSORS_3Axis_SensorsWithTemp *mag_data = 0;
@ -651,8 +651,6 @@ static int32_t PIOS_MPU9250_Mag_SetReg(uint8_t reg, uint8_t data)
*/ */
static int32_t PIOS_MPU9250_Mag_Sensitivity(void) static int32_t PIOS_MPU9250_Mag_Sensitivity(void)
{ {
int i;
/* Put mag in power down state before changing mode */ /* Put mag in power down state before changing mode */
PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_POWER_DOWN_MODE); PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_POWER_DOWN_MODE);
PIOS_DELAY_WaitmS(1); PIOS_DELAY_WaitmS(1);
@ -661,33 +659,32 @@ static int32_t PIOS_MPU9250_Mag_Sensitivity(void)
PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_FUSE_ROM_MODE); PIOS_MPU9250_Mag_SetReg(PIOS_MPU9250_CNTL1, PIOS_MPU9250_MAG_FUSE_ROM_MODE);
PIOS_DELAY_WaitmS(1); PIOS_DELAY_WaitmS(1);
/* Set addres and read flag */
PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_ADDR, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG);
/* Set the address of the register to read. */
PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_REG, PIOS_MPU9250_ASAX);
/* Trigger the byte transfer. */
PIOS_MPU9250_SetReg(PIOS_MPU9250_I2C_SLV0_CTRL, PIOS_MPU9250_I2C_SLV_ENABLE | 0x3);
PIOS_DELAY_WaitmS(1);
if (PIOS_MPU9250_ClaimBus(false) != 0) { if (PIOS_MPU9250_ClaimBus(false) != 0) {
return -1; return -1;
} }
/* Set addres and read flag */
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_ADDR);
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_MAG_I2C_ADDR | PIOS_MPU9250_MAG_I2C_READ_FLAG);
/* Set the address of the register to read. */
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_REG);
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_ASAX);
/* Trigger the byte transfer. */
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV0_CTRL);
PIOS_SPI_TransferByte(dev->spi_id, PIOS_MPU9250_I2C_SLV_ENABLE | 0x3);
PIOS_DELAY_WaitmS(1);
/* Read the mag data from SPI block */ /* Read the mag data from SPI block */
for (i = 0; i < 0x3; i++) {
PIOS_SPI_TransferByte(dev->spi_id, (PIOS_MPU9250_EXT_SENS_DATA_00 | 0x80) + i); uint8_t mpu9250_send_buf[4] = { PIOS_MPU9250_EXT_SENS_DATA_00 | 0x80 };
int32_t ret = PIOS_SPI_TransferByte(dev->spi_id, 0x0);
if (ret < 0) { if (PIOS_SPI_TransferBlock(dev->spi_id, mpu9250_send_buf, mpu9250_send_buf, sizeof(mpu9250_send_buf), 0) == 0) {
PIOS_MPU9250_ReleaseBus(); for (int i = 0; i < 3; ++i) {
return -1; dev->mag_sens_adj[i] = 1.0f + ((float)((uint8_t)mpu9250_send_buf[i + 1] - 128)) / 256.0f;
} }
dev->mag_sens_adj[i] = 1.0f; // 1.0f + ((float)((uint8_t)ret - 128)) / 256.0f; } else {
PIOS_MPU9250_ReleaseBus();
return -1;
} }
PIOS_MPU9250_ReleaseBus(); PIOS_MPU9250_ReleaseBus();