45 #include <semaphore.h> 51 #include <sys/types.h> 55 #include <board_config.h> 63 #include <px4_platform_common/px4_config.h> 64 #include <px4_platform_common/getopt.h> 65 #include <px4_platform_common/module_params.h> 66 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> 73 #define MB12XX_BASE_ADDR 0x70 // 7-bit address is 0x70 = 112. 8-bit address is 0xE0 = 224. 74 #define MB12XX_MIN_ADDR 0x5A // 7-bit address is 0x5A = 90. 8-bit address is 0xB4 = 180. 75 #define MB12XX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION 76 #define MB12XX_BUS_SPEED 100000 // 100kHz bus speed. 77 #define MB12XX_DEVICE_PATH "/dev/mb12xx" 80 #define MB12XX_TAKE_RANGE_REG 0x51 // Measure range Register. 81 #define MB12XX_SET_ADDRESS_1 0xAA // Change address 1 Register. 82 #define MB12XX_SET_ADDRESS_2 0xA5 // Change address 2 Register. 85 #define MB12XX_MIN_DISTANCE (0.20f) 86 #define MB12XX_MAX_DISTANCE (7.65f) 88 #define MB12XX_MEASURE_INTERVAL 100_ms // 60ms minimum for one sonar. 89 #define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES 100_ms // 30ms minimum between each sonar measurement (watch out for interference!). 91 class MB12XX :
public device::I2C,
public ModuleParams,
public px4::ScheduledWorkItem
97 virtual int init()
override;
136 int get_sensor_rotation(
const size_t index);
153 int _orb_class_instance{-1};
155 size_t _sensor_index{0};
157 size_t _sensor_count{0};
165 (ParamInt<px4::params::SENS_EN_MB12XX>) _p_sensor_enabled,
166 (ParamInt<px4::params::SENS_MB12_0_ROT>) _p_sensor0_rot,
167 (ParamInt<px4::params::SENS_MB12_1_ROT>) _p_sensor1_rot,
168 (ParamInt<px4::params::SENS_MB12_2_ROT>) _p_sensor2_rot,
169 (ParamInt<px4::params::SENS_MB12_3_ROT>) _p_sensor3_rot,
170 (ParamInt<px4::params::SENS_MB12_4_ROT>) _p_sensor4_rot,
171 (ParamInt<px4::params::SENS_MB12_5_ROT>) _p_sensor5_rot,
172 (ParamInt<px4::params::SENS_MB12_6_ROT>) _p_sensor6_rot,
173 (ParamInt<px4::params::SENS_MB12_7_ROT>) _p_sensor7_rot,
174 (ParamInt<px4::params::SENS_MB12_8_ROT>) _p_sensor8_rot,
175 (ParamInt<px4::params::SENS_MB12_9_ROT>) _p_sensor9_rot,
176 (ParamInt<px4::params::SENS_MB12_10_ROT>) _p_sensor10_rot,
177 (ParamInt<px4::params::SENS_MB12_11_ROT>) _p_sensor11_rot
183 ModuleParams(nullptr),
184 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(get_device_id()))
217 int ret_val = transfer(
nullptr, 0, &val[0], 2);
220 PX4_ERR(
"sensor %i read failed, address: 0x%02X", _sensor_index, get_device_address());
226 uint16_t distance_cm = val[0] << 8 | val[1];
227 float distance_m =
static_cast<float>(distance_cm) * 1e-2
f;
237 report.
type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
245 PX4_INFO(
"sensor %i measurement error, address 0x%02X", _sensor_index, get_device_address());
259 case 0:
return _p_sensor0_rot.get();
261 case 1:
return _p_sensor1_rot.get();
263 case 2:
return _p_sensor2_rot.get();
265 case 3:
return _p_sensor3_rot.get();
267 case 4:
return _p_sensor4_rot.get();
269 case 5:
return _p_sensor5_rot.get();
271 case 6:
return _p_sensor6_rot.get();
273 case 7:
return _p_sensor7_rot.get();
275 case 8:
return _p_sensor8_rot.get();
277 case 9:
return _p_sensor9_rot.get();
279 case 10:
return _p_sensor10_rot.get();
281 case 11:
return _p_sensor11_rot.get();
283 default:
return PX4_ERROR;
290 if (_p_sensor_enabled.get() == 0) {
291 PX4_WARN(
"disabled");
303 set_device_address(address);
311 PX4_INFO(
"sensor %i at address 0x%02X added",
_sensor_count, get_device_address());
323 PX4_ERR(
"no sensors discovered");
341 int ret_val = transfer(&cmd, 1,
nullptr, 0);
363 PX4_INFO(
"collection error");
371 PX4_INFO(
"multiple sensors are connected");
380 PX4_ERR(
"incompatible address requested");
384 PX4_INFO(
"requested address: %u", address);
386 uint8_t shifted_address = (address << 1);
389 if (transfer(cmd,
sizeof(cmd),
nullptr, 0) != PX4_OK) {
390 PX4_INFO(
"could not set the address");
393 set_device_address(address);
394 PX4_INFO(
"device address: %u", get_device_address());
402 ModuleParams::updateParams();
437 PX4_INFO(
"driver resetting");
446 if (g_dev !=
nullptr) {
448 PX4_ERR(
"address not set");
467 if (g_dev !=
nullptr) {
468 PX4_ERR(
"already started");
490 if (g_dev !=
nullptr) {
491 PX4_ERR(
"already started");
496 g_dev =
new MB12XX(i2c_bus);
498 if (g_dev ==
nullptr) {
504 if (g_dev->
init() != PX4_OK) {
513 PX4_INFO(
"driver started");
523 if (g_dev ==
nullptr) {
524 PX4_ERR(
"driver not running");
539 if (g_dev !=
nullptr) {
544 PX4_INFO(
"driver stopped");
559 PX4_ERR(
"%s open failed (try 'mb12xx start' if the driver is not running)",
MB12XX_DEVICE_PATH);
565 ssize_t sz =
read(fd, &report,
sizeof(report));
567 if (sz !=
sizeof(report)) {
568 PX4_ERR(
"immediate read failed");
572 print_message(report);
584 PX4_INFO(
"usage: mb12xx <command> [options]");
585 PX4_INFO(
"options:");
586 PX4_INFO(
"\t-a --all available busses");
588 PX4_INFO(
"\t-A --set device address (0-112, 0x00-0x70)");
589 PX4_INFO(
"command:");
590 PX4_INFO(
"\treset|set_address|start|status|stop|test|usage");
601 const char *myoptarg =
nullptr;
609 bool start_all =
false;
611 while ((ch = px4_getopt(argc, argv,
"abs:", &myoptind, &myoptarg)) != EOF) {
618 i2c_bus = atoi(myoptarg);
622 address = (uint8_t)atoi(myoptarg);
626 PX4_WARN(
"Unknown option!");
631 if (myoptind >= argc) {
636 if (!strcmp(argv[myoptind],
"reset")) {
641 if (!strcmp(argv[myoptind],
"set_address")) {
646 if (!strcmp(argv[myoptind],
"start")) {
656 if (!strcmp(argv[myoptind],
"status")) {
661 if (!strcmp(argv[myoptind],
"stop")) {
666 if (!strcmp(argv[myoptind],
"test")) {
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
measure the time elapsed performing an event
API for the uORB lightweight object broker.
#define MB12XX_SET_ADDRESS_2
__EXPORT int mb12xx_main(int argc, char *argv[])
Driver 'main' command.
void usage(const char *reason)
Print the correct usage.
int set_address(const uint8_t address=MB12XX_BASE_ADDR)
Sets a new device address.
px4::Array< uint8_t, RANGE_FINDER_MAX_SENSORS > _sensor_rotations
void print_info()
Diagnostics - print some basic information about the driver.
count the number of times an event occurs
int set_address(const uint8_t address=MB12XX_BASE_ADDR)
void stop()
Stop the automatic measurement state machine.
int measure()
Sends an i2c measure command to start the next sonar ping.
#define MB12XX_MAX_DISTANCE
High-resolution timer with callouts and timekeeping.
int get_sensor_rotation(const size_t index)
Gets the current sensor rotation value.
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
int collect()
Collects the most recent sensor measurement data from the i2c bus.
orb_advert_t _distance_sensor_topic
void perf_count(perf_counter_t handle)
Count a performance event.
int start()
Attempt to start driver on all available I2C busses.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define MB12XX_MEASURE_INTERVAL
#define MB12XX_MIN_DISTANCE
Local functions in support of the shell command.
void start()
Initialise the automatic measurement state machine and start it.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
#define MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES
#define MB12XX_TAKE_RANGE_REG
int status()
Print the driver status.
void perf_end(perf_counter_t handle)
End a performance event.
int start_bus(const int i2c_bus=MB12XX_BUS_DEFAULT)
Start the driver on a specific bus.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
static const int i2c_bus_options[]
#define MB12XX_BUS_DEFAULT
virtual int init() override
#define MB12XX_SET_ADDRESS_1
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
int orb_unadvertise(orb_advert_t handle)
int stop()
Stop the driver.
#define MB12XX_DEVICE_PATH
int usage()
Print information about the driver usage.
#define NUM_I2C_BUS_OPTIONS
perf_counter_t _sample_perf
int reset()
Reset the driver.
MB12XX(const int bus=MB12XX_BUS_DEFAULT)
void perf_begin(perf_counter_t handle)
Begin a performance event.
perf_counter_t _comms_error
__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.
px4::Array< uint8_t, RANGE_FINDER_MAX_SENSORS > _sensor_addresses
#define RANGE_FINDER_MAX_SENSORS
Performance measuring tools.
Base class for devices connected via I2C.