diff --git a/src/drivers/distance_sensor/mb12xx/CMakeLists.txt b/src/drivers/distance_sensor/mb12xx/CMakeLists.txt index 9d1dffd059..67fb66acd1 100644 --- a/src/drivers/distance_sensor/mb12xx/CMakeLists.txt +++ b/src/drivers/distance_sensor/mb12xx/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__mb12xx MAIN mb12xx + STACK_MAIN 1400 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index 4765e8571a..cc180c703e 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -39,562 +39,376 @@ * Driver for the Maxbotix sonar range finders connected via I2C. */ -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include #include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include - +#include +#include +#include +#include +#include #include #include -#include - +#include +#include +#include +#include +#include #include #include -#include +using namespace time_literals; /* Configuration Constants */ -#define MB12XX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION -#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ -#define MB12XX_DEVICE_PATH "/dev/mb12xx" +#define MB12XX_BASE_ADDR 0x70 // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224. +#define MB12XX_MIN_ADDR 0x5A // 7-bit address is 0x5A = 90. 8-bit address is 0xB4 = 180. +#define MB12XX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS_SPEED 100000 // 100kHz bus speed. +#define MB12XX_DEVICE_PATH "/dev/mb12xx" /* MB12xx Registers addresses */ - -#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ -#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ -#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ +#define MB12XX_TAKE_RANGE_REG 0x51 // Measure range Register. +#define MB12XX_SET_ADDRESS_1 0xAA // Change address 1 Register. +#define MB12XX_SET_ADDRESS_2 0xA5 // Change address 2 Register. /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) -#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ -#define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ +#define MB12XX_MEASURE_INTERVAL 100_ms // 60ms minimum for one sonar. +#define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!). -class MB12XX : public device::I2C, public px4::ScheduledWorkItem +class MB12XX : public device::I2C, public ModuleParams, public px4::ScheduledWorkItem { public: - MB12XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, - int bus = MB12XX_BUS_DEFAULT, int address = MB12XX_BASEADDR); + MB12XX(const int bus = MB12XX_BUS_DEFAULT); virtual ~MB12XX(); - virtual int init() override; - - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override; - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + virtual int init() override; /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + /** + * Sets a new device address. + * @param address The new sensor address to be set: 200-224 even addresses only. + * @return Returns PX4_OK iff successful, PX4_ERROR otherwise. + */ + int set_address(const uint8_t address = MB12XX_BASE_ADDR); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); protected: - virtual int probe() override; private: - uint8_t _rotation; - float _min_distance; - float _max_distance; - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_interval; - bool _collect_phase; - int _class_instance; - int _orb_class_instance; - - orb_advert_t _distance_sensor_topic; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ - int _cycling_rate; /* */ - uint8_t _index_counter; /* temporary sonar i2c address */ - px4::Array addr_ind; /* temp sonar i2c address vector */ /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + * Collects the most recent sensor measurement data from the i2c bus. + */ + int collect(); /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + * Gets the current sensor rotation value. + */ + int get_sensor_rotation(const size_t index); /** - * Stop the automatic measurement state machine. - */ - void stop(); + * Sends an i2c measure command to start the next sonar ping. + */ + int measure(); /** - * Set the min and max distance thresholds if you want the end points of the sensors - * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE - * and MB12XX_MAX_DISTANCE - */ - void set_minimum_distance(float min); - void set_maximum_distance(float max); - float get_minimum_distance(); - float get_maximum_distance(); + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - int measure(); - int collect(); + px4::Array _sensor_addresses {}; + px4::Array _sensor_rotations {}; + int _measure_interval{MB12XX_MEASURE_INTERVAL}; // Initialize the measure interval for a single sensor. + int _orb_class_instance{-1}; + + size_t _sensor_index{0}; // Initialize counter for cycling i2c adresses to zero. + + size_t _sensor_count{0}; + + orb_advert_t _distance_sensor_topic{nullptr}; + + perf_counter_t _comms_error{perf_alloc(PC_ELAPSED, "mb12xx_comms_error")}; + perf_counter_t _sample_perf{perf_alloc(PC_COUNT, "mb12xx_sample_perf")}; + + DEFINE_PARAMETERS( + (ParamInt) _p_sensor_enabled, + (ParamInt) _p_sensor0_rot, + (ParamInt) _p_sensor1_rot, + (ParamInt) _p_sensor2_rot, + (ParamInt) _p_sensor3_rot, + (ParamInt) _p_sensor4_rot, + (ParamInt) _p_sensor5_rot, + (ParamInt) _p_sensor6_rot, + (ParamInt) _p_sensor7_rot, + (ParamInt) _p_sensor8_rot, + (ParamInt) _p_sensor9_rot, + (ParamInt) _p_sensor10_rot, + (ParamInt) _p_sensor11_rot + ); }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); - -MB12XX::MB12XX(uint8_t rotation, int bus, int address) : - I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _rotation(rotation), - _min_distance(MB12XX_MIN_DISTANCE), - _max_distance(MB12XX_MAX_DISTANCE), - _reports(nullptr), - _sensor_ok(false), - _measure_interval(0), - _collect_phase(false), - _class_instance(-1), - _orb_class_instance(-1), - _distance_sensor_topic(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), - _comms_errors(perf_alloc(PC_COUNT, "mb12xx_com_err")), - _cycle_counter(0), /* initialising counter for cycling function to zero */ - _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ - _index_counter(0) /* initialising temp sonar i2c address to zero */ +MB12XX::MB12XX(const int bus) : + I2C("MB12xx", MB12XX_DEVICE_PATH, bus, MB12XX_BASE_ADDR, MB12XX_BUS_SPEED), + ModuleParams(nullptr), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { } MB12XX::~MB12XX() { - /* make sure we are truly inactive */ + // Ensure we are truly inactive. stop(); - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; + // Unadvertise the distance sensor topic. + if (_distance_sensor_topic != nullptr) { + orb_unadvertise(_distance_sensor_topic); } - if (_class_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); - } - - /* free perf counters */ + // Free perf counters. + perf_free(_comms_error); perf_free(_sample_perf); - perf_free(_comms_errors); -} - -int -MB12XX::init() -{ - int ret = PX4_ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) { - return ret; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); - - _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ - set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ - - if (_reports == nullptr) { - return ret; - } - - _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; - - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); - - if (_distance_sensor_topic == nullptr) { - PX4_ERR("failed to create distance_sensor object"); - } - - // XXX we should find out why we need to wait 200 ms here - px4_usleep(200000); - - /* check for connected rangefinders on each i2c port: - We start from i2c base address (0x70 = 112) and count downwards - So second iteration it uses i2c address 111, third iteration 110 and so on*/ - for (unsigned counter = 0; counter <= RANGE_FINDER_MAX_SENSORS; counter++) { - _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ - set_device_address(_index_counter); /* set I2c port to temp sonar i2c adress */ - int ret2 = measure(); - - if (ret2 == 0) { /* sonar is present -> store address_index in array */ - addr_ind.push_back(_index_counter); - PX4_DEBUG("sonar added"); - } - } - - _index_counter = MB12XX_BASEADDR; - set_device_address(_index_counter); /* set i2c port back to base adress for rest of driver */ - - /* if only one sonar detected, no special timing is required between firing, so use default */ - if (addr_ind.size() == 1) { - _cycling_rate = MB12XX_CONVERSION_INTERVAL; - - } else { - _cycling_rate = MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES; - } - - /* show the connected sonars in terminal */ - for (unsigned i = 0; i < addr_ind.size(); i++) { - PX4_DEBUG("sonar %d with address %d added", (i + 1), addr_ind[i]); - } - - PX4_DEBUG("Number of sonars connected: %lu", addr_ind.size()); - - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; - - return ret; -} - -int -MB12XX::probe() -{ - return measure(); -} - -void -MB12XX::set_minimum_distance(float min) -{ - _min_distance = min; -} - -void -MB12XX::set_maximum_distance(float max) -{ - _max_distance = max; -} - -float -MB12XX::get_minimum_distance() -{ - return _min_distance; -} - -float -MB12XX::get_maximum_distance() -{ - return _max_distance; -} - -int -MB12XX::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = _cycling_rate; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - int interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < _cycling_rate) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -MB12XX::read(device::file_t *filp, char *buffer, size_t buflen) -{ - - unsigned count = buflen / sizeof(struct distance_sensor_s); - struct distance_sensor_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - px4_usleep(_cycling_rate * 2); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - - } while (0); - - return ret; -} - -int -MB12XX::measure() -{ - - int ret; - - /* - * Send the command to begin a measurement. - */ - - uint8_t cmd = MB12XX_TAKE_RANGE_REG; - ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); - return ret; - } - - ret = OK; - - return ret; } int MB12XX::collect() { - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - perf_begin(_sample_perf); + uint8_t val[2] = {}; - ret = transfer(nullptr, 0, &val[0], 2); + // Increment i2c adress to next sensor. + _sensor_index++; + _sensor_index %= _sensor_count; - if (ret < 0) { - PX4_DEBUG("error reading from sensor: %d", ret); - perf_count(_comms_errors); + // Set the sensor i2c adress for the active cycle. + set_device_address(_sensor_addresses[_sensor_index]); + + // Transfer data from the bus. + int ret_val = transfer(nullptr, 0, &val[0], 2); + + if (ret_val < 0) { + PX4_ERR("sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address()); + perf_count(_comms_error); perf_end(_sample_perf); - return ret; + return ret_val; } uint16_t distance_cm = val[0] << 8 | val[1]; - float distance_m = float(distance_cm) * 1e-2f; + float distance_m = static_cast(distance_cm) * 1e-2f; - struct distance_sensor_s report; - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - report.orientation = _rotation; + distance_sensor_s report; report.current_distance = distance_m; - report.min_distance = get_minimum_distance(); - report.max_distance = get_maximum_distance(); - report.variance = 0.0f; - report.signal_quality = -1; - /* TODO: set proper ID */ - report.id = 0; + report.id = _sensor_addresses[_sensor_index]; + report.max_distance = MB12XX_MAX_DISTANCE; + report.min_distance = MB12XX_MIN_DISTANCE; + report.orientation = _sensor_rotations[_sensor_index]; + report.signal_quality = -1; + report.timestamp = hrt_absolute_time(); + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.variance = 0.0f; - /* publish it, if we are the primary */ - if (_distance_sensor_topic != nullptr) { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); + int instance_id; + orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id, ORB_PRIO_DEFAULT); + + // Begin the next measurement. + if (measure() != PX4_OK) { + PX4_INFO("sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address()); + perf_count(_comms_error); + perf_end(_sample_perf); + return ret_val; } - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -MB12XX::start() +int +MB12XX::get_sensor_rotation(const size_t index) { + switch (index) { + case 0: return _p_sensor0_rot.get(); - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); + case 1: return _p_sensor1_rot.get(); - /* schedule a cycle to start things */ - ScheduleDelayed(5); + case 2: return _p_sensor2_rot.get(); + + case 3: return _p_sensor3_rot.get(); + + case 4: return _p_sensor4_rot.get(); + + case 5: return _p_sensor5_rot.get(); + + case 6: return _p_sensor6_rot.get(); + + case 7: return _p_sensor7_rot.get(); + + case 8: return _p_sensor8_rot.get(); + + case 9: return _p_sensor9_rot.get(); + + case 10: return _p_sensor10_rot.get(); + + case 11: return _p_sensor11_rot.get(); + + default: return PX4_ERROR; + } } -void -MB12XX::stop() +int +MB12XX::init() { - ScheduleClear(); -} + if (_p_sensor_enabled.get() == 0) { + PX4_WARN("disabled"); + return PX4_ERROR; + } -void -MB12XX::Run() -{ - if (_collect_phase) { - _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ - set_device_address(_index_counter); + // Initialize the I2C device + if (I2C::init() != OK) { + return PX4_ERROR; + } - /* perform collection */ - if (OK != collect()) { - PX4_DEBUG("collection error"); - /* if error restart the measurement state machine */ - start(); - return; - } + // Check for connected rangefinders on each i2c port by decrementing from the base address, + // (MB12XX_BASE_ADDR = 112, MB12XX_MIN_ADDR = 90). + for (uint8_t address = MB12XX_BASE_ADDR; address > MB12XX_MIN_ADDR ; address--) { + set_device_address(address); - /* next phase is measurement */ - _collect_phase = false; + if (measure() == PX4_OK) { + // Store I2C address + _sensor_addresses[_sensor_count] = address; + _sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count); + _sensor_count++; - /* change i2c adress to next sonar */ - _cycle_counter = _cycle_counter + 1; + PX4_INFO("sensor %i at address 0x%02X added", _sensor_count, get_device_address()); - if (_cycle_counter >= addr_ind.size()) { - _cycle_counter = 0; - } + if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) { + break; + } - /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate - Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ - - if (_measure_interval > _cycling_rate) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - _cycling_rate); - - return; + px4_usleep(_measure_interval); } } - /* Measurement (firing) phase */ - - /* ensure sonar i2c adress is still correct */ - _index_counter = addr_ind[_cycle_counter]; - set_device_address(_index_counter); - - /* Perform measurement */ - if (OK != measure()) { - PX4_DEBUG("measure error sonar adress %d", _index_counter); + // Return an error if no sensors were detected. + if (_sensor_count == 0) { + PX4_ERR("no sensors discovered"); + return PX4_ERROR; } - /* next phase is collection */ - _collect_phase = true; + // If more than one sonar is detected, adjust the meaure interval to avoid sensor interference. + if (_sensor_count > 1) { + _measure_interval = MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES; + } - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_cycling_rate); + PX4_INFO("Total sensors connected: %i", _sensor_count); + return PX4_OK; +} + +int +MB12XX::measure() +{ + // Send the command to take a measurement. + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + int ret_val = transfer(&cmd, 1, nullptr, 0); + + return ret_val; } void MB12XX::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); + perf_print_counter(_comms_error); + PX4_INFO("poll interval: %ums", _measure_interval / 1000); + + for (size_t i = 0; i < _sensor_count; i++) { + PX4_INFO("sensor: %u, address %u", i, _sensor_addresses[i]); + } +} + +void +MB12XX::Run() +{ + // Collect the sensor data. + if (collect() != PX4_OK) { + PX4_INFO("collection error"); + } +} + +int +MB12XX::set_address(const uint8_t address) +{ + if (_sensor_count > 1) { + PX4_INFO("multiple sensors are connected"); + return PX4_ERROR; + } + + if (address < 2 || + address == 80 || + address == 164 || + address == 170 || + address > 224) { + PX4_ERR("incompatible address requested"); + return PX4_ERROR; + } + + PX4_INFO("requested address: %u", address); + + uint8_t shifted_address = (address << 1); + uint8_t cmd[3] = {MB12XX_SET_ADDRESS_1, MB12XX_SET_ADDRESS_2, shifted_address}; + + if (transfer(cmd, sizeof(cmd), nullptr, 0) != PX4_OK) { + PX4_INFO("could not set the address"); + } + + set_device_address(address); + PX4_INFO("device address: %u", get_device_address()); + return PX4_OK; +} + +void +MB12XX::start() +{ + // Fetch parameter values. + ModuleParams::updateParams(); + + // Schedule the driver cycle at regular intervals. + ScheduleOnInterval(_measure_interval); +} + +void +MB12XX::stop() +{ + ScheduleClear(); } /** @@ -605,24 +419,50 @@ namespace mb12xx MB12XX *g_dev; -int start(uint8_t rotation); -int start_bus(uint8_t rotation, int i2c_bus); -int stop(); -int test(); -int reset(); -int info(); +int reset(); +int set_address(const uint8_t address = MB12XX_BASE_ADDR); +int start(); +int start_bus(const int i2c_bus = MB12XX_BUS_DEFAULT); +int status(); +int stop(); +int test(); +int usage(); + +/** + * Reset the driver. + */ +int +reset() +{ + PX4_INFO("driver resetting"); + stop(); + start(); + return PX4_OK; +} + +int +set_address(const uint8_t address) +{ + if (g_dev != nullptr) { + if (g_dev->set_address(address) != PX4_OK) { + PX4_ERR("address not set"); + return PX4_ERROR; + } + } + + reset(); + return PX4_OK; +} /** - * * Attempt to start driver on all available I2C busses. * * This function will return as soon as the first sensor * is detected on one of the available busses or if no * sensors are detected. - * */ int -start(uint8_t rotation) +start() { if (g_dev != nullptr) { PX4_ERR("already started"); @@ -630,7 +470,7 @@ start(uint8_t rotation) } for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) { - if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) { + if (start_bus(i2c_bus_options[i]) == PX4_OK) { return PX4_OK; } } @@ -645,56 +485,53 @@ start(uint8_t rotation) * or could not be detected successfully. */ int -start_bus(uint8_t rotation, int i2c_bus) +start_bus(const int i2c_bus) { - int fd = -1; - if (g_dev != nullptr) { PX4_ERR("already started"); + return PX4_OK; + } + + // Instantiate the driver. + g_dev = new MB12XX(i2c_bus); + + if (g_dev == nullptr) { + delete g_dev; return PX4_ERROR; } - /* create the driver */ - g_dev = new MB12XX(rotation, i2c_bus); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(MB12XX_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - px4_close(fd); - return PX4_OK; - -fail: - - if (fd >= 0) { - px4_close(fd); - } - - if (g_dev != nullptr) { + // Initialize the sensor. + if (g_dev->init() != PX4_OK) { delete g_dev; g_dev = nullptr; + return PX4_ERROR; } - return PX4_ERROR; + // Start the driver. + g_dev->start(); + + PX4_INFO("driver started"); + return PX4_OK; } /** - * Stop the driver + * Print the driver status. + */ +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + g_dev->print_info(); + + return PX4_OK; +} + +/** + * Stop the driver. */ int stop() @@ -702,12 +539,9 @@ stop() if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - return PX4_ERROR; } + PX4_INFO("driver stopped"); return PX4_OK; } @@ -719,10 +553,6 @@ stop() int test() { - struct distance_sensor_s report; - ssize_t sz; - int ret; - int fd = px4_open(MB12XX_DEVICE_PATH, O_RDONLY); if (fd < 0) { @@ -730,8 +560,9 @@ test() return PX4_ERROR; } - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); + // Perform a simple demand read. + distance_sensor_s report {}; + ssize_t sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("immediate read failed"); @@ -740,182 +571,102 @@ test() print_message(report); - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_ERR("failed to set 2Hz poll rate"); - return PX4_ERROR; - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - PX4_ERR("timed out waiting for sensor data"); - return PX4_ERROR; - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("periodic read failed"); - return PX4_ERROR; - } - - print_message(report); - } - - /* reset the sensor polling to default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("failed to set default poll rate"); - return PX4_ERROR; - } - PX4_INFO("PASS"); return PX4_OK; } /** - * Reset the driver. + * Print information about the driver usage. */ int -reset() +usage() { - int fd = px4_open(MB12XX_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed"); - return PX4_ERROR; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - return PX4_ERROR; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - px4_close(fd); + PX4_INFO("usage: mb12xx [options]"); + PX4_INFO("options:"); + PX4_INFO("\t-a --all available busses"); + PX4_INFO("\t-b --bus i2cbus (%d)", MB12XX_BUS_DEFAULT); + PX4_INFO("\t-A --set device address (0-112, 0x00-0x70)"); + PX4_INFO("command:"); + PX4_INFO("\treset|set_address|start|status|stop|test|usage"); return PX4_OK; } +} // namespace mb12xx + /** - * Print a little info about the driver. + * Driver 'main' command. */ -int -info() +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]) { - if (g_dev == nullptr) { - PX4_ERR("driver poll restart failed"); - return PX4_ERROR; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return PX4_OK; -} - -} /* namespace */ - - -static void -mb12xx2_usage() -{ - PX4_INFO("usage: mb12xx command [options]"); - PX4_INFO("options:"); - PX4_INFO("\t-b --bus i2cbus (%d)", MB12XX_BUS_DEFAULT); - PX4_INFO("\t-a --all"); - PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING); - PX4_INFO("command:"); - PX4_INFO("\tstart|stop|test|reset|info"); -} - -int -mb12xx_main(int argc, char *argv[]) -{ - int ch; - int myoptind = 1; const char *myoptarg = nullptr; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + int ch = 0; + int i2c_bus = MB12XX_BUS_DEFAULT; + int myoptind = 1; + + uint8_t address = MB12XX_BASE_ADDR; + bool start_all = false; - int i2c_bus = MB12XX_BUS_DEFAULT; - - while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "abs:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); + case 'a': + start_all = true; break; case 'b': i2c_bus = atoi(myoptarg); break; - case 'a': - start_all = true; + case 's': + address = (uint8_t)atoi(myoptarg); break; default: PX4_WARN("Unknown option!"); - goto out_error; + return mb12xx::usage(); } } if (myoptind >= argc) { - goto out_error; + return mb12xx::usage(); } - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - if (start_all) { - return mb12xx::start(rotation); - - } else { - return mb12xx::start_bus(rotation, i2c_bus); - } - } - - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - return mb12xx::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - return mb12xx::test(); - } - - /* - * Reset the driver. - */ + // Reset the driver. if (!strcmp(argv[myoptind], "reset")) { return mb12xx::reset(); } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - return mb12xx::info(); + // Set the device I2C address. + if (!strcmp(argv[myoptind], "set_address")) { + return mb12xx::set_address(address); } -out_error: - mb12xx2_usage(); - return PX4_ERROR; + // Start/load the driver. + if (!strcmp(argv[myoptind], "start")) { + if (start_all) { + return mb12xx::start(); + + } else { + return mb12xx::start_bus(i2c_bus); + } + } + + // Print the driver status. + if (!strcmp(argv[myoptind], "status")) { + return mb12xx::status(); + } + + // Stop the driver. + if (!strcmp(argv[myoptind], "stop")) { + return mb12xx::stop(); + } + + // Test the driver/device. + if (!strcmp(argv[myoptind], "test")) { + return mb12xx::test(); + } + + // Print driver usage information. + return mb12xx::usage(); } diff --git a/src/drivers/distance_sensor/mb12xx/parameters.c b/src/drivers/distance_sensor/mb12xx/parameters.c index cd3535f85b..53a53276e3 100644 --- a/src/drivers/distance_sensor/mb12xx/parameters.c +++ b/src/drivers/distance_sensor/mb12xx/parameters.c @@ -40,3 +40,255 @@ * @group Sensors */ PARAM_DEFINE_INT32(SENS_EN_MB12XX, 0); + +/** + * MaxBotix MB12XX Sensor 0 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_0_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 1 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_1_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 2 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_2_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 3 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_3_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 4 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_4_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 5 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_5_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 6 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_6_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 7 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_7_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 8 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_8_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 9 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_9_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 10 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_10_ROT, 0); + +/** + * MaxBotix MB12XX Sensor 12 Rotation + * + * This parameter defines the rotation of the sensor relative to the platform. + * + * @reboot_required true + * @min 0 + * @max 7 + * @group Sensors + * + * @value 0 No rotation + * @value 1 Yaw 45° + * @value 2 Yaw 90° + * @value 3 Yaw 135° + * @value 4 Yaw 180° + * @value 5 Yaw 225° + * @value 6 Yaw 270° + * @value 7 Yaw 315° + */ +PARAM_DEFINE_INT32(SENS_MB12_11_ROT, 0); \ No newline at end of file