AP_InertialSensor: MPU9150 fix compiler warnings

This commit is contained in:
mirkix 2015-04-09 12:46:25 +02:00 committed by Andrew Tridgell
parent 0279fdb0de
commit 13ee34c58a
2 changed files with 6 additions and 6 deletions

View File

@ -400,9 +400,9 @@ bool AP_InertialSensor_MPU9150::_init_sensor(void)
hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision");
goto failed;
}
uint8_t rev;
rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) |
(buff[1] & 0x01);
// uint8_t rev;
// rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) |
// (buff[1] & 0x01);
// Do not do the checking, for some reason the MPU-9150 returns 0
// if (rev) {
@ -954,7 +954,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_set_int_latched(uint8_t enable)
* @param[out] more Number of remaining packets.
* @return 0 if successful.
*/
int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp,
int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t *timestamp,
uint8_t *sensors, uint8_t *more)
{
/* Assumes maximum packet size is gyro (6) + accel (6). */
@ -984,7 +984,7 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel,
}
}
timestamp = hal.scheduler->millis();
*timestamp = hal.scheduler->millis();
// read the data
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
more[0] = fifo_count / packet_size - 1;

View File

@ -47,7 +47,7 @@ private:
int16_t mpu_reset_fifo(uint8_t sensors);
int16_t mpu_set_sensors(uint8_t sensors);
int16_t mpu_set_int_latched(uint8_t enable);
int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more);
int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t *timestamp, uint8_t *sensors, uint8_t *more);
void _set_accel_filter_frequency(uint8_t filter_hz);
void _set_gyro_filter_frequency(uint8_t filter_hz);