AP_RangeFinder: fixed VL53L0X semaphore usage

libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp

@@ -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;
 }
 
 

libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.h

@@ -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