Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -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 8bf6e8e0cf
commit a8647f834e
2 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp
View File

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