42 #include <px4_platform_common/px4_config.h> 43 #include <px4_platform_common/getopt.h> 44 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 46 #include <sys/types.h> 50 #include <semaphore.h> 75 #define SF0X_TAKE_RANGE_REG 'd' 78 #define SF0X_DEFAULT_PORT "/dev/ttyS6" 83 SF0X(
const char *port =
SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
86 virtual int init()
override;
161 ScheduledWorkItem(MODULE_NAME,
px4::serial_port_to_wq(port)),
246 PX4_ERR(
"invalid HW model %d.", hw_model);
258 if (ret !=
OK) {
break; }
264 PX4_ERR(
"alloc failed");
278 PX4_ERR(
"failed to create distance_sensor object");
345 int interval = (1000000 / arg);
367 return CDev::ioctl(filp, cmd, arg);
393 ret +=
sizeof(*rbuf);
399 return ret ? ret : -EAGAIN;
442 if (ret !=
sizeof(cmd)) {
444 PX4_DEBUG(
"write fail %d", ret);
463 unsigned readlen =
sizeof(readbuf) - 1;
466 int ret =
::read(
_fd, &readbuf[0], readlen);
469 PX4_DEBUG(
"read err: %d", ret);
481 }
else if (ret == 0) {
487 float distance_m = -1.0f;
490 for (
int i = 0; i < ret; i++) {
500 PX4_DEBUG(
"val (float): %8.4f, raw: %s, valid: %s", (
double)distance_m,
_linebuf, ((valid) ?
"OK" :
"NO"));
505 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
555 PX4_ERR(
"open failed (%i)", errno);
559 struct termios uart_config;
564 tcgetattr(
_fd, &uart_config);
567 uart_config.c_oflag &= ~ONLCR;
570 uart_config.c_cflag &= ~(CSTOPB | PARENB);
587 if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
588 PX4_ERR(
"CFG: %d ISPD", termios_state);
591 if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
592 PX4_ERR(
"CFG: %d OSPD", termios_state);
595 if ((termios_state = tcsetattr(
_fd, TCSANOW, &uart_config)) < 0) {
596 PX4_ERR(
"baud %d ATTR", termios_state);
606 if (collect_ret == -EAGAIN) {
608 ScheduleDelayed(1042 * 8);
613 if (
OK != collect_ret) {
648 PX4_DEBUG(
"measure error");
664 _reports->print_info(
"report queue");
675 int start(
const char *port, uint8_t rotation);
685 start(
const char *port, uint8_t rotation)
689 if (g_dev !=
nullptr) {
690 PX4_WARN(
"already started");
695 g_dev =
new SF0X(port, rotation);
697 if (g_dev ==
nullptr) {
701 if (
OK != g_dev->
init()) {
709 PX4_ERR(
"device open fail (%i)", errno);
721 if (g_dev !=
nullptr) {
734 if (g_dev !=
nullptr) {
764 sz =
read(fd, &report,
sizeof(report));
766 if (sz !=
sizeof(report)) {
767 PX4_ERR(
"immediate read failed");
771 print_message(report);
775 PX4_ERR(
"failed to set 2Hz poll rate");
780 for (
unsigned i = 0; i < 5; i++) {
786 int ret =
poll(&fds, 1, 2000);
789 PX4_ERR(
"timed out");
794 sz =
read(fd, &report,
sizeof(report));
796 if (sz !=
sizeof(report)) {
797 PX4_ERR(
"read failed: got %zi vs exp. %zu", sz,
sizeof(report));
801 print_message(report);
806 PX4_ERR(
"ioctl SENSORIOCSPOLLRATE failed");
822 PX4_ERR(
"open failed (%i)", errno);
827 PX4_ERR(
"driver reset failed");
832 PX4_ERR(
"driver poll restart failed");
845 if (g_dev ==
nullptr) {
846 PX4_ERR(
"driver not running");
850 printf(
"state @ %p\n", g_dev);
861 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
865 const char *myoptarg =
nullptr;
867 while ((ch = px4_getopt(argc, argv,
"R:d:", &myoptind, &myoptarg)) != EOF) {
870 rotation = (uint8_t)atoi(myoptarg);
874 device_path = myoptarg;
878 PX4_WARN(
"Unknown option!");
883 if (myoptind >= argc) {
890 if (!strcmp(argv[myoptind],
"start")) {
897 if (!strcmp(argv[myoptind],
"stop")) {
904 if (!strcmp(argv[myoptind],
"test")) {
911 if (!strcmp(argv[myoptind],
"reset")) {
918 if (!strcmp(argv[myoptind],
"info") || !strcmp(argv[myoptind],
"status")) {
923 PX4_ERR(
"unrecognized command, try 'start', 'test', 'reset' or 'info'");
unsigned _consecutive_fail_count
#define RANGE_FINDER_BASE_DEVICE_PATH
virtual int open(file_t *filep)
Handle an open of the device.
measure the time elapsed performing an event
orb_advert_t _distance_sensor_topic
void print_info()
Diagnostics - print some basic information about the driver.
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
API for the uORB lightweight object broker.
virtual int register_class_devname(const char *class_devname)
Register a class device name, automatically adding device class instance suffix if need be...
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
int reset()
Reset the driver.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
float get_minimum_distance()
float get_maximum_distance()
ringbuffer::RingBuffer * _reports
count the number of times an event occurs
void start()
Initialise the automatic measurement state machine and start it.
int start(const char *port, uint8_t rotation)
Start the driver.
High-resolution timer with callouts and timekeeping.
int stop()
Stop the driver.
perf_counter_t _comms_errors
Global flash based parameter store.
virtual void poll_notify(pollevent_t events)
Report new poll events.
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
SF0X(const char *port=SF0X_DEFAULT_PORT, uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
void perf_count(perf_counter_t handle)
Count a performance event.
#define SF0X_DEFAULT_PORT
Abstract class for any character device.
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...
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance)
Register a class device name, automatically adding device class instance suffix if need be...
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
int info()
Print a little info about the driver.
void perf_end(perf_counter_t handle)
End a performance event.
void set_maximum_distance(float max)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
virtual int init() override
virtual ssize_t write(file_t *filep, const char *buffer, size_t buflen)
Perform a write to the device.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
constexpr _Tp min(_Tp a, _Tp b)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Local functions in support of the shell command.
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override
Perform a read from the device.
constexpr _Tp max(_Tp a, _Tp b)
enum SF0X_PARSE_STATE _parse_state
__EXPORT int sf0x_main(int argc, char *argv[])
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define SENSORIOCRESET
Reset the sensor to its default configuration.
CDev(const char *devname)
Constructor.
perf_counter_t _sample_perf
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override
Perform an ioctl operation on the device.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
#define SF0X_TAKE_RANGE_REG
#define RANGE_FINDER0_DEVICE_PATH
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).
void stop()
Stop the automatic measurement state machine.
virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
Perform a poll setup/teardown operation.
Performance measuring tools.