Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: fixed VL53L0X semaphore usage

Browse Source
This commit is contained in:
Andrew Tridgell 2018-07-27 08:15:55 +10:00
parent 06434a48c3
commit 3c95a74438
2 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

@ -239,17 +239,16 @@ AP_RangeFinder_Backend *AP_RangeFinder_VL53L0X::detect(RangeFinder::RangeFinder_
return nullptr;
}
if (sensor->dev->get_semaphore()->take(0)) {
if (!sensor->check_id()) {
sensor->dev->get_semaphore()->give();
delete sensor;
return nullptr;
}
sensor->dev->get_semaphore()->take_blocking();
if (!sensor->check_id() || !sensor->init()) {
sensor->dev->get_semaphore()->give();
delete sensor;
return nullptr;
}
sensor->init();
sensor->dev->get_semaphore()->give();
return sensor;
}
@ -556,7 +555,7 @@ bool AP_RangeFinder_VL53L0X::setMeasurementTimingBudget(uint32_t budget_us)
return true;
}
void AP_RangeFinder_VL53L0X::init()
bool AP_RangeFinder_VL53L0X::init()
{
// setup for 2.8V operation
write_register(VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV,
@ -584,8 +583,8 @@ void AP_RangeFinder_VL53L0X::init()
uint8_t spad_count;
bool spad_type_is_aperture;
if (!get_SPAD_info(&spad_count, &spad_type_is_aperture)) {
printf("Failed to get SPAD info\n");
return;
printf("VL53L0X: Failed to get SPAD info\n");
return false;
}
// The SPAD map (RefGoodSpadMap) is read by VL53L0X_get_info_from_device() in
@ -593,8 +592,8 @@ void AP_RangeFinder_VL53L0X::init()
// GLOBAL_CONFIG_SPAD_ENABLES_REF_0 through _6, so read it from there
uint8_t ref_spad_map[6];
if (!dev->read_registers(GLOBAL_CONFIG_SPAD_ENABLES_REF_0, ref_spad_map, 6)) {
printf("Failed to read SPAD map\n");
return;
printf("VL53L0X: Failed to read SPAD map\n");
return false;
}
// -- VL53L0X_set_reference_spads() begin (assume NVM values are valid)
@ -653,8 +652,8 @@ void AP_RangeFinder_VL53L0X::init()
write_register(SYSTEM_SEQUENCE_CONFIG, 0x01);
if (!performSingleRefCalibration(0x40)) {
printf("Failed SingleRefCalibration1\n");
return;
printf("VL53L0X: Failed SingleRefCalibration1\n");
return false;
}
// -- VL53L0X_perform_vhv_calibration() end
@ -663,8 +662,8 @@ void AP_RangeFinder_VL53L0X::init()
write_register(SYSTEM_SEQUENCE_CONFIG, 0x02);
if (!performSingleRefCalibration(0x00)) {
printf("Failed SingleRefCalibration2\n");
return;
printf("VL53L0X: Failed SingleRefCalibration2\n");
return false;
}
// -- VL53L0X_perform_phase_calibration() end
@ -677,6 +676,7 @@ void AP_RangeFinder_VL53L0X::init()
// call timer() every 33ms. We expect new data to be available every 33ms
dev->register_periodic_callback(33000,
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_VL53L0X::timer, void));
return true;
}

2
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h
View File

@ -24,7 +24,7 @@ private:
// constructor
AP_RangeFinder_VL53L0X(RangeFinder::RangeFinder_State &_state, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
void init();
bool init();
void timer();
// check sensor ID