40 #include <px4_platform_common/px4_config.h> 41 #include <px4_platform_common/defines.h> 42 #include <px4_platform_common/getopt.h> 43 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 48 #include <sys/types.h> 52 #include <semaphore.h> 69 #include <board_config.h> 72 #define SRF02_BASEADDR 0x70 // 7-bit address. 8-bit address is 0xE0. 73 #define SRF02_BUS_DEFAULT PX4_I2C_BUS_EXPANSION 74 #define SRF02_DEVICE_PATH "/dev/srf02" 77 #define SRF02_TAKE_RANGE_REG 0x51 // Measure range Register. 78 #define SRF02_SET_ADDRESS_0 0xA0 // Change address 0 Register. 79 #define SRF02_SET_ADDRESS_1 0xAA // Change address 1 Register. 80 #define SRF02_SET_ADDRESS_2 0xA5 // Change address 2 Register. 83 #define SRF02_MIN_DISTANCE (0.20f) 84 #define SRF02_MAX_DISTANCE (7.65f) 86 #define SRF02_CONVERSION_INTERVAL 100000 // 60ms for one sonar. 87 #define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 // 30ms between each sonar measurement (watch out for interference!). 89 class SRF02 :
public device::I2C,
public px4::ScheduledWorkItem
108 int probe()
override;
175 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id())),
203 uint8_t val[2] = {0, 0};
207 int ret = transfer(&cmd, 1,
nullptr, 0);
208 ret = transfer(
nullptr, 0, &val[0], 2);
211 PX4_DEBUG(
"error reading from sensor: %d", ret);
217 uint16_t distance_cm = val[0] << 8 | val[1];
218 float distance_m = float(distance_cm) * 1e-2
f;
229 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
271 PX4_ERR(
"failed to create distance_sensor object");
289 PX4_DEBUG(
"sonar added");
306 PX4_DEBUG(
"sonar %d with address %d added", (i + 1),
addr_ind[i]);
309 PX4_DEBUG(
"Number of sonars connected: %zu",
addr_ind.
size());
352 int interval = (1000000 / arg);
374 return I2C::ioctl(filp, cmd, arg);
386 int ret = transfer(cmd, 2,
nullptr, 0);
390 PX4_DEBUG(
"i2c::transfer returned %d", ret);
403 _reports->print_info(
"report queue");
435 ret +=
sizeof(*rbuf);
441 return ret ? ret : -EAGAIN;
482 PX4_DEBUG(
"collection error");
551 int start(uint8_t rotation);
552 int start_bus(uint8_t rotation,
int i2c_bus);
572 PX4_ERR(
"driver reset failed");
577 PX4_ERR(
"driver poll restart failed");
595 if (g_dev !=
nullptr) {
596 PX4_ERR(
"already started");
618 if (g_dev !=
nullptr) {
619 PX4_ERR(
"already started");
624 g_dev =
new SRF02(rotation, i2c_bus);
626 if (g_dev ==
nullptr) {
627 PX4_ERR(
"failed to instantiate the device");
631 if (
OK != g_dev->
init()) {
632 PX4_ERR(
"failed to initialize the device");
664 if (g_dev ==
nullptr) {
665 PX4_ERR(
"driver not running");
669 printf(
"state @ %p\n", g_dev);
681 if (g_dev !=
nullptr) {
686 PX4_ERR(
"driver not running");
704 PX4_ERR(
"%s open failed (try 'srf02 start' if the driver is not running)",
SRF02_DEVICE_PATH);
711 ssize_t sz =
read(fd, &report,
sizeof(report));
713 if (sz !=
sizeof(report)) {
714 PX4_ERR(
"immediate read failed");
718 print_message(report);
722 PX4_ERR(
"failed to set 2Hz poll rate");
727 for (
unsigned i = 0; i < 5; i++) {
733 int ret = poll(&fds, 1, 2000);
736 PX4_ERR(
"timed out waiting for sensor data");
741 sz =
read(fd, &report,
sizeof(report));
743 if (sz !=
sizeof(report)) {
744 PX4_ERR(
"periodic read failed");
748 print_message(report);
753 PX4_ERR(
"failed to set default poll rate");
764 PX4_INFO(
"usage: srf02 command [options]");
765 PX4_INFO(
"options:");
767 PX4_INFO(
"\t-a --all");
768 PX4_INFO(
"\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
769 PX4_INFO(
"command:");
770 PX4_INFO(
"\tstart|stop|test|reset|info");
782 const char *myoptarg =
nullptr;
788 uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
790 bool start_all =
false;
792 while ((ch = px4_getopt(argc, argv,
"ab:R:", &myoptind, &myoptarg)) != EOF) {
795 rotation = (uint8_t)atoi(myoptarg);
799 i2c_bus = atoi(myoptarg);
807 PX4_WARN(
"Unknown option!");
812 if (myoptind >= argc) {
817 if (!strcmp(argv[myoptind],
"start")) {
827 if (!strcmp(argv[myoptind],
"stop")) {
832 if (!strcmp(argv[myoptind],
"test")) {
837 if (!strcmp(argv[myoptind],
"reset")) {
842 if (!strcmp(argv[myoptind],
"info") ||
843 !strcmp(argv[myoptind],
"status")) {
perf_counter_t _sample_perf
int start(uint8_t rotation)
Attempt to start driver on all available I2C busses.
int probe_address(uint8_t address)
Test whether the device supported by the driver is present at a specific address. ...
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
#define SRF02_BUS_DEFAULT
#define RANGE_FINDER_BASE_DEVICE_PATH
int stop()
Stop the driver.
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
measure the time elapsed performing an event
API for the uORB lightweight object broker.
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
#define SRF02_DEVICE_PATH
void usage(const char *reason)
Print the correct usage.
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override
int start_bus(uint8_t rotation, int i2c_bus)
Start the driver on a specific bus.
SRF02(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus=SRF02_BUS_DEFAULT, int address=SRF02_BASEADDR)
bool push_back(const T &x)
count the number of times an event occurs
High-resolution timer with callouts and timekeeping.
px4::Array< uint8_t, RANGE_FINDER_MAX_SENSORS > addr_ind
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
orb_advert_t _distance_sensor_topic
int reset()
Reset the driver.
#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.
#define SRF02_MIN_DISTANCE
perf_counter_t _comms_errors
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
__EXPORT int srf02_main(int argc, char *argv[])
Driver 'main' command.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int status()
Print the driver status.
void perf_end(perf_counter_t handle)
End a performance event.
__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)
static const int i2c_bus_options[]
void stop()
Stop the automatic measurement state machine.
int usage()
Prints info about the driver argument usage.
#define SRF02_MAX_DISTANCE
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define SENSORIOCRESET
Reset the sensor to its default configuration.
ringbuffer::RingBuffer * _reports
void print_info()
Diagnostics - print some basic information about the driver.
ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override
void start()
Initialise the automatic measurement state machine and start it.
#define NUM_I2C_BUS_OPTIONS
Local functions in support of the shell command.
#define SRF02_TAKE_RANGE_REG
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
#define SRF02_CONVERSION_INTERVAL
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).
#define RANGE_FINDER_MAX_SENSORS
Performance measuring tools.
#define SRF02_INTERVAL_BETWEEN_SUCCESIVE_FIRES
Base class for devices connected via I2C.