dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'redundant_sensors' of github.com:PX4/Firmware

This commit is contained in:
Lorenz Meier 2013-12-10 11:37:25 +01:00
parent b2b9665e44 c72162cc5a
commit eed5b99a4a
4 changed files with 80 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/drivers/hmc5883/hmc5883.cpp
View File

@ -77,6 +77,7 @@
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
/* 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 */
@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
@ -1256,7 +1257,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
@ -1288,7 +1289,7 @@ test()
ssize_t sz;
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);

7
src/drivers/l3gd20/l3gd20.cpp
View File

@ -365,6 +365,13 @@ L3GD20::init()
if (_reports == nullptr)
goto out;
/* try to claim the generic accel node as well - it's OK if we fail at this */
ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default gyro device");
}
/* advertise sensor topic */
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));

53
src/drivers/lsm303d/lsm303d.cpp
View File

@ -80,7 +80,8 @@ static const int ERROR = -1;
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
/* register addresses: A: accel, M: mag, T: temp */
#define ADDR_WHO_AM_I 0x0F
@ -583,6 +584,40 @@ LSM303D::init()
reset();
/* register the first instance as plain name, the 2nd as two and counting */
ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default accel device");
} else {
unsigned instance = 1;
do {
char name[32];
sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance);
ret = register_driver(name, &fops, 0666, (void *)this);
instance++;
} while (ret);
}
/* try to claim the generic accel node as well - it's OK if we fail at this */
mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this);
if (mag_ret == OK) {
log("default mag device");
} else {
unsigned instance = 1;
do {
char name[32];
sprintf(name, "%s%d", MAG_DEVICE_PATH, instance);
ret = register_driver(name, &fops, 0666, (void *)this);
instance++;
} while (ret);
}
/* advertise mag topic */
struct mag_report zero_mag_report;
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
@ -1638,7 +1673,7 @@ LSM303D::toggle_logging()
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", MAG_DEVICE_PATH),
CDev("LSM303D_mag", "/dev/lsm303d_mag"),
_parent(parent)
{
}
@ -1704,7 +1739,7 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
@ -1715,7 +1750,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1723,7 +1758,7 @@ start()
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
@ -1758,10 +1793,10 @@ test()
int ret;
/* get the driver */
fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0)
err(1, "%s open failed", ACCEL_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(accel_report));
@ -1787,10 +1822,10 @@ test()
struct mag_report m_report;
/* get the driver */
fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0)
err(1, "%s open failed", MAG_DEVICE_PATH);
err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)

33
src/drivers/mpu6000/mpu6000.cpp
View File

@ -75,6 +75,9 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
@ -359,7 +362,7 @@ private:
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
@ -468,6 +471,20 @@ MPU6000::init()
/* fetch an initial set of measurements for advertisement */
measure();
/* try to claim the generic accel node as well - it's OK if we fail at this */
ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default accel device");
}
/* try to claim the generic accel node as well - it's OK if we fail at this */
ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default gyro device");
}
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
@ -1290,7 +1307,7 @@ MPU6000::print_info()
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
_parent(parent)
{
}
@ -1352,7 +1369,7 @@ start()
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
goto fail;
@ -1384,17 +1401,17 @@ test()
ssize_t sz;
/* get the driver */
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
MPU_DEVICE_PATH_ACCEL);
/* get the driver */
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -1452,7 +1469,7 @@ test()
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
err(1, "failed ");