zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: implement up to two sensors on PX4

mission-4.1.18
Andrew Tridgell 11 years ago
parent
2753449e75
commit
d9b6f7f0f7
2 changed files with 160 additions and 110 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 225
      libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
  2. 45
      libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

225
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
Unescape View File

@ -17,6 +17,25 @@ const extern AP_HAL::HAL& hal; @@ -17,6 +17,25 @@ const extern AP_HAL::HAL& hal;
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
{
// assumes max 2 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
_gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
if (_accel_fd[0] < 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
}
if (_gyro_fd[0] < 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
if (_accel_fd[1] >= 0) {
_num_accel_instances = 2;
}
if (_gyro_fd[1] >= 0) {
_num_gyro_instances = 2;
}
switch (sample_rate) {
case RATE_50HZ:
_default_filter_hz = 15;
@ -33,33 +52,6 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) @@ -33,33 +52,6 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
break;
}
_delta_time = _sample_time_usec * 1.0e-6f;
// init accelerometers
_accel_fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (_accel_fd < 0) {
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
}
_gyro_fd = open(GYRO_DEVICE_PATH, O_RDONLY);
if (_gyro_fd < 0) {
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
uint32_t driver_rate = 1000;
#else
uint32_t driver_rate = 800;
#endif
/*
* set the accel and gyro sampling rate.
*/
ioctl(_accel_fd, ACCELIOCSSAMPLERATE, driver_rate);
ioctl(_accel_fd, SENSORIOCSPOLLRATE, driver_rate);
ioctl(_gyro_fd, GYROIOCSSAMPLERATE, driver_rate);
ioctl(_gyro_fd, SENSORIOCSPOLLRATE, driver_rate);
_set_filter_frequency(_mpu6000_filter);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
@ -77,33 +69,103 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz) @@ -77,33 +69,103 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0) {
filter_hz = _default_filter_hz;
}
ioctl(_gyro_fd, GYROIOCSLOWPASS, filter_hz);
ioctl(_accel_fd, ACCELIOCSLOWPASS, filter_hz);
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
ioctl(_gyro_fd[i], GYROIOCSLOWPASS, filter_hz);
ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz);
}
}
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
bool AP_InertialSensor_PX4::update(void)
// multi-device interface
bool AP_InertialSensor_PX4::get_gyro_instance_health(uint8_t instance) const
{
if (instance >= _num_gyro_instances) {
return false;
}
if (_sample_time_usec == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_gyro_timestamp[instance]) > 2*_sample_time_usec) {
// gyros have not updated
return false;
}
return true;
}
uint8_t AP_InertialSensor_PX4::get_gyro_count(void) const
{
return _num_gyro_instances;
}
bool AP_InertialSensor_PX4::get_gyro_instance(uint8_t instance, Vector3f &gyro) const
{
Vector3f accel_scale = _accel_scale.get();
if (instance >= _num_gyro_instances) {
return false;
}
gyro = _gyro_in[instance];
gyro.rotate(_board_orientation);
gyro -= _gyro_offset;
return true;
}
bool AP_InertialSensor_PX4::get_accel_instance_health(uint8_t instance) const
{
if (instance >= _num_accel_instances) {
return false;
}
if (_sample_time_usec == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp[instance]) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if (fabsf(_accel.x) > 30 && fabsf(_accel.y) > 30 && fabsf(_accel.z) > 30 &&
(_previous_accels[instance] - _accel_in[instance]).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
}
return true;
}
uint8_t AP_InertialSensor_PX4::get_accel_count(void) const
{
return _num_accel_instances;
}
bool AP_InertialSensor_PX4::get_accel_instance(uint8_t instance, Vector3f &accel) const
{
if (instance >= _num_accel_instances) {
return false;
}
accel = _accel_in[instance];
accel.rotate(_board_orientation);
accel.x *= _accel_scale.get().x;
accel.y *= _accel_scale.get().y;
accel.z *= _accel_scale.get().z;
accel -= _accel_offset;
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
_previous_accel = _accel;
_accel = _accel_in;
_gyro = _gyro_in;
// add offsets and rotation
_accel.rotate(_board_orientation);
_accel.x *= accel_scale.x;
_accel.y *= accel_scale.y;
_accel.z *= accel_scale.z;
_accel -= _accel_offset;
_gyro.rotate(_board_orientation);
_gyro -= _gyro_offset;
get_accel_instance(0, _accel);
get_gyro_instance(0, _gyro);
if (_last_filter_hz != _mpu6000_filter) {
_set_filter_frequency(_mpu6000_filter);
@ -117,38 +179,39 @@ bool AP_InertialSensor_PX4::update(void) @@ -117,38 +179,39 @@ bool AP_InertialSensor_PX4::update(void)
float AP_InertialSensor_PX4::get_delta_time(void)
{
return _delta_time;
return _sample_time_usec * 1.0e-6f;
}
float AP_InertialSensor_PX4::get_gyro_drift_rate(void)
{
// 0.5 degrees/second/minute
// assume 0.5 degrees/second/minute
return ToRad(0.5/60);
}
void AP_InertialSensor_PX4::_get_sample(void)
{
struct accel_report accel_report;
struct gyro_report gyro_report;
if (_accel_fd == -1 || _gyro_fd == -1) {
return;
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
struct accel_report accel_report;
while (_accel_fd[i] != -1 &&
::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp[i]) {
_previous_accels[i] = _accel_in[i];
_accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp[i] = accel_report.timestamp;
}
}
for (uint8_t i=0; i<PX4_MAX_INS_INSTANCES; i++) {
struct gyro_report gyro_report;
while (_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp[i]) {
_gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_last_gyro_timestamp[i] = gyro_report.timestamp;
}
}
while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp) {
_accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp = accel_report.timestamp;
}
while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp) {
_gyro_in = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_last_gyro_timestamp = gyro_report.timestamp;
}
}
bool AP_InertialSensor_PX4::sample_available(void)
bool AP_InertialSensor_PX4::_sample_available(void)
{
uint64_t tnow = hrt_absolute_time();
while (tnow - _last_sample_timestamp > _sample_time_usec) {
@ -160,7 +223,7 @@ bool AP_InertialSensor_PX4::sample_available(void) @@ -160,7 +223,7 @@ bool AP_InertialSensor_PX4::sample_available(void)
bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
{
if (sample_available()) {
if (_sample_available()) {
return true;
}
uint32_t start = hal.scheduler->millis();
@ -172,7 +235,7 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms) @@ -172,7 +235,7 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag) {
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
}
if (sample_available()) {
if (_sample_available()) {
return true;
}
}
@ -182,37 +245,9 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms) @@ -182,37 +245,9 @@ bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_PX4::healthy(void)
bool AP_InertialSensor_PX4::healthy(void) const
{
if (_sample_time_usec == 0) {
// not initialised yet, show as healthy to prevent scary GCS
// warnings
return true;
}
uint64_t tnow = hrt_absolute_time();
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec ||
(tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// see if new samples are available
_get_sample();
tnow = hrt_absolute_time();
}
if ((tnow - _last_accel_timestamp) > 2*_sample_time_usec) {
// accels have not updated
return false;
}
if ((tnow - _last_gyro_timestamp) > 2*_sample_time_usec) {
// gyros have not updated
return false;
}
if (fabsf(_accel.x) > 30 && fabsf(_accel.y) > 30 && fabsf(_accel.z) > 30 &&
(_previous_accel - _accel).length() < 0.01f) {
// unchanging accel, large in all 3 axes. This is a likely
// accelerometer failure of the LSM303d
return false;
}
return true;
return get_gyro_instance_health(0) && get_accel_instance_health(0);
}
#endif // CONFIG_HAL_BOARD

45
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
Unescape View File

@ -13,37 +13,45 @@ @@ -13,37 +13,45 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#define PX4_MAX_INS_INSTANCES 2
class AP_InertialSensor_PX4 : public AP_InertialSensor
{
public:
AP_InertialSensor_PX4() :
AP_InertialSensor(),
_sample_time_usec(0),
_accel_fd(-1),
_gyro_fd(-1)
{}
_sample_time_usec(0)
{
}
/* Concrete implementation of AP_InertialSensor functions: */
bool update();
float get_delta_time();
float get_gyro_drift_rate();
bool sample_available();
bool wait_for_sample(uint16_t timeout_ms);
bool healthy(void);
bool healthy(void) const;
// multi-device interface
bool get_gyro_instance_health(uint8_t instance) const;
uint8_t get_gyro_count(void) const;
bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const;
bool get_accel_instance_health(uint8_t instance) const;
uint8_t get_accel_count(void) const;
bool get_accel_instance(uint8_t instance, Vector3f &accel) const;
private:
uint16_t _init_sensor( Sample_rate sample_rate );
void _get_sample(void);
uint64_t _last_update_usec;
float _delta_time;
Vector3f _accel_in;
Vector3f _gyro_in;
uint64_t _last_accel_timestamp;
uint64_t _last_gyro_timestamp;
bool _sample_available(void);
Vector3f _accel_in[PX4_MAX_INS_INSTANCES];
Vector3f _gyro_in[PX4_MAX_INS_INSTANCES];
uint64_t _last_accel_timestamp[PX4_MAX_INS_INSTANCES];
uint64_t _last_gyro_timestamp[PX4_MAX_INS_INSTANCES];
uint64_t _last_sample_timestamp;
bool _have_sample_available;
uint32_t _sample_time_usec;
bool _have_sample_available;
// support for updating filter at runtime
uint8_t _last_filter_hz;
@ -51,9 +59,16 @@ private: @@ -51,9 +59,16 @@ private:
void _set_filter_frequency(uint8_t filter_hz);
Vector3f _accel_data[PX4_MAX_INS_INSTANCES];
Vector3f _gyro_data[PX4_MAX_INS_INSTANCES];
Vector3f _previous_accels[PX4_MAX_INS_INSTANCES];
// accelerometer and gyro driver handles
int _accel_fd;
int _gyro_fd;
uint8_t _num_accel_instances;
uint8_t _num_gyro_instances;
int _accel_fd[PX4_MAX_INS_INSTANCES];
int _gyro_fd[PX4_MAX_INS_INSTANCES];
};
#endif
#endif // __AP_INERTIAL_SENSOR_PX4_H__

Write Preview
Loading…
Cancel
Save