diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 92779c0442..70836a025e 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -49,28 +49,25 @@ #include // ArduPilot Mega Vector/Matrix math Library #include -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE || defined(APM2_BETA_HARDWARE) #include "AP_Baro_BMP085.h" extern const AP_HAL::HAL& hal; #define BMP085_ADDRESS 0x77 //(0xEE >> 1) -#define BMP085_EOC 30 // End of conversion pin PC7 +#define BMP085_EOC 30 // End of conversion pin PC7 on APM1 // the apm2 hardware needs to check the state of the // chip using a direct IO port // On APM2 prerelease hw, the data ready port is hooked up to PE7, which // is not available to the arduino digitalRead function. -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 -#define BMP_DATA_READY() (PINE&0x80) -#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE -// No EOC connection from Baro on Flymaple +#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 +#define BMP_DATA_READY() hal.gpio->read(BMP085_EOC) +#else +// No EOC connection from Baro // Use times instead. // Temp conversion time is 4.5ms // Pressure conversion time is 25.5ms (for OVERSAMPLING=3) #define BMP_DATA_READY() (BMP085_State == 0 ? hal.scheduler->millis() > (_last_temp_read_command_time + 5) : hal.scheduler->millis() > (_last_press_read_command_time + 26)) -#else -#define BMP_DATA_READY() hal.gpio->read(BMP085_EOC) #endif // oversampling 3 gives 26ms conversion time. We then average @@ -280,4 +277,4 @@ void AP_Baro_BMP085::Calculate() _count /= 2; } } -#endif // CONFIG_HAL_BOARD + diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde index 186e5f031e..f89aee827f 100644 --- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde @@ -19,8 +19,10 @@ #include #include +#include +#include +#include -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 /* Build this example sketch only for the APM1. */ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; @@ -49,7 +51,7 @@ void loop() static uint32_t last_print; // accumulate values at 100Hz - if ((hal.scheduler->micros()- timer) > 20000L) { + if ((hal.scheduler->micros()- timer) > 10000L) { bmp085.accumulate(); timer = hal.scheduler->micros(); } @@ -78,13 +80,7 @@ void loop() (unsigned)bmp085.get_pressure_samples()); hal.console->println(); } + hal.scheduler->delay(1); } -#else // Non-APM1 -#warning AP_Baro_BMP085_test built as stub for APM2 -const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -void setup() {} -void loop() {} -#endif - AP_HAL_MAIN();