Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 10:46:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

hmc5883: fixed handling of 3 bus options

Browse Source 
use a table of possible bus options. This prevents us starting two
drivers on the same bus
This commit is contained in:
Andrew Tridgell
2015-01-09 17:23:32 +11:00
parent e4a3c3f76d
commit 54a22aed94
2 changed files with 123 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/hmc5883/hmc5883.cpp
+122 -187
View File
@@ -80,9 +80,6 @@
* HMC5883 internal constants and data structures. * HMC5883 internal constants and data structures.
*/ */
#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int"
#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@@ -114,9 +111,10 @@
#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */
enum HMC5883_BUS { enum HMC5883_BUS {
HMC5883_BUS_ALL, HMC5883_BUS_ALL = 0,
HMC5883_BUS_INTERNAL, HMC5883_BUS_I2C_INTERNAL,
HMC5883_BUS_EXTERNAL HMC5883_BUS_I2C_EXTERNAL,
HMC5883_BUS_SPI
}; };
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
@@ -1297,17 +1295,70 @@ namespace hmc5883
#endif #endif
const int ERROR = -1; const int ERROR = -1;
HMC5883 *g_dev_int = nullptr; /*
HMC5883 *g_dev_ext = nullptr; list of supported bus configurations
*/
struct hmc5883_bus_option {
enum HMC5883_BUS busid;
const char *devpath;
HMC5883_constructor interface_constructor;
uint8_t busnum;
HMC5883 *dev;
} bus_options[] = {
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
#ifdef PX4_I2C_BUS_ONBOARD
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
#endif
#ifdef PX4_SPIDEV_HMC
{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(int bus, enum Rotation rotation); void start(enum HMC5883_BUS busid, enum Rotation rotation);
void test(int bus); bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation);
void reset(int bus); struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid);
int info(int bus); void test(enum HMC5883_BUS busid);
int calibrate(int bus); void reset(enum HMC5883_BUS busid);
const char* get_path(int bus); int info(enum HMC5883_BUS busid);
int calibrate(enum HMC5883_BUS busid);
void usage(); void usage();
/**
* start driver for a specific bus option
*/
bool
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
{
if (bus.dev != nullptr)
errx(1,"bus option already started");
device::Device *interface = bus.interface_constructor(bus.busnum);
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new HMC5883(interface, bus.devpath, rotation);
if (bus.dev != nullptr && OK != bus.dev->init()) {
delete bus.dev;
bus.dev = NULL;
return false;
}
int fd = open(bus.devpath, O_RDONLY);
if (fd < 0)
return false;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
errx(1,"Failed to setup poll rate");
}
return true;
}
/** /**
* Start the driver. * Start the driver.
* *
@@ -1315,151 +1366,58 @@ void usage();
* is either successfully up and running or failed to start. * is either successfully up and running or failed to start.
*/ */
void void
start(int external_bus, enum Rotation rotation) start(enum HMC5883_BUS busid, enum Rotation rotation)
{ {
int fd; uint8_t i;
bool started = false;
/* create the driver, attempt expansion bus first */ for (i=0; i<NUM_BUS_OPTIONS; i++) {
if (g_dev_ext != nullptr) { if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
warnx("already started external"); // this device is already started
} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { continue;
device::Device *interface = nullptr;
/* create the driver, only attempt I2C for the external bus */
if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION);
if (interface->init() != OK) {
delete interface;
interface = nullptr;
warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION);
}
} }
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
#ifdef PX4_I2C_BUS_ONBOARD // not the one that is asked for
if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { continue;
interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
if (interface->init() != OK) {
delete interface;
interface = nullptr;
warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD);
}
}
#endif
/* interface will be null if init failed */
if (interface != nullptr) {
g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
}
}
/* if this failed, attempt onboard sensor */
if (g_dev_int != nullptr) {
warnx("already started internal");
} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) {
device::Device *interface = nullptr;
/* create the driver, try SPI first, fall back to I2C if unsuccessful */
#ifdef PX4_SPIDEV_HMC
if (HMC5883_SPI_interface != nullptr) {
interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS);
}
#endif
#ifdef PX4_I2C_BUS_ONBOARD
/* this device is already connected as external if present above */
if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD);
}
#endif
if (interface == nullptr) {
warnx("no internal bus scanned");
goto fail;
}
if (interface->init() != OK) {
delete interface;
warnx("no device on internal bus");
} else {
g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (external_bus == HMC5883_BUS_INTERNAL) {
goto fail;
}
}
if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) {
goto fail;
}
} }
started |= start_bus(bus_options[i], rotation);
} }
if (g_dev_int == nullptr && g_dev_ext == nullptr) if (!started)
goto fail; errx(1, "driver start failed");
/* set the poll rate to default, starts automatic data collection */
if (g_dev_int != nullptr) {
fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
}
if (g_dev_ext != nullptr) {
fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
}
// one or more drivers started OK
exit(0); exit(0);
fail:
if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
errx(1, "driver start failed");
} }
/**
* find a bus structure for a busid
*/
struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
{
for (uint8_t i=0; i<NUM_BUS_OPTIONS; i++) {
if ((busid == HMC5883_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
errx(1,"bus %u not started", (unsigned)busid);
}
/** /**
* Perform some basic functional tests on the driver; * Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled * make sure we can collect data from the sensor in polled
* and automatic modes. * and automatic modes.
*/ */
void void
test(int bus) test(enum HMC5883_BUS busid)
{ {
struct hmc5883_bus_option &bus = find_bus(busid);
struct mag_report report; struct mag_report report;
ssize_t sz; ssize_t sz;
int ret; int ret;
const char *path = get_path(bus); const char *path = bus.devpath;
int fd = open(path, O_RDONLY); int fd = open(path, O_RDONLY);
@@ -1557,10 +1515,11 @@ test(int bus)
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
* Using the self test method described above, the user can scale sensor * Using the self test method described above, the user can scale sensor
*/ */
int calibrate(int bus) int calibrate(enum HMC5883_BUS busid)
{ {
int ret; int ret;
const char *path = get_path(bus); struct hmc5883_bus_option &bus = find_bus(busid);
const char *path = bus.devpath;
int fd = open(path, O_RDONLY); int fd = open(path, O_RDONLY);
@@ -1585,9 +1544,10 @@ int calibrate(int bus)
* Reset the driver. * Reset the driver.
*/ */
void void
reset(int bus) reset(enum HMC5883_BUS busid)
{ {
const char *path = get_path(bus); struct hmc5883_bus_option &bus = find_bus(busid);
const char *path = bus.devpath;
int fd = open(path, O_RDONLY); int fd = open(path, O_RDONLY);
@@ -1607,28 +1567,13 @@ reset(int bus)
* Print a little info about the driver. * Print a little info about the driver.
*/ */
int int
info(int bus) info(enum HMC5883_BUS busid)
{ {
int ret = 1; struct hmc5883_bus_option &bus = find_bus(busid);
HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
if (g_dev == nullptr) { bus.dev->print_info();
warnx("not running on bus %d", bus); exit(0);
} else {
warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard"));
g_dev->print_info();
ret = 0;
}
return ret;
}
const char*
get_path(int bus)
{
return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT);
} }
void void
@@ -1650,22 +1595,25 @@ int
hmc5883_main(int argc, char *argv[]) hmc5883_main(int argc, char *argv[])
{ {
int ch; int ch;
int bus = HMC5883_BUS_ALL; enum HMC5883_BUS busid = HMC5883_BUS_ALL;
enum Rotation rotation = ROTATION_NONE; enum Rotation rotation = ROTATION_NONE;
bool calibrate = false; bool calibrate = false;
while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { while ((ch = getopt(argc, argv, "XISR:C")) != EOF) {
switch (ch) { switch (ch) {
case 'R': case 'R':
rotation = (enum Rotation)atoi(optarg); rotation = (enum Rotation)atoi(optarg);
break; break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
case 'I': case 'I':
bus = HMC5883_BUS_INTERNAL; busid = HMC5883_BUS_I2C_INTERNAL;
break; break;
#endif #endif
case 'X': case 'X':
bus = HMC5883_BUS_EXTERNAL; busid = HMC5883_BUS_I2C_EXTERNAL;
break;
case 'S':
busid = HMC5883_BUS_SPI;
break; break;
case 'C': case 'C':
calibrate = true; calibrate = true;
@@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[])
* Start/load the driver. * Start/load the driver.
*/ */
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
hmc5883::start(bus, rotation); hmc5883::start(busid, rotation);
if (calibrate) { if (calibrate) {
if (hmc5883::calibrate(bus) == 0) { if (hmc5883::calibrate(busid) == 0) {
errx(0, "calibration successful"); errx(0, "calibration successful");
} else { } else {
@@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[])
* Test the driver/device. * Test the driver/device.
*/ */
if (!strcmp(verb, "test")) if (!strcmp(verb, "test"))
hmc5883::test(bus); hmc5883::test(busid);
/* /*
* Reset the driver. * Reset the driver.
*/ */
if (!strcmp(verb, "reset")) if (!strcmp(verb, "reset"))
hmc5883::reset(bus); hmc5883::reset(busid);
/* /*
* Print driver information. * Print driver information.
*/ */
if (!strcmp(verb, "info") || !strcmp(verb, "status")) { if (!strcmp(verb, "info") || !strcmp(verb, "status"))
if (bus == HMC5883_BUS_ALL) { hmc5883::info(busid);
int ret = 0;
if (hmc5883::info(HMC5883_BUS_INTERNAL)) {
ret = 1;
}
if (hmc5883::info(HMC5883_BUS_EXTERNAL)) {
ret = 1;
}
exit(ret);
} else {
exit(hmc5883::info(bus));
}
}
/* /*
* Autocalibrate the scaling * Autocalibrate the scaling
*/ */
if (!strcmp(verb, "calibrate")) { if (!strcmp(verb, "calibrate")) {
if (hmc5883::calibrate(bus) == 0) { if (hmc5883::calibrate(busid) == 0) {
errx(0, "calibration successful"); errx(0, "calibration successful");
} else { } else {
src/drivers/hmc5883/hmc5883.h
+1
View File
@@ -50,3 +50,4 @@
/* interface factories */ /* interface factories */
extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_SPI_interface(int bus) weak_function;
extern device::Device *HMC5883_I2C_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function;
typedef device::Device* (*HMC5883_constructor)(int);