aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_OpticalFlow: px4flow retries init 10 times at startup

Browse Source 
This resolves an issue in which some px4flow sensors are slow to startup
This commit is contained in:
Randy Mackay 2017-08-22 13:49:44 +09:00
parent fdb4c7b5ee
commit bc38affcb1
1 changed files with 32 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp
View File

@ -27,7 +27,8 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define PX4FLOW_BASE_I2C_ADDR 0x42 #define PX4FLOW_BASE_I2C_ADDR 0x42
#define PX4FLOW_INIT_RETRIES 10 // attempt to initialise the sensor up to 10 times at startup
// constructor // constructor
AP_OpticalFlow_PX4Flow::AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend) : AP_OpticalFlow_PX4Flow::AP_OpticalFlow_PX4Flow(OpticalFlow &_frontend) :
@ -55,30 +56,39 @@ AP_OpticalFlow_PX4Flow *AP_OpticalFlow_PX4Flow::detect(OpticalFlow &_frontend)
*/ */
bool AP_OpticalFlow_PX4Flow::scan_buses(void) bool AP_OpticalFlow_PX4Flow::scan_buses(void)
{ {
for (uint8_t bus = 0; bus < 3; bus++) { bool success = false;
#ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS uint8_t retry_attempt = 0;
// only one bus from HAL
if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) { while (!success && retry_attempt < PX4FLOW_INIT_RETRIES) {
continue; for (uint8_t bus = 0; bus < 3; bus++) {
#ifdef HAL_OPTFLOW_PX4FLOW_I2C_BUS
// only one bus from HAL
if (bus != HAL_OPTFLOW_PX4FLOW_I2C_BUS) {
continue;
}
#endif
AP_HAL::OwnPtr<AP_HAL::Device> tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_address());
if (!tdev) {
continue;
}
if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
continue;
}
struct i2c_integral_frame frame;
success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
tdev->get_semaphore()->give();
if (success) {
printf("Found PX4Flow on bus %u\n", bus);
dev = std::move(tdev);
break;
}
} }
#endif retry_attempt++;
AP_HAL::OwnPtr<AP_HAL::Device> tdev = hal.i2c_mgr->get_device(bus, PX4FLOW_BASE_I2C_ADDR + get_address()); if (!success) {
if (!tdev) { hal.scheduler->delay(10);
continue;
}
if (!tdev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
continue;
}
struct i2c_integral_frame frame;
bool ok = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame));
tdev->get_semaphore()->give();
if (ok) {
printf("Found PX4Flow on bus %u\n", bus);
dev = std::move(tdev);
break;
} }
} }
return !!dev; return success;
} }
// setup the device // setup the device