mirror of https://github.com/ArduPilot/ardupilot
RangeFinder: add PulsedLight LRF
This commit is contained in:
Randy Mackay
parent
8fcbb7e59b
commit
e88251f7e1
|
|
@ -6,4 +6,5 @@
|
|||
#include "AP_RangeFinder_SharpGP2Y.h"
|
||||
#include "AP_RangeFinder_MaxsonarXL.h"
|
||||
#include "AP_RangeFinder_MaxsonarI2CXL.h"
|
||||
#include "AP_RangeFinder_PulsedLightLRF.h"
|
||||
#include "AP_RangeFinder_analog.h"
|
||||
|
|
|
|||
|
|
@ -0,0 +1,123 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* AP_RangeFinder_PulsedLightLRF.cpp - Arduino Library for Pulsed Light's Laser Range Finder
|
||||
* Code by Randy Mackay. DIYDrones.com
|
||||
*
|
||||
* Sensor should be connected to the I2C port
|
||||
*
|
||||
* Variables:
|
||||
* bool healthy : indicates whether last communication with sensor was successful
|
||||
*
|
||||
* Methods:
|
||||
* take_reading(): ask the sonar to take a new distance measurement
|
||||
* read() : read last distance measured (in cm)
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_RangeFinder_PulsedLightLRF.h"
|
||||
#include <AP_HAL.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(FilterInt16 *filter) :
|
||||
RangeFinder(NULL, filter),
|
||||
healthy(true),
|
||||
_addr(AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR)
|
||||
{
|
||||
min_distance = AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
// init - simply sets the i2c address
|
||||
void AP_RangeFinder_PulsedLightLRF::init(uint8_t address)
|
||||
{
|
||||
// set sensor i2c address
|
||||
_addr = address;
|
||||
}
|
||||
|
||||
// take_reading - ask sensor to make a range reading
|
||||
bool AP_RangeFinder_PulsedLightLRF::take_reading()
|
||||
{
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
|
||||
|
||||
// exit immediately if we can't take the semaphore
|
||||
if (i2c_sem == NULL || !i2c_sem->take(5)) {
|
||||
healthy = false;
|
||||
return healthy;
|
||||
}
|
||||
|
||||
// send command to take reading
|
||||
if (hal.i2c->writeRegister(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_COMMAND_REG, AP_RANGEFINDER_PULSEDLIGHTLRF_CMDREG_ACQUISITION) != 0) {
|
||||
healthy = false;
|
||||
}else{
|
||||
healthy = true;
|
||||
}
|
||||
|
||||
// return semaphore
|
||||
i2c_sem->give();
|
||||
|
||||
return healthy;
|
||||
}
|
||||
|
||||
// read - return last value measured by sensor
|
||||
int AP_RangeFinder_PulsedLightLRF::read()
|
||||
{
|
||||
uint8_t buff[2];
|
||||
int16_t ret_value = 0;
|
||||
|
||||
// get pointer to i2c bus semaphore
|
||||
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
|
||||
|
||||
// exit immediately if we can't take the semaphore
|
||||
if (i2c_sem == NULL || !i2c_sem->take(5)) {
|
||||
healthy = false;
|
||||
return healthy;
|
||||
}
|
||||
|
||||
// assume the worst
|
||||
healthy = false;
|
||||
|
||||
// read the high byte
|
||||
if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTHIGH_REG, 1, &buff[0]) == 0) {
|
||||
// read the low byte
|
||||
if (hal.i2c->readRegisters(_addr, AP_RANGEFINDER_PULSEDLIGHTLRF_DISTLOW_REG, 1, &buff[1]) == 0) {
|
||||
healthy = true;
|
||||
// combine results into distance
|
||||
ret_value = buff[0] << 8 | buff[1];
|
||||
}
|
||||
}
|
||||
|
||||
// ensure distance is within min and max
|
||||
ret_value = constrain_int16(ret_value, min_distance, max_distance);
|
||||
ret_value = _mode_filter->apply(ret_value);
|
||||
|
||||
// return semaphore
|
||||
i2c_sem->give();
|
||||
|
||||
// kick off another reading for next time
|
||||
// To-Do: replace this with continuous mode
|
||||
take_reading();
|
||||
|
||||
// to-do: do we really want to return 0 if reading the distance fails?
|
||||
return ret_value;
|
||||
}
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef __AP_RANGEFINDER_PULSEDLIGHTLRF_H__
|
||||
#define __AP_RANGEFINDER_PULSEDLIGHTLRF_H__
|
||||
|
||||
#include "RangeFinder.h"
|
||||
|
||||
/* 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) |
|
||||
* | |
|
||||
* | J6-1(NA) J6-2(NA) |
|
||||
* ------------------------------------------------------------------------------------
|
||||
*
|
||||
* 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
|
||||
*
|
||||
* APM2.x's I2C connector from outside edge: GND, Data, CLK, 3.3V
|
||||
*/
|
||||
|
||||
// i2c address
|
||||
#define AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR 0x70
|
||||
|
||||
// min and max distances
|
||||
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MIN_DISTANCE 0
|
||||
#define AP_RANGEFINDER_PULSEDLIGHTLRF_MAX_DISTANCE 2500
|
||||
|
||||
// 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
|
||||
|
||||
// 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
|
||||
|
||||
class AP_RangeFinder_PulsedLightLRF : public RangeFinder
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
// constructor
|
||||
AP_RangeFinder_PulsedLightLRF(FilterInt16 *filter);
|
||||
|
||||
// init - simply sets the i2c address
|
||||
void init(uint8_t address = AP_RANGEFINDER_PULSEDLIGHTLRF_ADDR);
|
||||
|
||||
// take_reading - ask sensor to make a range reading
|
||||
bool take_reading();
|
||||
|
||||
// read value from sensor and return distance in cm
|
||||
int read();
|
||||
|
||||
// heath
|
||||
bool healthy;
|
||||
|
||||
protected:
|
||||
uint8_t _addr;
|
||||
};
|
||||
#endif // __AP_RANGEFINDER_PULSEDLIGHTLRF_H__
|
||||
Loading…
Reference in New Issue