diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 224665b509..5ea7aa874e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -67,12 +67,14 @@ bool AP_RangeFinder_PulsedLightLRF::take_reading() } // send command to take reading - if (hal.i2c->writeRegister(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_COMMAND_REG, AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_ACQUISITION) != 0) { + if (hal.i2c->writeRegister(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG, AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE) != 0) { healthy = false; }else{ healthy = true; } + hal.scheduler->delay_microseconds(200); + // return semaphore i2c_sem->give(); @@ -99,6 +101,7 @@ int16_t AP_RangeFinder_PulsedLightLRF::read() // read the high byte if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) == 0) { + hal.scheduler->delay_microseconds(200); // read the low byte if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) == 0) { healthy = true; @@ -111,6 +114,8 @@ int16_t AP_RangeFinder_PulsedLightLRF::read() ret_value = constrain_int16(ret_value, min_distance, max_distance); ret_value = _mode_filter->apply(ret_value); + hal.scheduler->delay_microseconds(200); + // return semaphore i2c_sem->give(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h index cd39d152d1..999d73ed72 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h @@ -8,72 +8,35 @@ /* Connection diagram * * ------------------------------------------------------------------------------------ - * | J5-1(NA) J5-2(NA) J5-3(NA) J5-4(NA) J5-5(NA) | - * | J1-1(GND) J2-5(I2C Data) | - * | J1-2(5V) J2-4(I2C Clk) | - * | J1-3(Enable) J2-3(NA) | - * | J1-4(Ser RX) J2-2(Laser 5-20V) | - * | J1-5(Ser TX) (HV Bypass) J2-1(Boost 5V) | + * | J2-1(LED) J2-2(5V) J2-3(Enable) J2-4(Ref Clk) J2-5(GND) J2-6(GND) | * | | - * | J6-1(NA) J6-2(NA) | + * | | + * | J1-3(I2C Clk) J1-2(I2C Data) J1-1(GND) | * ------------------------------------------------------------------------------------ * * To connect to APM2.x: - * APM I2C Clock <-> J2-4 - * APM I2C Data <-> J2-5 - * APM GND (from output Rail) <-> J1-1 - * APM 5V (from output Rail fed by ESC or BEC) <-> J1-2 + * APM I2C Clock <-> J1-3 + * APM I2C Data <-> J1-2 + * APM GND (from output Rail) <-> J1-1 J2-5 + * APM 5V (from output Rail fed by ESC or BEC) <-> J2-2 * * APM2.x's I2C connector from outside edge: GND, Data, CLK, 3.3V */ // i2c address -#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x70 +#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x42 // min and max distances #define AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE 0 #define AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE 1400 // registers -#define AP_RANGEFINDER_PULSEDLIGHTLRF_COMMAND_REG 0x00 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MODESTATUS_REG 0x01 // mode & status register to turn on/off continuous measurements and averaging -#define AP_RANGEFINDER_PULSEDLIGHTLRF_SIGNALSTRENGTH_REG 0x05 // signal strenght -#define AP_RANGEFINDER_PULSEDLIGHTLRF_VELOCITY_REG 0x06 // velocity sensed -#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x07 // high byte of distance measurement -#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG 0x08 // low byte of distance measurement -#define AP_RANGEFINDER_PULSEDLIGHTLRF_PROCESSINGCNTRL_REG 0x09 // to set criteria for successful reads including min/max distances and signal strengths -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_REG 0x13 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CONTINUOUSRATE_REG 0x14 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AUTOINCREMENT 0x80 // To-Do: this does not work - we still need to read from each registry individually instead of reading from multiple contiguous registries all at once +#define AP_RANGEFINDER_PULSEDLIGHTLRF_MEASURE_REG 0x00 +#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG 0x0f // high byte of distance measurement +#define AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG 0x10 // low byte of distance measurement // command register values -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_ACQUISITION 0x01 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_READBLOCK 0x02 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_READWRITE_RAM 0x03 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_ERASEFLASH 0x04 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_POWERDOWN 0x06 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_READFLASH 0x07 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_SOFTRESET 0x09 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_STOREUSERCMD 0x0a - -// mode & status - valid values for mode/status register 0x01 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MODESTATUS_CONTINUOUS 0x20 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_MODESTATUS_AVERAGING 0x80 - -// averaging - valid values for averaging register 0x13 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_NONE 0 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_2 1 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_4 2 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_8 3 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_16 4 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_64 5 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_128 6 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_AVERAGING_256 8 - -// continuous rates - valid values for continuous rate register 0x14 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CONTINUOUSRATE_100_HZ 1 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CONTINUOUSRATE_10_HZ 10 -#define AP_RANGEFINDER_PULSEDLIGHTLRF_CONTINUOUSRATE_1_HZ 100 +#define AP_RANGEFINDER_PULSEDLIGHTLRF_MSRREG_ACQUIRE 0x61 class AP_RangeFinder_PulsedLightLRF : public RangeFinder {