44 #include <px4_platform_common/px4_config.h> 45 #include <px4_platform_common/defines.h> 46 #include <px4_platform_common/getopt.h> 47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 48 #include <px4_platform_common/module.h> 55 #define TERARANGER_BUS_DEFAULT PX4_I2C_BUS_EXPANSION 56 #define TERARANGER_DEVICE_PATH "/dev/teraranger" 57 #define TERARANGER_ONE_BASEADDR 0x30 // 7-bit address. 58 #define TERARANGER_EVO_BASEADDR 0x31 // 7-bit address. 61 #define TERARANGER_MEASURE_REG 0x00 // Measure range register. 62 #define TERARANGER_WHO_AM_I_REG 0x01 // Who am I test register. 63 #define TERARANGER_WHO_AM_I_REG_VAL 0xA1 66 #define TERARANGER_ONE_MAX_DISTANCE (14.00f) 67 #define TERARANGER_ONE_MIN_DISTANCE (0.20f) 69 #define TERARANGER_EVO_3M_MAX_DISTANCE (3.0f) 70 #define TERARANGER_EVO_3M_MIN_DISTANCE (0.10f) 72 #define TERARANGER_EVO_60M_MAX_DISTANCE (60.0f) 73 #define TERARANGER_EVO_60M_MIN_DISTANCE (0.50f) 75 #define TERARANGER_EVO_600HZ_MAX_DISTANCE (8.0f) 76 #define TERARANGER_EVO_600HZ_MIN_DISTANCE (0.75f) 78 #define TERARANGER_MEASUREMENT_INTERVAL 50_us 80 class TERARANGER :
public device::I2C,
public px4::ScheduledWorkItem
84 const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
89 virtual int init()
override;
111 virtual int probe()
override;
132 int probe_address(
const uint8_t address);
140 bool _collect_phase{
false};
142 int _orb_class_instance{-1};
144 uint8_t _orientation{0};
146 float _max_distance{0};
147 float _min_distance{0};
156 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
157 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
158 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
159 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
160 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
161 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
162 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
163 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
164 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
165 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
166 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
167 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
168 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
169 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
170 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
171 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
172 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
173 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
174 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
175 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
176 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
177 0xfa, 0xfd, 0xf4, 0xf3
180 static uint8_t
crc8(uint8_t *p, uint8_t len)
186 i = (crc ^ *p++) & 0xFF;
187 crc = (
crc_table[i] ^ (crc << 8)) & 0xFF;
196 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id())),
197 _orientation(orientation)
230 int ret_val = transfer(
nullptr, 0, &val[0], 3);
233 PX4_ERR(
"error reading from sensor: %d", ret_val);
239 uint16_t distance_mm = (val[0] << 8) | val[1];
240 float distance_m =
static_cast<float>(distance_mm) * 1e-3
f;
248 report.signal_quality = -1;
251 report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
252 report.variance = 0.0f;
254 if (
crc8(val, 2) == val[2] &&
255 (
float)report.current_distance > report.min_distance &&
256 (
float)report.current_distance < report.max_distance) {
277 PX4_WARN(
"Disabled");
350 PX4_ERR(
"invalid HW model %d.", hw_model);
362 int ret_val = transfer(&cmd,
sizeof(cmd),
nullptr, 0);
364 if (ret_val != PX4_OK) {
366 PX4_DEBUG(
"i2c::transfer returned %d", ret_val);
377 uint8_t who_am_i = 0;
382 if (transfer(&cmd, 1,
nullptr, 0) ==
OK) {
388 PX4_DEBUG(
"WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n",
434 int start(
const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
436 const uint8_t orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
453 if (g_dev !=
nullptr) {
454 PX4_ERR(
"already started");
476 if (g_dev !=
nullptr) {
477 PX4_ERR(
"already started");
484 if (g_dev ==
nullptr) {
490 if (g_dev->
init() != PX4_OK) {
499 PX4_INFO(
"driver started");
509 if (g_dev !=
nullptr) {
515 PX4_INFO(
"driver stopped");
525 if (g_dev ==
nullptr) {
526 PX4_ERR(
"driver not running");
538 PRINT_MODULE_DESCRIPTION(
542 I2C bus driver for TeraRanger rangefinders. 544 The sensor/driver must be enabled using the parameter SENS_EN_TRANGER. 546 Setup/usage information: https://docs.px4.io/en/sensor/rangefinders.html#teraranger-rangefinders 549 Start driver on any bus (start on bus where first sensor found). 550 $ teraranger start -a 551 Start driver on specified bus 552 $ teraranger start -b 1 557 PRINT_MODULE_USAGE_NAME("teraranger",
"driver");
558 PRINT_MODULE_USAGE_SUBCATEGORY(
"distance_sensor");
559 PRINT_MODULE_USAGE_COMMAND_DESCR(
"start",
"Start driver");
560 PRINT_MODULE_USAGE_PARAM_FLAG(
'a',
"Attempt to start driver on all I2C buses (first one found)",
true);
561 PRINT_MODULE_USAGE_PARAM_INT(
'b', 1, 1, 2000,
"Start driver on specific I2C bus",
true);
562 PRINT_MODULE_USAGE_PARAM_INT(
'R', 25, 1, 25,
"Sensor rotation - downward facing by default",
true);
563 PRINT_MODULE_USAGE_COMMAND_DESCR(
"stop",
"Stop driver");
564 PRINT_MODULE_USAGE_COMMAND_DESCR(
"status",
"Print driver information");
576 const char *myoptarg =
nullptr;
582 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
584 bool start_all =
false;
586 while ((ch = px4_getopt(argc, argv,
"ab:R:", &myoptind, &myoptarg)) != EOF) {
593 i2c_bus = atoi(myoptarg);
597 rotation = (uint8_t)atoi(myoptarg);
601 PX4_WARN(
"Unknown option!");
606 if (myoptind >= argc) {
611 if (!strcmp(argv[myoptind],
"start")) {
622 if (!strcmp(argv[myoptind],
"status")) {
627 if (!strcmp(argv[myoptind],
"stop")) {
#define TERARANGER_WHO_AM_I_REG_VAL
virtual int probe() override
measure the time elapsed performing an event
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
API for the uORB lightweight object broker.
#define TERARANGER_EVO_60M_MAX_DISTANCE
#define TERARANGER_WHO_AM_I_REG
void usage(const char *reason)
Print the correct usage.
#define TERARANGER_EVO_BASEADDR
#define TERARANGER_EVO_3M_MIN_DISTANCE
TERARANGER(const int bus=TERARANGER_BUS_DEFAULT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, const int address=TERARANGER_ONE_BASEADDR)
Local functions in support of the shell command.
int status()
Print the driver status.
void stop()
Stop the automatic measurement state machine.
void print_info()
Diagnostics - print some basic information about the driver.
#define TERARANGER_MEASUREMENT_INTERVAL
count the number of times an event occurs
#define TERARANGER_EVO_600HZ_MAX_DISTANCE
__EXPORT int teraranger_main(int argc, char *argv[])
Driver 'main' command.
Global flash based parameter store.
#define TERARANGER_ONE_MIN_DISTANCE
perf_counter_t _sample_perf
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
int stop()
Stop the driver.
void perf_count(perf_counter_t handle)
Count a performance event.
virtual int init() override
perf_counter_t _comms_errors
void perf_free(perf_counter_t handle)
Free a counter.
#define TERARANGER_EVO_600HZ_MIN_DISTANCE
void init()
Activates/configures the hardware registers.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static uint8_t crc8(uint8_t *p, uint8_t len)
#define TERARANGER_EVO_3M_MAX_DISTANCE
void perf_end(perf_counter_t handle)
End a performance event.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
#define TERARANGER_BUS_DEFAULT
int start_bus(const int i2c_bus=TERARANGER_BUS_DEFAULT, const uint8_t orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the driver on a specific bus.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
static const int i2c_bus_options[]
void start()
Initialise the automatic measurement state machine and start it.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
int usage()
Prints info about the driver argument usage.
#define TERARANGER_ONE_MAX_DISTANCE
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
orb_advert_t _distance_sensor_topic
#define TERARANGER_MEASURE_REG
int orb_unadvertise(orb_advert_t handle)
static const uint8_t crc_table[]
int start(const uint8_t orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Attempt to start driver on all available I2C busses.
#define TERARANGER_EVO_60M_MIN_DISTANCE
#define NUM_I2C_BUS_OPTIONS
#define TERARANGER_ONE_BASEADDR
void perf_begin(perf_counter_t handle)
Begin a performance event.
int collect()
Collects the most recent sensor measurement data from the i2c bus.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
int measure()
Sends an i2c measure command to the sensors.
#define TERARANGER_DEVICE_PATH
Performance measuring tools.
Base class for devices connected via I2C.