Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: px4flow retries init 10 times at startup 

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;
#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
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)
{
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;
bool success = false;
uint8_t retry_attempt = 0;
while (!success && retry_attempt < PX4FLOW_INIT_RETRIES) {
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
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;
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;
retry_attempt++;
if (!success) {
hal.scheduler->delay(10);
}
}
return !!dev;
return success;
}
// setup the device