This commit is contained in:
Chris Anderson 2012-01-03 22:11:22 -08:00
commit 7d1c7d706e
7 changed files with 49 additions and 9 deletions

View File

@ -108,7 +108,9 @@ static void init_ardupilot()
// //
#ifndef DESKTOP_BUILD #ifndef DESKTOP_BUILD
I2c.begin(); I2c.begin();
I2c.timeOut(20); I2c.timeOut(5);
// initially set a fast I2c speed, and drop it on first failures
I2c.setSpeed(true);
#endif #endif
SPI.begin(); SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

View File

@ -108,7 +108,9 @@ static void init_ardupilot()
// //
#ifndef DESKTOP_BUILD #ifndef DESKTOP_BUILD
I2c.begin(); I2c.begin();
I2c.timeOut(20); I2c.timeOut(5);
// initially set a fast I2c speed, and drop it on first failures
I2c.setSpeed(true);
#endif #endif
SPI.begin(); SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

View File

@ -152,7 +152,13 @@ void AP_Baro_BMP085::ReadPress()
{ {
uint8_t buf[3]; uint8_t buf[3];
if (!healthy && millis() < _retry_time) {
return;
}
if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
_retry_time = millis() + 1000;
I2c.setSpeed(false);
healthy = false; healthy = false;
return; return;
} }
@ -197,7 +203,13 @@ void AP_Baro_BMP085::ReadTemp()
{ {
uint8_t buf[2]; uint8_t buf[2];
if (!healthy && millis() < _retry_time) {
return;
}
if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
_retry_time = millis() + 1000;
I2c.setSpeed(false);
healthy = false; healthy = false;
return; return;
} }

View File

@ -50,6 +50,7 @@ class AP_Baro_BMP085 : public AP_Baro
uint8_t _temp_index; uint8_t _temp_index;
uint8_t _press_index; uint8_t _press_index;
uint32_t _retry_time;
void Command_ReadPress(); void Command_ReadPress();
void Command_ReadTemp(); void Command_ReadTemp();

View File

@ -246,11 +246,20 @@ AP_Compass_HMC5843::init()
// Read Sensor data // Read Sensor data
bool AP_Compass_HMC5843::read() bool AP_Compass_HMC5843::read()
{ {
if (!healthy && !re_initialise()) { if (!healthy) {
return false; if (millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = millis() + 1000;
return false;
}
} }
if (!read_raw()) { if (!read_raw()) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = millis() + 1000;
I2c.setSpeed(false);
return false; return false;
} }

View File

@ -53,6 +53,7 @@ class AP_Compass_HMC5843 : public Compass
virtual bool re_initialise(void); virtual bool re_initialise(void);
bool read_register(uint8_t address, uint8_t *value); bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, byte value); bool write_register(uint8_t address, byte value);
uint32_t _retry_time; // when unhealthy the millis() value to retry at
public: public:
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}

View File

@ -53,8 +53,6 @@ extern "C" {
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. # error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
#endif #endif
#define DF_LAST_PAGE 8192
// AT45DB321D Commands (from Datasheet) // AT45DB321D Commands (from Datasheet)
#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53 #define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53
#define DF_TRANSFER_PAGE_TO_BUFFER_2 0x55 #define DF_TRANSFER_PAGE_TO_BUFFER_2 0x55
@ -133,8 +131,23 @@ void DataFlash_APM2::Init(void)
// get page size: 512 or 528 (by default: 528) // get page size: 512 or 528 (by default: 528)
df_PageSize=PageSize(); df_PageSize=PageSize();
// the last page is reserved for config information ReadManufacturerID();
df_NumPages = DF_LAST_PAGE - 1;
// see page 22 of the spec for the density code
uint8_t density_code = (df_device >> 8) & 0x1F;
// note that we set df_NumPages to one lower than the highest, as
// the last page is reserved for a config page
if (density_code == 0x7) {
// 32 Mbit
df_NumPages = 8191;
} else if (density_code == 0x6) {
// 16 Mbit
df_NumPages = 4095;
} else {
// what is this??? card not inserted perhaps?
df_NumPages = 0;
}
} }
// This function is mainly to test the device // This function is mainly to test the device
@ -155,7 +168,7 @@ void DataFlash_APM2::ReadManufacturerID()
// This function return 1 if Card is inserted on SD slot // This function return 1 if Card is inserted on SD slot
bool DataFlash_APM2::CardInserted() bool DataFlash_APM2::CardInserted()
{ {
return (digitalRead(DF_CARDDETECT) == 0); return (df_NumPages >= 4095 && digitalRead(DF_CARDDETECT) == 0);
} }
// Read the status register // Read the status register