2016-12-15 20:53:30 -04:00
|
|
|
/*
|
|
|
|
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/>.
|
|
|
|
*/
|
|
|
|
/*
|
2017-07-28 09:46:52 -03:00
|
|
|
driver for TeraRanger I2C rangefinders
|
2016-12-15 20:53:30 -04:00
|
|
|
*/
|
2017-07-28 09:46:52 -03:00
|
|
|
#include "AP_RangeFinder_TeraRangerI2C.h"
|
2016-12-15 20:53:30 -04:00
|
|
|
|
|
|
|
#include <utility>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2016-12-16 18:40:58 -04:00
|
|
|
#include <AP_Math/crc.h>
|
2018-10-11 20:35:04 -03:00
|
|
|
#include <AP_Common/Semaphore.h>
|
2016-12-15 20:53:30 -04:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
// registers
|
2017-07-28 09:46:52 -03:00
|
|
|
#define TR_MEASURE 0x00
|
|
|
|
#define TR_WHOAMI 0x01
|
|
|
|
#define TR_WHOAMI_VALUE 0xA1
|
2016-12-15 20:53:30 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
The constructor also initializes the rangefinder. Note that this
|
|
|
|
constructor is not called until detect() returns true, so we
|
|
|
|
already know that we should setup the rangefinder
|
|
|
|
*/
|
2017-07-28 09:20:10 -03:00
|
|
|
AP_RangeFinder_TeraRangerI2C::AP_RangeFinder_TeraRangerI2C(RangeFinder::RangeFinder_State &_state,
|
2019-02-03 22:21:58 -04:00
|
|
|
AP_RangeFinder_Params &_params,
|
2017-07-28 09:20:10 -03:00
|
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev)
|
2018-07-04 11:22:17 -03:00
|
|
|
: AP_RangeFinder_Backend(_state, _params)
|
2017-07-28 09:20:10 -03:00
|
|
|
, dev(std::move(i2c_dev))
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2017-07-28 09:46:52 -03:00
|
|
|
detect if a TeraRanger rangefinder is connected. We'll detect by
|
2016-12-15 20:53:30 -04:00
|
|
|
trying to take a reading on I2C. If we get a result the sensor is
|
|
|
|
there.
|
|
|
|
*/
|
2017-07-28 09:20:10 -03:00
|
|
|
AP_RangeFinder_Backend *AP_RangeFinder_TeraRangerI2C::detect(RangeFinder::RangeFinder_State &_state,
|
2018-07-04 11:22:17 -03:00
|
|
|
AP_RangeFinder_Params &_params,
|
2017-07-28 09:20:10 -03:00
|
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> i2c_dev)
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
2018-10-25 23:39:48 -03:00
|
|
|
if (!i2c_dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
2018-07-04 11:22:17 -03:00
|
|
|
AP_RangeFinder_TeraRangerI2C *sensor = new AP_RangeFinder_TeraRangerI2C(_state, _params, std::move(i2c_dev));
|
2016-12-15 20:53:30 -04:00
|
|
|
if (!sensor) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!sensor->init()) {
|
|
|
|
delete sensor;
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
initialise sensor
|
|
|
|
*/
|
2017-07-28 09:46:52 -03:00
|
|
|
bool AP_RangeFinder_TeraRangerI2C::init(void)
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
2017-02-18 00:36:12 -04:00
|
|
|
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
2016-12-15 20:53:30 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
dev->set_retries(10);
|
|
|
|
|
2016-12-16 18:40:58 -04:00
|
|
|
// check WHOAMI
|
|
|
|
uint8_t whoami;
|
2017-07-28 09:46:52 -03:00
|
|
|
if (!dev->read_registers(TR_WHOAMI, &whoami, 1) ||
|
|
|
|
whoami != TR_WHOAMI_VALUE) {
|
2018-08-03 11:42:02 -03:00
|
|
|
dev->get_semaphore()->give();
|
2016-12-16 18:40:58 -04:00
|
|
|
return false;
|
|
|
|
}
|
2018-08-03 11:41:15 -03:00
|
|
|
|
2016-12-15 20:53:30 -04:00
|
|
|
if (!measure()) {
|
|
|
|
dev->get_semaphore()->give();
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// give time for the sensor to process the request
|
|
|
|
hal.scheduler->delay(70);
|
|
|
|
|
2017-08-08 02:54:09 -03:00
|
|
|
uint16_t _distance_cm;
|
2018-08-03 11:41:15 -03:00
|
|
|
if (!collect_raw(_distance_cm)) {
|
2016-12-15 20:53:30 -04:00
|
|
|
dev->get_semaphore()->give();
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
dev->get_semaphore()->give();
|
|
|
|
|
|
|
|
dev->set_retries(1);
|
|
|
|
|
|
|
|
dev->register_periodic_callback(50000,
|
2017-07-28 09:46:52 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&AP_RangeFinder_TeraRangerI2C::timer, void));
|
2016-12-15 20:53:30 -04:00
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// measure() - ask sensor to make a range reading
|
2017-07-28 09:46:52 -03:00
|
|
|
bool AP_RangeFinder_TeraRangerI2C::measure()
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
2017-07-28 09:46:52 -03:00
|
|
|
uint8_t cmd = TR_MEASURE;
|
2016-12-15 20:53:30 -04:00
|
|
|
return dev->transfer(&cmd, 1, nullptr, 0);
|
|
|
|
}
|
|
|
|
|
2018-08-03 11:41:45 -03:00
|
|
|
// collect_raw() - return last value measured by sensor
|
2018-08-03 11:41:15 -03:00
|
|
|
bool AP_RangeFinder_TeraRangerI2C::collect_raw(uint16_t &raw_distance)
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
|
|
|
uint8_t d[3];
|
|
|
|
|
2018-08-03 11:41:15 -03:00
|
|
|
// Take range reading
|
2016-12-15 20:53:30 -04:00
|
|
|
if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2018-08-03 11:41:15 -03:00
|
|
|
// Check for CRC
|
2016-12-16 18:40:58 -04:00
|
|
|
if (d[2] != crc_crc8(d, 2)) {
|
|
|
|
return false;
|
2018-08-03 11:41:15 -03:00
|
|
|
} else {
|
2018-08-03 11:42:52 -03:00
|
|
|
raw_distance = ((uint16_t(d[0]) << 8) | d[1]);
|
|
|
|
return true;
|
2016-12-16 18:40:58 -04:00
|
|
|
}
|
2018-08-03 11:41:15 -03:00
|
|
|
}
|
2016-12-15 20:53:30 -04:00
|
|
|
|
2018-08-03 11:41:15 -03:00
|
|
|
// Checks for error code and if correct converts to cm
|
2018-08-03 11:42:20 -03:00
|
|
|
bool AP_RangeFinder_TeraRangerI2C::process_raw_measure(uint16_t raw_distance, uint16_t &output_distance_cm)
|
2018-08-03 11:41:15 -03:00
|
|
|
{
|
|
|
|
// Check for error codes
|
|
|
|
if (raw_distance == 0xFFFF) {
|
2018-08-03 11:42:52 -03:00
|
|
|
// Too far away is unreliable so we dont enforce max range here
|
2018-08-03 11:41:15 -03:00
|
|
|
return false;
|
2018-08-03 11:42:34 -03:00
|
|
|
} else if (raw_distance == 0x0000) {
|
2018-08-03 11:41:45 -03:00
|
|
|
// Too close
|
2018-07-04 11:22:17 -03:00
|
|
|
output_distance_cm = params.min_distance_cm;
|
2018-08-03 11:42:52 -03:00
|
|
|
return true;
|
2018-08-03 11:42:34 -03:00
|
|
|
} else if (raw_distance == 0x0001) {
|
2018-08-03 11:41:15 -03:00
|
|
|
// Unable to measure
|
|
|
|
return false;
|
2018-08-03 11:42:34 -03:00
|
|
|
} else {
|
|
|
|
output_distance_cm = raw_distance/10; // Conversion to centimeters
|
|
|
|
return true;
|
2018-08-03 11:41:15 -03:00
|
|
|
}
|
2016-12-15 20:53:30 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
timer called at 20Hz
|
|
|
|
*/
|
2017-07-28 09:46:52 -03:00
|
|
|
void AP_RangeFinder_TeraRangerI2C::timer(void)
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
2018-08-03 11:41:15 -03:00
|
|
|
// Take a reading
|
|
|
|
uint16_t _raw_distance = 0;
|
|
|
|
uint16_t _distance_cm = 0;
|
|
|
|
|
2018-10-11 20:35:04 -03:00
|
|
|
if (collect_raw(_raw_distance)) {
|
|
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
|
AP_RangeFinder: support last_reading_ms
Benewake, LeddarOne, LightWareSerial, MAVLink, MaxsonarI2CXL, MaxsonarSerialLV, NMEA, PX4_PWM, uLanding and Wasp already stored the last read time so for these drivers, this change just moves that storage to the state structure
analog, BBB_PRU, Bebop, LightWareI2C, PulsedLightLRF, TeraRangerI2C, VL53L0X did not store the last read time so this was added
2018-08-27 04:02:51 -03:00
|
|
|
if (process_raw_measure(_raw_distance, _distance_cm)){
|
2018-08-03 11:41:15 -03:00
|
|
|
accum.sum += _distance_cm;
|
|
|
|
accum.count++;
|
|
|
|
}
|
2016-12-15 20:53:30 -04:00
|
|
|
}
|
|
|
|
// and immediately ask for a new reading
|
|
|
|
measure();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update the state of the sensor
|
|
|
|
*/
|
2017-07-28 09:46:52 -03:00
|
|
|
void AP_RangeFinder_TeraRangerI2C::update(void)
|
2016-12-15 20:53:30 -04:00
|
|
|
{
|
2018-10-11 20:35:04 -03:00
|
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
|
|
|
|
if (accum.count > 0) {
|
|
|
|
state.distance_cm = accum.sum / accum.count;
|
|
|
|
state.last_reading_ms = AP_HAL::millis();
|
|
|
|
accum.sum = 0;
|
|
|
|
accum.count = 0;
|
|
|
|
update_status();
|
|
|
|
} else {
|
|
|
|
set_status(RangeFinder::RangeFinder_NoData);
|
2016-12-15 20:53:30 -04:00
|
|
|
}
|
|
|
|
}
|