From d9b6f7f0f7f70e83a922f6fb4f2b067bc6f27028 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:44:31 +1100 Subject: [PATCH] AP_InertialSensor: implement up to two sensors on PX4 --- .../AP_InertialSensor_PX4.cpp | 225 ++++++++++-------- .../AP_InertialSensor/AP_InertialSensor_PX4.h | 45 ++-- 2 files changed, 160 insertions(+), 110 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index a2694411d5..d1bf77c444 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -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= _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 _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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 5a39f3d3cf..fd37b8c6b9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -13,37 +13,45 @@ #include #include +#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__