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> 52 #include <sys/types.h> 56 #include <semaphore.h> 74 #include <board_config.h> 77 #define SF1XX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION 78 #define SF1XX_BASEADDR 0x66 79 #define SF1XX_DEVICE_PATH "/dev/sf1xx" 82 class SF1XX :
public device::I2C,
public px4::ScheduledWorkItem
101 int probe()
override;
171 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id())),
208 PX4_WARN(
"disabled.");
251 PX4_ERR(
"invalid HW model %d.", hw_model);
278 PX4_ERR(
"failed to create distance_sensor object");
287 PX4_INFO(
"(%dm %dHz) with address %d found", (
int)
_max_distance,
359 int interval = (1000000 / arg);
381 return I2C::ioctl(filp, cmd, arg);
407 ret +=
sizeof(*rbuf);
413 return ret ? ret : -EAGAIN;
455 ret = transfer(&cmd, 1,
nullptr, 0);
459 PX4_DEBUG(
"i2c::transfer returned %d", ret);
474 uint8_t val[2] = {0, 0};
477 ret = transfer(
nullptr, 0, &val[0], 2);
480 PX4_DEBUG(
"error reading from sensor: %d", ret);
486 uint16_t distance_cm = val[0] << 8 | val[1];
487 float distance_m = float(distance_cm) * 1e-2
f;
491 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
541 PX4_DEBUG(
"collection error");
557 _reports->print_info(
"report queue");
568 int start(uint8_t rotation);
569 int start_bus(uint8_t rotation,
int i2c_bus);
587 if (g_dev !=
nullptr) {
588 PX4_ERR(
"already started");
612 if (g_dev !=
nullptr) {
613 PX4_ERR(
"already started");
618 g_dev =
new SF1XX(rotation, i2c_bus);
620 if (g_dev ==
nullptr) {
624 if (
OK != g_dev->
init()) {
645 if (g_dev !=
nullptr) {
659 if (g_dev !=
nullptr) {
664 PX4_ERR(
"driver not running");
686 PX4_ERR(
"%s open failed (try 'sf1xx start' if the driver is not running)",
SF1XX_DEVICE_PATH);
691 sz =
read(fd, &report,
sizeof(report));
693 if (sz !=
sizeof(report)) {
694 PX4_ERR(
"immediate read failed");
698 print_message(report);
702 PX4_ERR(
"failed to set 2Hz poll rate");
707 for (
unsigned i = 0; i < 5; i++) {
713 ret = poll(&fds, 1, 2000);
716 PX4_ERR(
"timed out waiting for sensor data");
721 sz =
read(fd, &report,
sizeof(report));
723 if (sz !=
sizeof(report)) {
724 PX4_ERR(
"periodic read failed");
728 print_message(report);
733 PX4_ERR(
"failed to set default poll rate");
757 PX4_ERR(
"driver reset failed");
762 PX4_ERR(
"driver poll restart failed");
777 if (g_dev ==
nullptr) {
778 PX4_ERR(
"driver not running");
782 printf(
"state @ %p\n", g_dev);
794 PRINT_MODULE_DESCRIPTION(
798 I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20. 800 Setup/usage information: https://docs.px4.io/en/sensor/sfxx_lidar.html 804 Attempt to start driver on any bus (start on bus where first sensor found). 810 PRINT_MODULE_USAGE_NAME("sf1xx",
"driver");
811 PRINT_MODULE_USAGE_SUBCATEGORY(
"distance_sensor");
812 PRINT_MODULE_USAGE_COMMAND_DESCR(
"start",
"Start driver");
813 PRINT_MODULE_USAGE_PARAM_FLAG(
'a',
"Attempt to start driver on all I2C buses",
true);
814 PRINT_MODULE_USAGE_PARAM_INT(
'b', 1, 1, 2000,
"Start driver on specific I2C bus",
true);
815 PRINT_MODULE_USAGE_PARAM_INT(
'R', 25, 1, 25,
"Sensor rotation - downward facing by default",
true);
816 PRINT_MODULE_USAGE_COMMAND_DESCR(
"stop",
"Stop driver");
817 PRINT_MODULE_USAGE_COMMAND_DESCR(
"test",
"Test driver (basic functional tests)");
818 PRINT_MODULE_USAGE_COMMAND_DESCR(
"reset",
"Reset driver");
819 PRINT_MODULE_USAGE_COMMAND_DESCR(
"info",
"Print driver information");
828 const char *myoptarg =
nullptr;
829 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
830 bool start_all =
false;
834 while ((ch = px4_getopt(argc, argv,
"ab:R:", &myoptind, &myoptarg)) != EOF) {
837 rotation = (uint8_t)atoi(myoptarg);
841 i2c_bus = atoi(myoptarg);
849 PX4_WARN(
"Unknown option!");
854 if (myoptind >= argc) {
861 if (!strcmp(argv[myoptind],
"start")) {
873 if (!strcmp(argv[myoptind],
"stop")) {
880 if (!strcmp(argv[myoptind],
"test")) {
887 if (!strcmp(argv[myoptind],
"reset")) {
894 if (!strcmp(argv[myoptind],
"info") || !strcmp(argv[myoptind],
"status")) {
ringbuffer::RingBuffer * _reports
#define RANGE_FINDER_BASE_DEVICE_PATH
#define SF1XX_DEVICE_PATH
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.
perf_counter_t _sample_perf
virtual ~SF1XX() override
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
float get_minimum_distance()
int reset()
Reset the driver.
void set_maximum_distance(float max)
count the number of times an event occurs
High-resolution timer with callouts and timekeeping.
ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override
Global flash based parameter store.
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
int start(uint8_t rotation)
Attempt to start driver on all available I2C busses.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void perf_count(perf_counter_t handle)
Count a performance event.
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void set_minimum_distance(float min)
Set the min and max distance thresholds if you want the end points of the sensors range to be brought...
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int start_bus(uint8_t rotation, int i2c_bus)
Start the driver on a specific bus.
#define SF1XX_BUS_DEFAULT
void start()
Initialise the automatic measurement state machine and start it.
void perf_end(perf_counter_t handle)
End a performance event.
__EXPORT int sf1xx_main(int argc, char *argv[])
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Local functions in support of the shell command.
int info()
Print a little info about the driver.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
static const int i2c_bus_options[]
int stop()
Stop the driver.
constexpr _Tp min(_Tp a, _Tp b)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
void stop()
Stop the automatic measurement state machine.
SF1XX(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus=SF1XX_BUS_DEFAULT, int address=SF1XX_BASEADDR)
void print_info()
Diagnostics - print some basic information about the driver.
constexpr _Tp max(_Tp a, _Tp b)
perf_counter_t _comms_errors
float get_maximum_distance()
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
static void sf1xx_usage()
#define SENSORIOCRESET
Reset the sensor to its default configuration.
int orb_unadvertise(orb_advert_t handle)
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
#define NUM_I2C_BUS_OPTIONS
int probe_address(uint8_t address)
Test whether the device supported by the driver is present at a specific address. ...
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Performance measuring tools.
Base class for devices connected via I2C.
orb_advert_t _distance_sensor_topic