Browse Source

MPU6000: fixed some build warnings

master
Andrew Tridgell 13 years ago
parent
commit
0763bbd1af
  1. 30
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

30
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@ -370,13 +370,12 @@ void AP_InertialSensor_MPU6000::read(uint32_t ) @@ -370,13 +370,12 @@ void AP_InertialSensor_MPU6000::read(uint32_t )
uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg )
{
uint8_t dump;
uint8_t return_value;
uint8_t addr = reg | 0x80; // Set most significant bit
digitalWrite(_cs_pin, LOW);
dump = SPI.transfer(addr);
SPI.transfer(addr);
return_value = SPI.transfer(0);
digitalWrite(_cs_pin, HIGH);
@ -386,10 +385,9 @@ uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg ) @@ -386,10 +385,9 @@ uint8_t AP_InertialSensor_MPU6000::register_read( uint8_t reg )
void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
{
uint8_t dump;
digitalWrite(_cs_pin, LOW);
dump = SPI.transfer(reg);
dump = SPI.transfer(val);
SPI.transfer(reg);
SPI.transfer(val);
digitalWrite(_cs_pin, HIGH);
}
@ -551,14 +549,12 @@ void AP_InertialSensor_MPU6000::set_accel_offsets(int16_t offsetX, int16_t offse @@ -551,14 +549,12 @@ void AP_InertialSensor_MPU6000::set_accel_offsets(int16_t offsetX, int16_t offse
// at a time into the MPUREG_MEM_R_W register
void AP_InertialSensor_MPU6000::dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[])
{
uint8_t dump;
register_write(MPUREG_BANK_SEL,bank);
register_write(MPUREG_MEM_START_ADDR,address);
digitalWrite(_cs_pin, LOW);
dump = SPI.transfer(MPUREG_MEM_R_W);
for( uint8_t i=0; i<num_bytes; i++ ) {
dump = SPI.transfer(data[i]);
SPI.transfer(MPUREG_MEM_R_W);
for (uint8_t i=0; i<num_bytes; i++) {
SPI.transfer(data[i]);
}
digitalWrite(_cs_pin, HIGH);
}
@ -663,10 +659,9 @@ void AP_InertialSensor_MPU6000::FIFO_getPacket() @@ -663,10 +659,9 @@ void AP_InertialSensor_MPU6000::FIFO_getPacket()
{
uint8_t i;
int16_t q_long[4];
uint8_t dump;
uint8_t addr = MPUREG_FIFO_R_W | 0x80; // Set most significant bit to indicate a read
digitalWrite(_cs_pin, LOW); // enable the device
dump = SPI.transfer(addr); // send address we want to read from
SPI.transfer(addr); // send address we want to read from
for(i = 0; i < _fifoCountL; i++){
_received_packet[i] = SPI.transfer(0); // request value
}
@ -854,16 +849,15 @@ void AP_InertialSensor_MPU6000::dmp_set_fifo_rate(uint8_t rate) @@ -854,16 +849,15 @@ void AP_InertialSensor_MPU6000::dmp_set_fifo_rate(uint8_t rate)
// The official invensense name is inv_key_0_96 (??)
void AP_InertialSensor_MPU6000::dmp_set_sensor_fusion_accel_gain(uint8_t gain)
{
uint8_t dump;
//inv_key_0_96
register_write(MPUREG_BANK_SEL,0x00);
register_write(MPUREG_MEM_START_ADDR, 0x60);
digitalWrite(_cs_pin, LOW);
dump = SPI.transfer(MPUREG_MEM_R_W);
dump = SPI.transfer(0x00);
dump = SPI.transfer(gain); // Original : 0x80 To test: 0x40, 0x20 (too less)
dump = SPI.transfer(0x00);
dump = SPI.transfer(0x00);
SPI.transfer(MPUREG_MEM_R_W);
SPI.transfer(0x00);
SPI.transfer(gain); // Original : 0x80 To test: 0x40, 0x20 (too less)
SPI.transfer(0x00);
SPI.transfer(0x00);
digitalWrite(_cs_pin, HIGH);
}

Loading…
Cancel
Save