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

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

Browse Source 
Conflicts:
	src/drivers/px4fmu/fmu.cpp
This commit is contained in:
Thomas Gubler
2015-02-01 11:06:47 +01:00
parent 7c958a2cca a2a244584e
commit 84ff3c671d
63 changed files with 807 additions and 484 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_motor_control.c
+3 -2
View File
@@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
outputs.output[3] = motor4;
static orb_advert_t pub = 0;
if (pub == 0) {
pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
/* advertise to channel 0 / primary */
pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
}
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
@@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
fsync(ardrone_fd);
/* publish just written values */
orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
if (ret == sizeof(buf)) {
return OK;
src/drivers/drv_accel.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -81,9 +81,7 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel0);
ORB_DECLARE(sensor_accel1);
ORB_DECLARE(sensor_accel2);
ORB_DECLARE(sensor_accel);
/*
* ioctl() definitions
src/drivers/drv_baro.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -63,9 +63,7 @@ struct baro_report {
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro0);
ORB_DECLARE(sensor_baro1);
ORB_DECLARE(sensor_baro2);
ORB_DECLARE(sensor_baro);
/*
* ioctl() definitions
src/drivers/drv_blinkm.h
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/drivers/drv_gyro.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -81,9 +81,7 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro0);
ORB_DECLARE(sensor_gyro1);
ORB_DECLARE(sensor_gyro2);
ORB_DECLARE(sensor_gyro);
/*
* ioctl() definitions
src/drivers/drv_mag.h
+2 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,7 +41,6 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_device.h"
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@@ -81,15 +80,8 @@ struct mag_scale {
/*
* ObjDev tag for raw magnetometer data.
*/
ORB_DECLARE(sensor_mag0);
ORB_DECLARE(sensor_mag1);
ORB_DECLARE(sensor_mag2);
ORB_DECLARE(sensor_mag);
/*
* mag device types, for _device_id
*/
#define DRV_MAG_DEVTYPE_HMC5883 1
#define DRV_MAG_DEVTYPE_LSM303D 2
/*
* ioctl() definitions
src/drivers/drv_orb_dev.h
+7 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,7 +35,9 @@
#define _DRV_UORB_H
/**
* @file uORB published object driver.
* @file drv_orb_dev.h
*
* uORB published object driver.
*/
#include <sys/types.h>
@@ -84,4 +86,7 @@
/** Get the global advertiser handle for the topic */
#define ORBIOCGADVERTISER _ORBIOC(13)
/** Get the priority for the topic */
#define ORBIOCGPRIORITY _ORBIOC(14)
#endif /* _DRV_UORB_H */
src/drivers/drv_sensor.h
+18
View File
@@ -43,6 +43,24 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_device.h"
/**
* Sensor type definitions.
*
* Used to create a unique device id for redundant and combo sensors
*/
#define DRV_MAG_DEVTYPE_HMC5883 0x01
#define DRV_MAG_DEVTYPE_LSM303D 0x02
#define DRV_ACC_DEVTYPE_LSM303D 0x11
#define DRV_ACC_DEVTYPE_BMA180 0x12
#define DRV_ACC_DEVTYPE_MPU6000 0x13
#define DRV_GYR_DEVTYPE_MPU6000 0x21
#define DRV_GYR_DEVTYPE_L3GD20 0x22
#define DRV_RNG_DEVTYPE_MB12XX 0x31
#define DRV_RNG_DEVTYPE_LL40LS 0x32
/*
* ioctl() definitions
*
src/drivers/hil/hil.cpp
+4 -3
View File
@@ -332,8 +332,9 @@ HIL::task_main()
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
int dummy;
_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &dummy, ORB_PRIO_LOW);
pollfd fds[2];
fds[0].fd = _t_actuators;
@@ -425,7 +426,7 @@ HIL::task_main()
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
}
}
src/drivers/hmc5883/hmc5883.cpp
+8 -27
View File
@@ -69,7 +69,6 @@
#include <drivers/drv_device.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <float.h>
#include <getopt.h>
@@ -157,10 +156,9 @@ private:
float _range_ga;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _mag_topic;
orb_advert_t _subsystem_pub;
orb_id_t _mag_orb_id;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_mag_orb_id(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -419,7 +416,6 @@ HMC5883::init()
reset();
_class_instance = register_class_devname(MAG_DEVICE_PATH);
_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
ret = OK;
/* sensor is ok, but not calibrated */
@@ -850,6 +846,7 @@ HMC5883::collect()
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
@@ -902,7 +899,8 @@ HMC5883::collect()
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
@@ -925,9 +923,10 @@ HMC5883::collect()
if (_mag_topic != -1) {
/* publish it */
orb_publish(_mag_orb_id, _mag_topic, &new_report);
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(_mag_orb_id, &new_report);
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic < 0)
debug("ADVERT FAIL");
@@ -1185,24 +1184,6 @@ int HMC5883::check_calibration()
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
// XXX Change advertisement
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
if (!(_pub_blocked)) {
if (_subsystem_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}
/* return 0 if calibrated, 1 else */
src/drivers/l3gd20/l3gd20.cpp
+29 -27
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,7 +33,7 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
* Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
*
* Note: With the exception of the self-test feature, the ST L3G4200D is
* also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
@@ -214,14 +220,14 @@ private:
struct hrt_call _call;
unsigned _call_interval;
RingBuffer *_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
unsigned _current_rate;
@@ -273,6 +279,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_orb_id(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
// enable debug() calls
_debug_enabled = true;
_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
// default scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
@@ -456,20 +471,6 @@ L3GD20::init()
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_orb_id = ORB_ID(sensor_gyro2);
break;
}
reset();
measure();
@@ -478,7 +479,8 @@ L3GD20::init()
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(_orb_id, &grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
@@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
@@ -867,7 +869,7 @@ L3GD20::reset()
disable_i2c();
/* set default configuration */
write_checked_reg(ADDR_CTRL_REG1,
write_checked_reg(ADDR_CTRL_REG1,
REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
@@ -911,7 +913,7 @@ L3GD20::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -974,7 +976,7 @@ L3GD20::measure()
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
perf_count(_errors);
return;
}
#endif
@@ -994,7 +996,7 @@ L3GD20::measure()
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
@@ -1050,7 +1052,7 @@ L3GD20::measure()
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _gyro_topic, &report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
@@ -1072,7 +1074,7 @@ L3GD20::print_info()
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
src/drivers/lsm303d/lsm303d.cpp
+47 -47
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -211,6 +211,12 @@ static const int ERROR = -1;
#define LSM303D_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@@ -275,7 +281,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
@@ -295,7 +301,7 @@ private:
enum Rotation _rotation;
// values used to
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;
@@ -329,6 +335,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -507,7 +520,7 @@ private:
LSM303D *_parent;
orb_advert_t _mag_topic;
orb_id_t _mag_orb_id;
int _mag_orb_class_instance;
int _mag_class_instance;
void measure();
@@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
@@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_constant_accel_count(0),
_checked_next(0)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
// enable debug() calls
_debug_enabled = true;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
/* Prime _mag with parents devid. */
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
// default scale factors
_accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.0f;
@@ -618,7 +638,6 @@ LSM303D::init()
if (_accel_reports == nullptr)
goto out;
/* advertise accel topic */
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@@ -641,26 +660,14 @@ LSM303D::init()
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
switch (_mag->_mag_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag0);
break;
case CLASS_DEVICE_SECONDARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag1);
break;
case CLASS_DEVICE_TERTIARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic < 0) {
warnx("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
@@ -668,21 +675,8 @@ LSM303D::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic < 0) {
warnx("ADVERT ERR");
@@ -712,7 +706,7 @@ LSM303D::reset()
disable_i2c();
/* enable accel*/
write_checked_reg(ADDR_CTRL_REG1,
write_checked_reg(ADDR_CTRL_REG1,
REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
/* enable mag */
@@ -746,7 +740,7 @@ LSM303D::probe()
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
if (success) {
_checked_values[0] = WHO_I_AM;
return OK;
@@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
@@ -1424,7 +1418,7 @@ LSM303D::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -1548,7 +1542,7 @@ LSM303D::measure()
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
@@ -1570,7 +1564,7 @@ LSM303D::measure()
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
@@ -1641,7 +1635,7 @@ LSM303D::mag_measure()
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
@@ -1666,7 +1660,7 @@ LSM303D::print_info()
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_orb_id(nullptr),
_mag_orb_class_instance(-1),
_mag_class_instance(-1)
{
}
@@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
int
LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
return _parent->mag_ioctl(filp, cmd, arg);
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->mag_ioctl(filp, cmd, arg);
}
}
void
src/drivers/mkblctrl/mkblctrl.cpp
+3 -2
View File
@@ -457,8 +457,9 @@ MK::task_main()
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
int dummy;
_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &dummy, ORB_PRIO_HIGH);
/* advertise the blctrl status */
esc_status_s esc;
src/drivers/mpu6000/mpu6000.cpp
+59 -60
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -175,6 +175,12 @@
#define MPU6000_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
the MPU6000 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
@@ -234,7 +240,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
RingBuffer *_gyro_reports;
@@ -360,6 +366,13 @@ private:
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Measurement self test
*
@@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG };
/**
* Helper class implementing the gyro driver node.
@@ -457,7 +470,7 @@ protected:
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
@@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
@@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
/* Prime _gyro with parents devid. */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
@@ -596,6 +615,7 @@ MPU6000::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
@@ -613,22 +633,8 @@ MPU6000::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_accel_topic < 0) {
warnx("ADVERT FAIL");
@@ -639,22 +645,8 @@ MPU6000::init()
struct gyro_report grp;
_gyro_reports->get(&grp);
switch (_gyro->_gyro_class_instance) {
case CLASS_DEVICE_PRIMARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
break;
}
_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_gyro->_gyro_topic < 0) {
warnx("ADVERT FAIL");
@@ -683,7 +675,7 @@ int MPU6000::reset()
// for it to come out of sleep
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
@@ -741,7 +733,7 @@ int MPU6000::reset()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// default case to cope with new chip revisions, which
// presumably won't have the accel scaling bug
// presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter;
/*
/*
choose next highest filter frequency available
*/
if (frequency_hz == 0) {
@@ -921,7 +913,7 @@ MPU6000::gyro_self_test()
if (self_test())
return 1;
/*
/*
* Maximum deviation of 20 degrees, according to
* http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
* Section 6.1, initial ZRO tolerance
@@ -982,7 +974,7 @@ MPU6000::factory_self_test()
// gyro self test has to be done at 250DPS
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
struct MPUReport mpu_report;
struct MPUReport mpu_report;
float accel_baseline[3];
float gyro_baseline[3];
float accel[3];
@@ -1012,10 +1004,10 @@ MPU6000::factory_self_test()
}
#if 1
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
BITS_GYRO_ST_Z);
// accel 8g, self-test enabled all axes
@@ -1085,7 +1077,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
}
}
for (uint8_t i=0; i<3; i++) {
float diff = gyro[i]-gyro_baseline[i];
float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@@ -1100,7 +1092,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
}
}
write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@@ -1247,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
@@ -1536,13 +1528,13 @@ MPU6000::check_registers(void)
the data registers.
*/
uint8_t v;
if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@@ -1655,7 +1647,7 @@ MPU6000::measure()
_register_wait--;
return;
}
/*
* Swap axes and negate y
@@ -1716,7 +1708,7 @@ MPU6000::measure()
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
@@ -1737,7 +1729,7 @@ MPU6000::measure()
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@@ -1763,12 +1755,12 @@ MPU6000::measure()
perf_begin(_controller_latency_perf);
perf_begin(_system_latency_perf);
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &arb);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@@ -1791,7 +1783,7 @@ MPU6000::print_info()
for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@@ -1818,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
@@ -1863,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
return _parent->gyro_ioctl(filp, cmd, arg);
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);
}
}
/**
src/drivers/ms5611/ms5611.cpp
+15 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -57,6 +57,7 @@
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
@@ -134,8 +135,7 @@ protected:
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
perf_counter_t _sample_perf;
@@ -171,6 +171,13 @@ protected:
*/
void cycle();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
@@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
_SENS(0),
_msl_pressure(101325),
_baro_topic(-1),
_orb_class_instance(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@@ -263,7 +271,6 @@ MS5611::init()
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
@@ -303,7 +310,9 @@ MS5611::init()
ret = OK;
_baro_topic = orb_advertise(_orb_id, &brp);
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic < 0) {
warnx("failed to create sensor_baro publication");
@@ -725,7 +734,7 @@ MS5611::collect()
/* publish it */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _baro_topic, &report);
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {
src/drivers/px4fmu/fmu.cpp
+4 -6
View File
@@ -145,7 +145,7 @@ private:
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _actuator_output_topic;
int _actuator_output_topic_instance;
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
@@ -260,7 +260,7 @@ PX4FMU::PX4FMU() :
_groups_required(0),
_groups_subscribed(0),
_control_subs{-1},
_actuator_output_topic(nullptr),
_actuator_output_topic_instance(-1),
_poll_fds_num(0),
_pwm_limit{},
_failsafe_pwm{0},
@@ -331,8 +331,6 @@ PX4FMU::init()
log("default PWM output device");
}
_actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance);
/* reset GPIOs */
gpio_reset();
@@ -683,10 +681,10 @@ PX4FMU::task_main()
/* publish mixed control outputs */
if (_outputs_pub < 0) {
_outputs_pub = orb_advertise(_actuator_output_topic, &outputs);
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(_actuator_output_topic, _outputs_pub, &outputs);
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
}
src/drivers/px4io/px4io.cpp
+4 -9
View File
@@ -1685,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs()
/* lazily advertise on first publication */
if (_to_outputs == 0) {
_to_outputs = orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
&outputs);
int instance;
_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &instance, ORB_PRIO_MAX);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
_to_outputs,
&outputs);
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
}
return OK;