mirror of https://github.com/ArduPilot/ardupilot
Wire and SPI Init: move to sketch system.pde from libraries
* Wire.begin removed from AP_Baro_BMP085::init() * SPI.begin removed from AP_Baro_MS5611::init() * SPI.begin removed from AP_InertialSensor_MPU6000::hardware_init() * Both Wire.begin and SPI.begin added very early in init_ardupilot in ArduCopter/system.pde and ArduPlane/system.pde
This commit is contained in:
parent
a7b9c8b9cd
commit
6710cf5c5a
|
@ -103,6 +103,12 @@ static void init_ardupilot()
|
||||||
"\n\nFree RAM: %u\n"),
|
"\n\nFree RAM: %u\n"),
|
||||||
memcheck_available_memory());
|
memcheck_available_memory());
|
||||||
|
|
||||||
|
//
|
||||||
|
// Initialize Wire and SPI libraries
|
||||||
|
//
|
||||||
|
Wire.begin();
|
||||||
|
SPI.begin();
|
||||||
|
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
|
||||||
//
|
//
|
||||||
// Initialize the isr_registry.
|
// Initialize the isr_registry.
|
||||||
//
|
//
|
||||||
|
|
|
@ -100,6 +100,12 @@ static void init_ardupilot()
|
||||||
"\n\nFree RAM: %u\n"),
|
"\n\nFree RAM: %u\n"),
|
||||||
memcheck_available_memory());
|
memcheck_available_memory());
|
||||||
|
|
||||||
|
//
|
||||||
|
// Initialize Wire and SPI libraries
|
||||||
|
//
|
||||||
|
Wire.begin();
|
||||||
|
SPI.begin();
|
||||||
|
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
|
||||||
//
|
//
|
||||||
// Initialize the ISR registry.
|
// Initialize the ISR registry.
|
||||||
//
|
//
|
||||||
|
|
|
@ -64,8 +64,6 @@ bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware)
|
||||||
|
|
||||||
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
|
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
|
||||||
|
|
||||||
if( initialiseWireLib != 0 )
|
|
||||||
Wire.begin();
|
|
||||||
|
|
||||||
oss = 3; // Over Sampling setting 3 = High resolution
|
oss = 3; // Over Sampling setting 3 = High resolution
|
||||||
BMP085_State = 0; // Initial state
|
BMP085_State = 0; // Initial state
|
||||||
|
|
|
@ -115,12 +115,6 @@ uint8_t AP_Baro_MS5611::MS5611_Ready()
|
||||||
// SPI should be initialized externally
|
// SPI should be initialized externally
|
||||||
void AP_Baro_MS5611::init()
|
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
|
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
||||||
digitalWrite(MS5611_CS, HIGH);
|
digitalWrite(MS5611_CS, HIGH);
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
|
@ -248,14 +248,6 @@ void AP_InertialSensor_MPU6000::register_write(uint8_t reg, uint8_t val)
|
||||||
|
|
||||||
void AP_InertialSensor_MPU6000::hardware_init()
|
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
|
// MPU6000 chip select setup
|
||||||
pinMode(_cs_pin, OUTPUT);
|
pinMode(_cs_pin, OUTPUT);
|
||||||
digitalWrite(_cs_pin, HIGH);
|
digitalWrite(_cs_pin, HIGH);
|
||||||
|
|
Loading…
Reference in New Issue