fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 06:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into ros

Browse Source 
Conflicts:
	src/platforms/px4_middleware.h
This commit is contained in:
Thomas Gubler
2015-01-15 12:42:28 +01:00
parent e60c1a842c e62bd37e73
commit 96db9e8188
26 changed files with 351 additions and 283 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.
*/
#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 */
#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 */
enum HMC5883_BUS {
HMC5883_BUS_ALL,
HMC5883_BUS_INTERNAL,
HMC5883_BUS_EXTERNAL
HMC5883_BUS_ALL = 0,
HMC5883_BUS_I2C_INTERNAL,
HMC5883_BUS_I2C_EXTERNAL,
HMC5883_BUS_SPI
};
/* oddly, ERROR is not defined for c++ */
@@ -1297,17 +1295,70 @@ namespace hmc5883
#endif
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 test(int bus);
void reset(int bus);
int info(int bus);
int calibrate(int bus);
const char* get_path(int bus);
void start(enum HMC5883_BUS busid, enum Rotation rotation);
bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation);
struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid);
void test(enum HMC5883_BUS busid);
void reset(enum HMC5883_BUS busid);
int info(enum HMC5883_BUS busid);
int calibrate(enum HMC5883_BUS busid);
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.
*
@@ -1315,151 +1366,58 @@ void usage();
* is either successfully up and running or failed to start.
*/
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 */
if (g_dev_ext != nullptr) {
warnx("already started external");
} else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) {
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);
}
for (i=0; i<NUM_BUS_OPTIONS; i++) {
if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
#ifdef PX4_I2C_BUS_ONBOARD
if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) {
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;
}
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation);
}
if (g_dev_int == nullptr && g_dev_ext == nullptr)
goto fail;
/* 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);
}
if (!started)
errx(1, "driver start failed");
// one or more drivers started OK
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;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(int bus)
test(enum HMC5883_BUS busid)
{
struct hmc5883_bus_option &bus = find_bus(busid);
struct mag_report report;
ssize_t sz;
int ret;
const char *path = get_path(bus);
const char *path = bus.devpath;
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.
* Using the self test method described above, the user can scale sensor
*/
int calibrate(int bus)
int calibrate(enum HMC5883_BUS busid)
{
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);
@@ -1585,9 +1544,10 @@ int calibrate(int bus)
* Reset the driver.
*/
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);
@@ -1607,28 +1567,13 @@ reset(int bus)
* Print a little info about the driver.
*/
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);
if (g_dev == nullptr) {
warnx("not running on bus %d", bus);
} 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);
warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
bus.dev->print_info();
exit(0);
}
void
@@ -1650,22 +1595,25 @@ int
hmc5883_main(int argc, char *argv[])
{
int ch;
int bus = HMC5883_BUS_ALL;
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
while ((ch = getopt(argc, argv, "XISR:C")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
case 'I':
bus = HMC5883_BUS_INTERNAL;
busid = HMC5883_BUS_I2C_INTERNAL;
break;
#endif
case 'X':
bus = HMC5883_BUS_EXTERNAL;
busid = HMC5883_BUS_I2C_EXTERNAL;
break;
case 'S':
busid = HMC5883_BUS_SPI;
break;
case 'C':
calibrate = true;
@@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
hmc5883::start(bus, rotation);
hmc5883::start(busid, rotation);
if (calibrate) {
if (hmc5883::calibrate(bus) == 0) {
if (hmc5883::calibrate(busid) == 0) {
errx(0, "calibration successful");
} else {
@@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[])
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
hmc5883::test(bus);
hmc5883::test(busid);
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
hmc5883::reset(bus);
hmc5883::reset(busid);
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
if (bus == HMC5883_BUS_ALL) {
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));
}
}
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
hmc5883::info(busid);
/*
* Autocalibrate the scaling
*/
if (!strcmp(verb, "calibrate")) {
if (hmc5883::calibrate(bus) == 0) {
if (hmc5883::calibrate(busid) == 0) {
errx(0, "calibration successful");
} else {
src/drivers/hmc5883/hmc5883.h
+1
View File
@@ -50,3 +50,4 @@
/* interface factories */
extern device::Device *HMC5883_SPI_interface(int bus) weak_function;
extern device::Device *HMC5883_I2C_interface(int bus) weak_function;
typedef device::Device* (*HMC5883_constructor)(int);
src/drivers/mpu6000/mpu6000.cpp
+7
View File
@@ -251,6 +251,8 @@ private:
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _system_latency_perf;
perf_counter_t _controller_latency_perf;
uint8_t _register_wait;
uint64_t _reset_wait;
@@ -491,6 +493,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")),
_system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
_reset_wait(0),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -1731,6 +1735,9 @@ MPU6000::measure()
_gyro->parent_poll_notify();
if (!(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
perf_begin(_system_latency_perf);
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &arb);
}
src/drivers/px4io/px4io.cpp
+6 -10
View File
@@ -260,9 +260,9 @@ private:
int _mavlink_fd; ///< mavlink file descriptor.
perf_counter_t _perf_update; ///<local performance counter for status updates
perf_counter_t _perf_write; ///<local performance counter for PWM control writes
perf_counter_t _perf_chan_count; ///<local performance counter for channel number changes
perf_counter_t _perf_update; ///< local performance counter for status updates
perf_counter_t _perf_write; ///< local performance counter for PWM control writes
perf_counter_t _perf_sample_latency; ///< total system latency (based on passed-through timestamp)
/* cached IO state */
uint16_t _status; ///< Various IO status flags
@@ -493,7 +493,7 @@ PX4IO::PX4IO(device::Device *interface) :
_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
_status(0),
_alarms(0),
_t_actuator_controls_0(-1),
@@ -551,7 +551,7 @@ PX4IO::~PX4IO()
/* deallocate perfs */
perf_free(_perf_update);
perf_free(_perf_write);
perf_free(_perf_chan_count);
perf_free(_perf_sample_latency);
g_dev = nullptr;
}
@@ -1111,6 +1111,7 @@ PX4IO::io_set_control_state(unsigned group)
if (changed) {
orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls);
perf_set(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample));
}
}
break;
@@ -1570,11 +1571,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
channel_count = RC_INPUT_MAX_CHANNELS;
}
/* count channel count changes to identify signal integrity issues */
if (channel_count != _rc_chan_count) {
perf_count(_perf_chan_count);
}
_rc_chan_count = channel_count;
input_rc.timestamp_publication = hrt_absolute_time();