mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
AP_InertialSensor: implement up to two sensors on PX4
This commit is contained in:
Andrew Tridgell
parent
2753449e75
commit
d9b6f7f0f7
@ -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 )
|
||||
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)
|
||||
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 ==================== */
|
||||
|
||||
// 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
|
||||
{
|
||||
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)
|
||||
{
|
||||
Vector3f accel_scale = _accel_scale.get();
|
||||
|
||||
// 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)
|
||||
|
||||
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)
|
||||
|
||||
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)
|
||||
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)
|
||||
/**
|
||||
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
|
||||
|
||||
@ -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:
|
||||
|
||||
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__
|
||||
|
||||
Loading…
Reference in New Issue
Block a user