Skip to content

Commit

Permalink
Fix lis3mdl handling when multiple sensors are available
Browse files Browse the repository at this point in the history 
Fixes PX4#10740
  • Loading branch information
Hannes Diethelm committed Oct 24, 2018
1 parent d2e2b5e commit e277f4c
Show file tree
Hide file tree
Showing 3 changed files with 142 additions and 118 deletions.
2 changes: 1 addition & 1 deletion src/drivers/magnetometer/lis3mdl/lis3mdl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@
/* Max measurement rate is 80Hz */
#define LIS3MDL_CONVERSION_INTERVAL (1000000 / 80) /* 12,500 microseconds */

#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
#define NUM_BUS_OPTIONS (sizeof(lis3mdl::bus_options)/sizeof(lis3mdl::bus_options[0]))

#define ADDR_WHO_AM_I 0x0f
#define ID_WHO_AM_I 0x3d
Expand Down
237 changes: 133 additions & 104 deletions src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,12 +46,13 @@
extern "C" __EXPORT int lis3mdl_main(int argc, char *argv[]);

int
lis3mdl::calibrate(LIS3MDL_BUS bus_id)
lis3mdl::calibrate(struct lis3mdl_bus_option &bus)
{
int ret;
struct lis3mdl_bus_option &bus = find_bus(bus_id);
const char *path = bus.devpath;

PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath);

int fd = open(path, O_RDONLY);

if (fd < 0) {
Expand All @@ -69,55 +70,52 @@ lis3mdl::calibrate(LIS3MDL_BUS bus_id)
}

int
lis3mdl::info(LIS3MDL_BUS bus_id)
lis3mdl::info(struct lis3mdl_bus_option &bus)
{
struct lis3mdl_bus_option &bus = find_bus(bus_id);

PX4_WARN("running on bus: %u (%s)\n", (unsigned)bus.bus_id, bus.devpath);
PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath);
bus.dev->print_info();
return 1;
return 0;
}

bool
lis3mdl::init(LIS3MDL_BUS bus_id)
int
lis3mdl::init(struct lis3mdl_bus_option &bus)
{
struct lis3mdl_bus_option &bus = find_bus(bus_id);
const char *path = bus.devpath;

int fd = open(path, O_RDONLY);

if (fd < 0) {
return false;
return 1;
}

if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1, "Failed to setup poll rate");
return false;
return 1;

} else {
PX4_INFO("Poll rate set to max (80hz)");
}

close(fd);

return true;
return 0;
}

bool
int
lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation)
{
if (bus.dev != nullptr) {
errx(1, "bus option already started");
return false;
return 1;
}

device::Device *interface = bus.interface_constructor(bus.busnum);

if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.bus_id);
return false;
return 1;
}

bus.dev = new LIS3MDL(interface, bus.devpath, rotation);
Expand All @@ -126,61 +124,49 @@ lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation)
bus.dev->init() != OK) {
delete bus.dev;
bus.dev = NULL;
return false;
return 1;
}

return true;
return 0;
}

int
lis3mdl::start(LIS3MDL_BUS bus_id, Rotation rotation)
lis3mdl::start(struct lis3mdl_bus_option &bus, Rotation rotation)
{
bool started = false;
if (bus.dev == NULL) {
return start_bus(bus, rotation);

for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_id == LIS3MDL_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}

if (bus_id != LIS3MDL_BUS_ALL && bus_options[i].bus_id != bus_id) {
// not the one that is asked for
continue;
}

started |= start_bus(bus_options[i], rotation);
//init(bus_id);
} else {
// this device is already started
return 1;
}

return started;
}

int
lis3mdl::stop()
lis3mdl::stop(struct lis3mdl_bus_option &bus)
{
bool stopped = false;

for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_options[i].dev != nullptr) {
bus_options[i].dev->stop();
delete bus_options[i].dev;
bus_options[i].dev = nullptr;
stopped = true;
}
}
if (bus.dev != NULL) {
bus.dev->stop();
delete bus.dev;
bus.dev = nullptr;
return 0;

return !stopped;
} else {
// this device is already stopped
return 1;
}
}

bool
lis3mdl::test(LIS3MDL_BUS bus_id)
int
lis3mdl::test(struct lis3mdl_bus_option &bus)
{
struct lis3mdl_bus_option &bus = find_bus(bus_id);
struct mag_report report;
ssize_t sz;
int ret;
const char *path = bus.devpath;

PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath);

int fd = open(path, O_RDONLY);

if (fd < 0) {
Expand Down Expand Up @@ -243,15 +229,16 @@ lis3mdl::test(LIS3MDL_BUS bus_id)
}

PX4_INFO("PASS");
return 1;
return 0;
}

bool
lis3mdl::reset(LIS3MDL_BUS bus_id)
int
lis3mdl::reset(struct lis3mdl_bus_option &bus)
{
struct lis3mdl_bus_option &bus = find_bus(bus_id);
const char *path = bus.devpath;

PX4_INFO("running on bus: %u (%s)", (unsigned)bus.bus_id, bus.devpath);

int fd = open(path, O_RDONLY);

if (fd < 0) {
Expand Down Expand Up @@ -280,6 +267,7 @@ lis3mdl::usage()
PX4_WARN(" -R rotation");
PX4_WARN(" -C calibrate on start");
PX4_WARN(" -X only external bus");
PX4_WARN(" -S only spi bus");
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS)
PX4_WARN(" -I only internal bus");
#endif
Expand Down Expand Up @@ -332,74 +320,115 @@ lis3mdl_main(int argc, char *argv[])
}

const char *verb = argv[myoptind];
int ret;
bool dev_found = false;
bool cmd_found = false;

// Start/load the driver
if (!strcmp(verb, "start")) {
// Start/load the driver

if (lis3mdl::start(bus_id, rotation)) {
if (calibrate) {
if (OK != lis3mdl::calibrate(bus_id)) {
PX4_WARN("calibration failed");
return 1;
}
cmd_found = true;
ret = 1; // default: failed, will be set to success if one start succeeds

for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_id != LIS3MDL_BUS_ALL && bus_id != lis3mdl::bus_options[i].bus_id) {
// not the one that is asked for
continue;
}

lis3mdl::init(bus_id);
dev_found = true;

return 0;
// Start/load the driver
if (lis3mdl::start(lis3mdl::bus_options[i], rotation) == OK) {
if (calibrate) {
if (lis3mdl::calibrate(lis3mdl::bus_options[i]) != OK) {
PX4_WARN("calibration failed");
lis3mdl::stop(lis3mdl::bus_options[i]); //Stop, failed

} else {
return 1;
} else {
lis3mdl::init(lis3mdl::bus_options[i]);
ret = 0; // one succeed
}

} else {
lis3mdl::init(lis3mdl::bus_options[i]);
ret = 0; // one succeed
}
}
}
}

// Stop the driver
if (!strcmp(verb, "stop")) {
return lis3mdl::stop();
}
} else {
// Other commands

// Test the driver/device
if (!strcmp(verb, "test")) {
return lis3mdl::test(bus_id);
}
ret = 0; // default: success, will be set to failed if one action fails

// Reset the driver
if (!strcmp(verb, "reset")) {
return lis3mdl::reset(bus_id);
}
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (bus_id != LIS3MDL_BUS_ALL && bus_id != lis3mdl::bus_options[i].bus_id) {
// not the one that is asked for
continue;
}

// Print driver information
if (!strcmp(verb, "info") ||
!strcmp(verb, "status")) {
return lis3mdl::info(bus_id);
}
if (lis3mdl::bus_options[i].dev == NULL) {
if (bus_id != LIS3MDL_BUS_ALL) {
PX4_ERR("bus %u not started", (unsigned)bus_id);
return 1;

// Autocalibrate the scaling
if (!strcmp(verb, "calibrate")) {
if (lis3mdl::calibrate(bus_id) == 0) {
PX4_INFO("calibration successful");
return 0;
} else {
continue;
}
}

} else {
PX4_INFO("calibration failed");
return 1;
}
dev_found = true;

}
// Stop the driver
if (!strcmp(verb, "stop")) {
cmd_found = true;
ret |= lis3mdl::stop(lis3mdl::bus_options[i]);
}

PX4_INFO("unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'");
return 1;
}
// Test the driver/device
if (!strcmp(verb, "test")) {
cmd_found = true;
ret |= lis3mdl::test(lis3mdl::bus_options[i]);
}

struct
lis3mdl::lis3mdl_bus_option &lis3mdl::find_bus(LIS3MDL_BUS bus_id)
{
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((bus_id == LIS3MDL_BUS_ALL ||
bus_id == bus_options[i].bus_id) && bus_options[i].dev != NULL) {
return bus_options[i];
// Reset the driver
if (!strcmp(verb, "reset")) {
cmd_found = true;
ret |= lis3mdl::reset(lis3mdl::bus_options[i]);
}

// Print driver information
if (!strcmp(verb, "info") ||
!strcmp(verb, "status")) {
cmd_found = true;
ret |= lis3mdl::info(lis3mdl::bus_options[i]);
}

// Autocalibrate the scaling
if (!strcmp(verb, "calibrate")) {
cmd_found = true;

if (lis3mdl::calibrate(lis3mdl::bus_options[i]) == OK) {
PX4_INFO("calibration successful");

} else {
PX4_WARN("calibration failed");
ret = 1;
}
}
}
}

errx(1, "bus %u not started", (unsigned)bus_id);
if (!dev_found) {
PX4_WARN("no device found, please start driver first");
return 1;

} else if (!cmd_found) {
PX4_WARN("unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'");
return 1;

} else {
return ret;
}
}
Loading

0 comments on commit e277f4c

Please sign in to comment.