diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index abe891eeb2..92b1e54f7d 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -103,6 +103,12 @@ static void init_ardupilot() "\n\nFree RAM: %u\n"), memcheck_available_memory()); + // + // Initialize Wire and SPI libraries + // + Wire.begin(); + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate // // Initialize the isr_registry. // diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4c00fa7760..aaae707ef8 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -100,6 +100,12 @@ static void init_ardupilot() "\n\nFree RAM: %u\n"), memcheck_available_memory()); + // + // Initialize Wire and SPI libraries + // + Wire.begin(); + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate // // Initialize the ISR registry. // diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 9c98050387..6eb84d274e 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -64,8 +64,6 @@ bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware) pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input - if( initialiseWireLib != 0 ) - Wire.begin(); oss = 3; // Over Sampling setting 3 = High resolution BMP085_State = 0; // Initial state diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 24aef779c7..89b47e417d 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -115,12 +115,6 @@ uint8_t AP_Baro_MS5611::MS5611_Ready() // SPI should be initialized externally void AP_Baro_MS5611::init() { - SPI.begin(); - #if F_CPU == 16000000 - SPI.setClockDivider(SPI_CLOCK_DIV16); - #else - # error MS5611 requires SPI at 1MHZ! Need appropriate SPI clock divider - #endif pinMode(MS5611_CS, OUTPUT); // Chip select Pin digitalWrite(MS5611_CS, HIGH); delay(1); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index ce4ed4d4ea..860a941f6d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -248,14 +248,6 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val) void AP_InertialSensor_MPU6000::hardware_init() { - // Need to initialize SPI if it hasn't already. - SPI.begin(); - #if F_CPU == 16000000 - SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1MHz - #else - # error MPU6000 requires SPI at 1MHZ! Need appropriate SPI clock divider. - #endif - // MPU6000 chip select setup pinMode(_cs_pin, OUTPUT); digitalWrite(_cs_pin, HIGH);