From 85da4955b6e7c061839bd15efbe49e28ac811ca7 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 10 Apr 2019 02:11:39 -0700 Subject: [PATCH] AP_Rangefinder: Enforce checking I2C status on VL53L1X --- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp | 353 ++++++++++-------- .../AP_RangeFinder/AP_RangeFinder_VL53L1X.h | 21 +- 2 files changed, 202 insertions(+), 172 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index d233c66b44..fdf35d0bc3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -74,8 +74,10 @@ AP_RangeFinder_Backend *AP_RangeFinder_VL53L1X::detect(RangeFinder::RangeFinder_ bool AP_RangeFinder_VL53L1X::check_id(void) { uint8_t v1, v2; - v1 = read_register(0x010F); - v2 = read_register(0x0110); + if (!(read_register(0x010F, v1) && + read_register(0x0110, v2))) { + return false; + } if ((v1 != 0xEA) || (v2 != 0xCC)) { @@ -90,61 +92,63 @@ bool AP_RangeFinder_VL53L1X::check_id(void) */ bool AP_RangeFinder_VL53L1X::init() { - // setup for 2.8V operation - write_register(PAD_I2C_HV__EXTSUP_CONFIG, - read_register(PAD_I2C_HV__EXTSUP_CONFIG) | 0x01); + uint8_t pad_i2c_hv_extsup_config = 0; + uint16_t mm_config_outer_offset_mm = 0; + if (!(// setup for 2.8V operation + read_register(PAD_I2C_HV__EXTSUP_CONFIG, pad_i2c_hv_extsup_config) && + write_register(PAD_I2C_HV__EXTSUP_CONFIG, + pad_i2c_hv_extsup_config | 0x01) && - // store oscillator info for later use - fast_osc_frequency = read_register16(OSC_MEASURED__FAST_OSC__FREQUENCY); - osc_calibrate_val = read_register16(RESULT__OSC_CALIBRATE_VAL); + // store oscillator info for later use + read_register16(OSC_MEASURED__FAST_OSC__FREQUENCY, fast_osc_frequency) && + read_register16(RESULT__OSC_CALIBRATE_VAL, osc_calibrate_val) && - // static config - write_register16(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate); // should already be this value after reset - write_register(GPIO__TIO_HV_STATUS, 0x02); - write_register(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8); // tuning parm default - write_register(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16); // tuning parm default - write_register(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01); - write_register(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF); - write_register(ALGO__RANGE_MIN_CLIP, 0); // tuning parm default - write_register(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2); // tuning parm default + // static config + write_register16(DSS_CONFIG__TARGET_TOTAL_RATE_MCPS, TargetRate) && // should already be this value after reset + write_register(GPIO__TIO_HV_STATUS, 0x02) && + write_register(SIGMA_ESTIMATOR__EFFECTIVE_PULSE_WIDTH_NS, 8) && // tuning parm default + write_register(SIGMA_ESTIMATOR__EFFECTIVE_AMBIENT_WIDTH_NS, 16) && // tuning parm default + write_register(ALGO__CROSSTALK_COMPENSATION_VALID_HEIGHT_MM, 0x01) && + write_register(ALGO__RANGE_IGNORE_VALID_HEIGHT_MM, 0xFF) && + write_register(ALGO__RANGE_MIN_CLIP, 0) && // tuning parm default + write_register(ALGO__CONSISTENCY_CHECK__TOLERANCE, 2) && // tuning parm default - // general config - write_register16(SYSTEM__THRESH_RATE_HIGH, 0x0000); - write_register16(SYSTEM__THRESH_RATE_LOW, 0x0000); - write_register(DSS_CONFIG__APERTURE_ATTENUATION, 0x38); + // general config + write_register16(SYSTEM__THRESH_RATE_HIGH, 0x0000) && + write_register16(SYSTEM__THRESH_RATE_LOW, 0x0000) && + write_register(DSS_CONFIG__APERTURE_ATTENUATION, 0x38) && - // timing config - write_register16(RANGE_CONFIG__SIGMA_THRESH, 360); // tuning parm default - write_register16(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192); // tuning parm default + // timing config + write_register16(RANGE_CONFIG__SIGMA_THRESH, 360) && // tuning parm default + write_register16(RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, 192) && // tuning parm default - // dynamic config - write_register(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01); - write_register(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01); - write_register(SD_CONFIG__QUANTIFIER, 2); // tuning parm default + // dynamic config + write_register(SYSTEM__GROUPED_PARAMETER_HOLD_0, 0x01) && + write_register(SYSTEM__GROUPED_PARAMETER_HOLD_1, 0x01) && + write_register(SD_CONFIG__QUANTIFIER, 2) && // tuning parm default - // from VL53L1_preset_mode_timed_ranging_* - // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1, - // and things don't seem to work if we don't set GPH back to 0 (which the API - // does here). - write_register(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00); - write_register(SYSTEM__SEED_CONFIG, 1); // tuning parm default + // from VL53L1_preset_mode_timed_ranging_* + // GPH is 0 after reset, but writing GPH0 and GPH1 above seem to set GPH to 1, + // and things don't seem to work if we don't set GPH back to 0 (which the API + // does here). + write_register(SYSTEM__GROUPED_PARAMETER_HOLD, 0x00) && + write_register(SYSTEM__SEED_CONFIG, 1) && // tuning parm default - // from VL53L1_config_low_power_auto_mode - write_register(SYSTEM__SEQUENCE_CONFIG, 0x8B); // VHV, PHASECAL, DSS1, RANGE - write_register16(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8); - write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2); // REQUESTED_EFFFECTIVE_SPADS - - setDistanceMode(Long); - setMeasurementTimingBudget(40000); - - // the API triggers this change in VL53L1_init_and_start_range() once a - // measurement is started; assumes MM1 and MM2 are disabled - write_register16(ALGO__PART_TO_PART_RANGE_OFFSET_MM, - read_register16(MM_CONFIG__OUTER_OFFSET_MM) * 4); - - // set continuous mode - startContinuous(50); - calibrated = false; + // from VL53L1_config_low_power_auto_mode + write_register(SYSTEM__SEQUENCE_CONFIG, 0x8B) && // VHV, PHASECAL, DSS1, RANGE + write_register16(DSS_CONFIG__MANUAL_EFFECTIVE_SPADS_SELECT, 200 << 8) && + write_register(DSS_CONFIG__ROI_MODE_CONTROL, 2) && // REQUESTED_EFFFECTIVE_SPADS + read_register16(MM_CONFIG__OUTER_OFFSET_MM, mm_config_outer_offset_mm) && + setDistanceMode(Long) && + setMeasurementTimingBudget(40000) && + // the API triggers this change in VL53L1_init_and_start_range() once a + // measurement is started; assumes MM1 and MM2 are disabled + write_register16(ALGO__PART_TO_PART_RANGE_OFFSET_MM, mm_config_outer_offset_mm * 4) && + // set continuous mode + startContinuous(50) + )) { + return false; + } // call timer() every 50ms. We expect new data to be available every 50ms dev->register_periodic_callback(50000, @@ -158,54 +162,63 @@ bool AP_RangeFinder_VL53L1X::init() bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode) { // save existing timing budget - uint32_t budget_us = getMeasurementTimingBudget(); + uint32_t budget_us = 0; + if (!getMeasurementTimingBudget(budget_us)) { + return false; + } switch (distance_mode) { case Short: // from VL53L1_preset_mode_standard_ranging_short_range() - // timing config - write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07); - write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05); - write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38); + if (!(// timing config + write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x07) && + write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x05) && + write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x38) && - // dynamic config - write_register(SD_CONFIG__WOI_SD0, 0x07); - write_register(SD_CONFIG__WOI_SD1, 0x05); - write_register(SD_CONFIG__INITIAL_PHASE_SD0, 6); // tuning parm default - write_register(SD_CONFIG__INITIAL_PHASE_SD1, 6); // tuning parm default + // dynamic config + write_register(SD_CONFIG__WOI_SD0, 0x07) && + write_register(SD_CONFIG__WOI_SD1, 0x05) && + write_register(SD_CONFIG__INITIAL_PHASE_SD0, 6) && // tuning parm default + write_register(SD_CONFIG__INITIAL_PHASE_SD1, 6))) { // tuning parm default + return false; + } break; case Medium: // from VL53L1_preset_mode_standard_ranging() - // timing config - write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B); - write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09); - write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78); + if (!(// timing config + write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0B) && + write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x09) && + write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0x78) && - // dynamic config - write_register(SD_CONFIG__WOI_SD0, 0x0B); - write_register(SD_CONFIG__WOI_SD1, 0x09); - write_register(SD_CONFIG__INITIAL_PHASE_SD0, 10); // tuning parm default - write_register(SD_CONFIG__INITIAL_PHASE_SD1, 10); // tuning parm default + // dynamic config + write_register(SD_CONFIG__WOI_SD0, 0x0B) && + write_register(SD_CONFIG__WOI_SD1, 0x09) && + write_register(SD_CONFIG__INITIAL_PHASE_SD0, 10) && // tuning parm default + write_register(SD_CONFIG__INITIAL_PHASE_SD1, 10))) { // tuning parm default + return false; + } break; case Long: // from VL53L1_preset_mode_standard_ranging_long_range() - // timing config - write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F); - write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D); - write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8); + if (!(// timing config + write_register(RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F) && + write_register(RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D) && + write_register(RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8) && - // dynamic config - write_register(SD_CONFIG__WOI_SD0, 0x0F); - write_register(SD_CONFIG__WOI_SD1, 0x0D); - write_register(SD_CONFIG__INITIAL_PHASE_SD0, 14); // tuning parm default - write_register(SD_CONFIG__INITIAL_PHASE_SD1, 14); // tuning parm default + // dynamic config + write_register(SD_CONFIG__WOI_SD0, 0x0F) && + write_register(SD_CONFIG__WOI_SD1, 0x0D) && + write_register(SD_CONFIG__INITIAL_PHASE_SD0, 14) && // tuning parm default + write_register(SD_CONFIG__INITIAL_PHASE_SD1, 14))) { // tuning parm default + return false; + } break; @@ -215,9 +228,7 @@ bool AP_RangeFinder_VL53L1X::setDistanceMode(DistanceMode distance_mode) } // reapply timing budget - setMeasurementTimingBudget(budget_us); - - return true; + return setMeasurementTimingBudget(budget_us); } // Set the measurement timing budget in microseconds, which is the time allowed @@ -239,10 +250,13 @@ bool AP_RangeFinder_VL53L1X::setMeasurementTimingBudget(uint32_t budget_us) // VL53L1_calc_timeout_register_values() begin - uint32_t macro_period_us; + uint8_t range_config_vcsel_period = 0; + if(!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period)) { + return false; + } // "Update Macro Period for Range A VCSEL Period" - macro_period_us = calcMacroPeriod(read_register(RANGE_CONFIG__VCSEL_PERIOD_A)); + uint32_t macro_period_us = calcMacroPeriod(range_config_vcsel_period); // "Update Phase timeout - uses Timing A" // Timeout of 1000 is tuning parm default (TIMED_PHASECAL_CONFIG_TIMEOUT_US_DEFAULT) @@ -251,67 +265,80 @@ bool AP_RangeFinder_VL53L1X::setMeasurementTimingBudget(uint32_t budget_us) if (phasecal_timeout_mclks > 0xFF) { phasecal_timeout_mclks = 0xFF; } - write_register(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks); - // "Update MM Timing A timeout" - // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT) - // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register - // actually ends up with a slightly different value because it gets assigned, - // retrieved, recalculated with a different macro period, and reassigned, - // but it probably doesn't matter because it seems like the MM ("mode - // mitigation"?) sequence steps are disabled in low power auto mode anyway. - write_register16(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( - timeoutMicrosecondsToMclks(1, macro_period_us))); + if (!( write_register(PHASECAL_CONFIG__TIMEOUT_MACROP, phasecal_timeout_mclks) && - // "Update Range Timing A timeout" - write_register16(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( - timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))); + // "Update MM Timing A timeout" + // Timeout of 1 is tuning parm default (LOWPOWERAUTO_MM_CONFIG_TIMEOUT_US_DEFAULT) + // via VL53L1_get_preset_mode_timing_cfg(). With the API, the register + // actually ends up with a slightly different value because it gets assigned, + // retrieved, recalculated with a different macro period, and reassigned, + // but it probably doesn't matter because it seems like the MM ("mode + // mitigation"?) sequence steps are disabled in low power auto mode anyway. + write_register16(MM_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( + timeoutMicrosecondsToMclks(1, macro_period_us))) && + + // "Update Range Timing A timeout" + write_register16(RANGE_CONFIG__TIMEOUT_MACROP_A, encodeTimeout( + timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))) && + + // "Update Macro Period for Range B VCSEL Period" + read_register(RANGE_CONFIG__VCSEL_PERIOD_B, range_config_vcsel_period) + )) { + return false; + } // "Update Macro Period for Range B VCSEL Period" - macro_period_us = calcMacroPeriod(read_register(RANGE_CONFIG__VCSEL_PERIOD_B)); + macro_period_us = calcMacroPeriod(range_config_vcsel_period); // "Update MM Timing B timeout" // (See earlier comment about MM Timing A timeout.) - write_register16(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( - timeoutMicrosecondsToMclks(1, macro_period_us))); + return write_register16(MM_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( + timeoutMicrosecondsToMclks(1, macro_period_us))) && - // "Update Range Timing B timeout" - write_register16(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( - timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))); - - // VL53L1_calc_timeout_register_values() end - - return true; + // "Update Range Timing B timeout" + write_register16(RANGE_CONFIG__TIMEOUT_MACROP_B, encodeTimeout( + timeoutMicrosecondsToMclks(range_config_timeout_us, macro_period_us))); } // Get the measurement timing budget in microseconds // based on VL53L1_SetMeasurementTimingBudgetMicroSeconds() -uint32_t AP_RangeFinder_VL53L1X::getMeasurementTimingBudget() +bool AP_RangeFinder_VL53L1X::getMeasurementTimingBudget(uint32_t &budget) { // assumes PresetMode is LOWPOWER_AUTONOMOUS and these sequence steps are // enabled: VHV, PHASECAL, DSS1, RANGE // "Update Macro Period for Range A VCSEL Period" - uint32_t macro_period_us = calcMacroPeriod(read_register(RANGE_CONFIG__VCSEL_PERIOD_A)); + uint8_t range_config_vcsel_period_a = 0; + if (!read_register(RANGE_CONFIG__VCSEL_PERIOD_A, range_config_vcsel_period_a)) { + return false; + } + + uint32_t macro_period_us = calcMacroPeriod(range_config_vcsel_period_a); + + uint16_t timeout_macrop_a = 0; + if (!read_register16(RANGE_CONFIG__TIMEOUT_MACROP_A, timeout_macrop_a)) { + return false; + } // "Get Range Timing A timeout" - uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout( - read_register16(RANGE_CONFIG__TIMEOUT_MACROP_A)), macro_period_us); + uint32_t range_config_timeout_us = timeoutMclksToMicroseconds(decodeTimeout(timeout_macrop_a), macro_period_us); - return 2 * range_config_timeout_us + TimingGuard; + budget = 2 * range_config_timeout_us + TimingGuard; + return true; } // Start continuous ranging measurements, with the given inter-measurement // period in milliseconds determining how often the sensor takes a measurement. -void AP_RangeFinder_VL53L1X::startContinuous(uint32_t period_ms) +bool AP_RangeFinder_VL53L1X::startContinuous(uint32_t period_ms) { // fix for actual measurement period shorter than set uint32_t adjusted_period_ms = period_ms + (period_ms * 64 / 1000); // from VL53L1_set_inter_measurement_period_ms() - write_register32(SYSTEM__INTERMEASUREMENT_PERIOD, adjusted_period_ms * osc_calibrate_val); - write_register(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range - write_register(SYSTEM__MODE_START, 0x40); // mode_range__timed + return write_register32(SYSTEM__INTERMEASUREMENT_PERIOD, adjusted_period_ms * osc_calibrate_val) && + write_register(SYSTEM__INTERRUPT_CLEAR, 0x01) && // sys_interrupt_clear_range + write_register(SYSTEM__MODE_START, 0x40); // mode_range__timed } // Decode sequence step timeout in MCLKs from register value @@ -382,31 +409,37 @@ uint32_t AP_RangeFinder_VL53L1X::calcMacroPeriod(uint8_t vcsel_period) // "Setup ranges after the first one in low power auto mode by turning off // FW calibration steps and programming static values" // based on VL53L1_low_power_auto_setup_manual_calibration() -void AP_RangeFinder_VL53L1X::setupManualCalibration(void) +bool AP_RangeFinder_VL53L1X::setupManualCalibration(void) { - uint8_t saved_vhv_init; - uint8_t saved_vhv_timeout; - // "save original vhv configs" - saved_vhv_init = read_register(VHV_CONFIG__INIT); - saved_vhv_timeout = read_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND); + uint8_t saved_vhv_init = 0; + uint8_t saved_vhv_timeout = 0; + uint8_t phasecal_result_vcsel_start = 0; - // "disable VHV init" - write_register(VHV_CONFIG__INIT, saved_vhv_init & 0x7F); + return // "save original vhv configs" + read_register(VHV_CONFIG__INIT, saved_vhv_init) && + read_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, saved_vhv_timeout) && - // "set loop bound to tuning param" - write_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, - (saved_vhv_timeout & 0x03) + (3 << 2)); // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT) + // "disable VHV init" + write_register(VHV_CONFIG__INIT, saved_vhv_init & 0x7F) && - // "override phasecal" - write_register(PHASECAL_CONFIG__OVERRIDE, 0x01); - write_register(CAL_CONFIG__VCSEL_START, read_register(PHASECAL_RESULT__VCSEL_START)); + // "set loop bound to tuning param" + write_register(VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, + (saved_vhv_timeout & 0x03) + (3 << 2)) && // tuning parm default (LOWPOWERAUTO_VHV_LOOP_BOUND_DEFAULT) + + // "override phasecal" + write_register(PHASECAL_CONFIG__OVERRIDE, 0x01) && + read_register(PHASECAL_RESULT__VCSEL_START, phasecal_result_vcsel_start) && + write_register(CAL_CONFIG__VCSEL_START, phasecal_result_vcsel_start); } // check if sensor has new reading available // assumes interrupt is active low (GPIO_HV_MUX__CTRL bit 4 is 1) bool AP_RangeFinder_VL53L1X::dataReady(void) { - return ((read_register(GPIO__TIO_HV_STATUS) & 0x01) == 0); + uint8_t gpio_tio_hv_status = 0; + + return read_register(GPIO__TIO_HV_STATUS, gpio_tio_hv_status) && + ((gpio_tio_hv_status & 0x01) == 0); } // read - return last value measured by sensor @@ -421,73 +454,71 @@ bool AP_RangeFinder_VL53L1X::get_reading(uint16_t &reading_mm) } } - const uint8_t range_status = read_register(RESULT__RANGE_STATUS); + uint8_t range_status = 0; + + if (!(read_register(RESULT__RANGE_STATUS, range_status) && + read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, reading_mm))) { + return false; + } - reading_mm = read_register16(RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0); // "apply correction gain" // gain factor of 2011 is tuning parm default (VL53L1_TUNINGPARM_LITE_RANGING_GAIN_FACTOR_DEFAULT) // Basically, this appears to scale the result by 2011/2048, or about 98% // (with the 1024 added for proper rounding). reading_mm = ((uint32_t)reading_mm * 2011 + 0x0400) / 0x0800; - if (!calibrated) - { - setupManualCalibration(); - calibrated = true; + if (!write_register(SYSTEM__INTERRUPT_CLEAR, 0x01)) { // sys_interrupt_clear_range + return false; } - write_register(SYSTEM__INTERRUPT_CLEAR, 0x01); // sys_interrupt_clear_range - switch ((DeviceError)range_status) { case RANGECOMPLETE: - return true; + break; default: #ifdef VL53L1X_DEBUG hal.console->printf("VL53L1X: %d ms status %d\n", AP_HAL::millis(), (int)range_status); #endif // VL53L1X_DEBUG return false; - } + + if (!calibrated) { + calibrated = setupManualCalibration(); + } + + return calibrated; } -uint8_t AP_RangeFinder_VL53L1X::read_register(uint16_t reg) +bool AP_RangeFinder_VL53L1X::read_register(uint16_t reg, uint8_t &value) { - uint8_t v = 0; uint8_t b[2] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF) }; - dev->transfer(b, 2, &v, 1); - return v; + return dev->transfer(b, 2, &value, 1); } -uint16_t AP_RangeFinder_VL53L1X::read_register16(uint16_t reg) +bool AP_RangeFinder_VL53L1X::read_register16(uint16_t reg, uint16_t & value) { uint16_t v = 0; uint8_t b[2] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF) }; - dev->transfer(b, 2, (uint8_t *)&v, 2); - return be16toh(v); + if (!dev->transfer(b, 2, (uint8_t *)&v, 2)) { + return false; + } + value = be16toh(v); + return true; } -uint32_t AP_RangeFinder_VL53L1X::read_register32(uint16_t reg) -{ - uint32_t v = 0; - uint8_t b[2] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF) }; - dev->transfer(b, 2, (uint8_t *)&v, 4); - return be32toh(v); -} - -void AP_RangeFinder_VL53L1X::write_register(uint16_t reg, uint8_t value) +bool AP_RangeFinder_VL53L1X::write_register(uint16_t reg, uint8_t value) { uint8_t b[3] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF), value }; - dev->transfer(b, 3, nullptr, 0); + return dev->transfer(b, 3, nullptr, 0); } -void AP_RangeFinder_VL53L1X::write_register16(uint16_t reg, uint16_t value) +bool AP_RangeFinder_VL53L1X::write_register16(uint16_t reg, uint16_t value) { uint8_t b[4] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF), uint8_t(value >> 8), uint8_t(value & 0xFF) }; - dev->transfer(b, 4, nullptr, 0); + return dev->transfer(b, 4, nullptr, 0); } -void AP_RangeFinder_VL53L1X::write_register32(uint16_t reg, uint32_t value) +bool AP_RangeFinder_VL53L1X::write_register32(uint16_t reg, uint32_t value) { uint8_t b[6] = { uint8_t(reg >> 8), uint8_t(reg & 0xFF), @@ -495,7 +526,7 @@ void AP_RangeFinder_VL53L1X::write_register32(uint16_t reg, uint32_t value) uint8_t((value >> 16) & 0xFF), uint8_t((value >> 8) & 0xFF), uint8_t((value) & 0xFF) }; - dev->transfer(b, 6, nullptr, 0); + return dev->transfer(b, 6, nullptr, 0); } /* diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h index be5e6b91a6..7db2967396 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.h @@ -1274,21 +1274,20 @@ private: uint32_t counter; bool calibrated; - uint8_t read_register(uint16_t reg); - uint16_t read_register16(uint16_t reg); - uint32_t read_register32(uint16_t reg); - void write_register(uint16_t reg, uint8_t value); - void write_register16(uint16_t reg, uint16_t value); - void write_register32(uint16_t reg, uint32_t value); + bool read_register(uint16_t reg, uint8_t &value) WARN_IF_UNUSED; + bool read_register16(uint16_t reg, uint16_t &value) WARN_IF_UNUSED; + bool write_register(uint16_t reg, uint8_t value) WARN_IF_UNUSED; + bool write_register16(uint16_t reg, uint16_t value) WARN_IF_UNUSED; + bool write_register32(uint16_t reg, uint32_t value) WARN_IF_UNUSED; bool dataReady(void); - bool setDistanceMode(DistanceMode distance_mode); - bool setMeasurementTimingBudget(uint32_t budget_us); - uint32_t getMeasurementTimingBudget(); - void startContinuous(uint32_t period_ms); + bool setDistanceMode(DistanceMode distance_mode) WARN_IF_UNUSED; + bool setMeasurementTimingBudget(uint32_t budget_us) WARN_IF_UNUSED; + bool getMeasurementTimingBudget(uint32_t &budget) WARN_IF_UNUSED; + bool startContinuous(uint32_t period_ms) WARN_IF_UNUSED; uint32_t decodeTimeout(uint16_t reg_val); uint16_t encodeTimeout(uint32_t timeout_mclks); uint32_t timeoutMclksToMicroseconds(uint32_t timeout_mclks, uint32_t macro_period_us); uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_us, uint32_t macro_period_us); uint32_t calcMacroPeriod(uint8_t vcsel_period); - void setupManualCalibration(void); + bool setupManualCalibration(void); };