diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 284fcfbdc7..5889aa78f0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -278,19 +278,20 @@ AP_InertialSensor::init( Start_style style, switch (sample_rate) { case RATE_50HZ: - _delta_time = 1.0f/50; + _sample_period_usec = 20000; break; case RATE_100HZ: - _delta_time = 1.0f/100; + _sample_period_usec = 10000; break; case RATE_200HZ: - _delta_time = 1.0f/200; + _sample_period_usec = 5000; break; case RATE_400HZ: default: - _delta_time = 1.0f/400; + _sample_period_usec = 2500; break; } + _delta_time = 1.0e-6f * _sample_period_usec; } @@ -301,9 +302,11 @@ void AP_InertialSensor::_detect_backends(Sample_rate sample_rate) { #if HAL_INS_DEFAULT == HAL_INS_HIL - _backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]); + _backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate); #elif HAL_INS_DEFAULT == HAL_INS_MPU6000 - _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]); + _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate); +#elif HAL_INS_DEFAULT == HAL_INS_PX4 + _backends[0] = AP_InertialSensor_PX4::detect(*this, sample_rate); #else #error Unrecognised HAL_INS_TYPE setting #endif @@ -912,13 +915,32 @@ void AP_InertialSensor::update(void) } /* - wait for a sample to be available + wait for a sample to be available. This waits for at least one new + accel and one new gyro sample. It is up to the backend to define + what a new sample means. Some backends are based on the sensor + providing a sample, some are based on time. */ void AP_InertialSensor::wait_for_sample(void) { + bool have_sample = false; + + // wait the right amount of time for a sample to be due + uint32_t now = hal.scheduler->micros(); + while (now - _last_sample_usec > _sample_period_usec) { + _last_sample_usec += _sample_period_usec; + have_sample = true; + } + if (!have_sample) { + uint32_t sample_due = _last_sample_usec + _sample_period_usec; + uint32_t wait_usec = sample_due - now; + hal.scheduler->delay_microseconds(wait_usec); + _last_sample_usec += _sample_period_usec; + } + + // but also wait for at least one backend to have a sample of both + // accel and gyro bool gyro_available = false; bool accel_available = false; - while (!gyro_available || !accel_available) { for (uint8_t i=0; iaccel_sample_available(); } } - hal.scheduler->delay(1); + if (!gyro_available || !accel_available) { + hal.scheduler->delay_microseconds(100); + } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 02a094590f..bf1099ffb6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -247,10 +247,17 @@ private: // time between samples float _delta_time; + + // last time a wait_for_sample() returned a sample + uint32_t _last_sample_usec; + + // time between samples in microseconds + uint32_t _sample_period_usec; }; #include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" +#include "AP_InertialSensor_PX4.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 3102d5db05..0fde6615f3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -4,17 +4,16 @@ #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" -AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) : - _imu(imu), - _gyro(gyro), - _accel(accel) +AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : + _imu(imu) {} /* rotate gyro vector and add the gyro offset */ -void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, uint32_t now) +void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now) { + _imu._gyro[instance] = gyro; _imu._gyro[instance].rotate(_imu._board_orientation); _imu._gyro[instance] -= _imu._gyro_offset[instance]; _imu._last_gyro_sample_time_usec[instance] = now; @@ -23,8 +22,9 @@ void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, uint32 /* rotate accel vector, scale and add the accel offset */ -void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, uint32_t now) +void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now) { + _imu._accel[instance] = accel; _imu._accel[instance].rotate(_imu._board_orientation); const Vector3f &accel_scale = _imu._accel_scale[instance].get(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index cc8b15cfda..d6b373b5c0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -23,7 +23,7 @@ class AP_InertialSensor_Backend { public: - AP_InertialSensor_Backend(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel); + AP_InertialSensor_Backend(AP_InertialSensor &imu); // we declare a virtual destructor so that drivers can // override with a custom destructor if need be. @@ -50,15 +50,11 @@ public: protected: AP_InertialSensor &_imu; ///< access to frontend - // references to instance vectors - Vector3f &_gyro; - Vector3f &_accel; - // rotate gyro vector and offset - void _rotate_and_offset_gyro(uint8_t instance, uint32_t now); + void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro, uint32_t now); // rotate accel vector, scale and offset - void _rotate_and_offset_accel(uint8_t instance, uint32_t now); + void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel, uint32_t now); // note that each backend is also expected to have a detect() // function which instantiates an instance of the backend sensor diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 9acb38afa4..22428f643a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -5,10 +5,9 @@ const extern AP_HAL::HAL& hal; -AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) : - AP_InertialSensor_Backend(imu, gyro, accel), - _sample_period_usec(0), - _last_sample_usec(0) +AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _sample_period_usec(0) { } @@ -16,11 +15,9 @@ AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &g detect the sensor */ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu, - AP_InertialSensor::Sample_rate sample_rate, - Vector3f &gyro, - Vector3f &accel) + AP_InertialSensor::Sample_rate sample_rate) { - AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu, gyro, accel); + AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu); if (sensor == NULL) { return NULL; } @@ -57,15 +54,10 @@ bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_r bool AP_InertialSensor_HIL::update(void) { - uint32_t now = hal.scheduler->micros(); - while (now - _last_sample_usec > _sample_period_usec) { - _last_sample_usec += _sample_period_usec; - } return true; } bool AP_InertialSensor_HIL::_sample_available() { - return (hal.scheduler->micros() - _last_sample_usec > _sample_period_usec); + return true; } - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index eaf1bdfe0c..b49a7e56ca 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -8,7 +8,7 @@ class AP_InertialSensor_HIL : public AP_InertialSensor_Backend { public: - AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel); + AP_InertialSensor_HIL(AP_InertialSensor &imu); /* update accel and gyro state */ bool update(); @@ -18,15 +18,12 @@ public: // detect the sensor static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, - AP_InertialSensor::Sample_rate sample_rate, - Vector3f &gyro, - Vector3f &accel); + AP_InertialSensor::Sample_rate sample_rate); private: bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); bool _sample_available(void); uint32_t _sample_period_usec; - uint32_t _last_sample_usec; }; #endif // __AP_INERTIALSENSOR_HIL_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index c2f84e5161..42db807b1a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -173,10 +173,8 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f); * variants however */ -AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, - Vector3f &gyro, - Vector3f &accel) : - AP_InertialSensor_Backend(imu, gyro, accel), +AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), _drdy_pin(NULL), _spi(NULL), _spi_sem(NULL), @@ -193,11 +191,9 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu, detect the sensor */ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu, - AP_InertialSensor::Sample_rate sample_rate, - Vector3f &gyro, - Vector3f &accel) + AP_InertialSensor::Sample_rate sample_rate) { - AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu, gyro, accel); + AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu); if (sensor == NULL) { return NULL; } @@ -276,21 +272,22 @@ bool AP_InertialSensor_MPU6000::update( void ) // we have a full set of samples uint16_t num_samples; uint32_t now = hal.scheduler->micros(); + Vector3f accel, gyro; hal.scheduler->suspend_timer_procs(); - _gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel(_accel_sum.x, _accel_sum.y, _accel_sum.z); + gyro(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); + accel(_accel_sum.x, _accel_sum.y, _accel_sum.z); num_samples = _sum_count; _accel_sum.zero(); _gyro_sum.zero(); _sum_count = 0; hal.scheduler->resume_timer_procs(); - _gyro *= _gyro_scale / num_samples; - _rotate_and_offset_gyro(_gyro_instance, now); + gyro *= _gyro_scale / num_samples; + _rotate_and_offset_gyro(_gyro_instance, gyro, now); - _accel *= MPU6000_ACCEL_SCALE_1G / num_samples; - _rotate_and_offset_accel(_accel_instance, now); + accel *= MPU6000_ACCEL_SCALE_1G / num_samples; + _rotate_and_offset_accel(_accel_instance, accel, now); if (_last_filter_hz != _imu.get_filter()) { if (_spi_sem->take(10)) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 743fe49d20..c27dab15ff 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -15,7 +15,7 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend { public: - AP_InertialSensor_MPU6000(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel); + AP_InertialSensor_MPU6000(AP_InertialSensor &imu); /* update accel and gyro state */ bool update(); @@ -25,9 +25,8 @@ public: // detect the sensor static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, - AP_InertialSensor::Sample_rate sample_rate, - Vector3f &gyro, - Vector3f &accel); + AP_InertialSensor::Sample_rate sample_rate); + private: #if MPU6000_DEBUG void _dump_registers(void); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index daf14cf16a..9830aa836e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -2,6 +2,7 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + #include "AP_InertialSensor_PX4.h" const extern AP_HAL::HAL& hal; @@ -15,11 +16,35 @@ const extern AP_HAL::HAL& hal; #include #include -#include +#include -uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) +AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _last_get_sample_timestamp(0), + _sample_time_usec(0) { - // assumes max 2 instances +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu, + AP_InertialSensor::Sample_rate sample_rate) +{ + AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor(sample_rate)) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_InertialSensor_PX4::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) +{ + // assumes max 3 instances _accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY); _accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY); _accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY); @@ -32,45 +57,44 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) for (uint8_t i=0; i= 0) { _num_accel_instances = i+1; + _accel_instance[i] = _imu.register_accel(); } if (_gyro_fd[i] >= 0) { _num_gyro_instances = i+1; + _gyro_instance[i] = _imu.register_gyro(); } } if (_num_accel_instances == 0) { - hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); + return false; } if (_num_gyro_instances == 0) { - hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); + return false; } switch (sample_rate) { - case RATE_50HZ: + case AP_InertialSensor::RATE_50HZ: _default_filter_hz = 15; _sample_time_usec = 20000; break; - case RATE_100HZ: + case AP_InertialSensor::RATE_100HZ: _default_filter_hz = 30; _sample_time_usec = 10000; break; - case RATE_200HZ: + case AP_InertialSensor::RATE_200HZ: _default_filter_hz = 30; _sample_time_usec = 5000; break; - case RATE_400HZ: - default: + case AP_InertialSensor::RATE_400HZ: _default_filter_hz = 30; _sample_time_usec = 2500; break; + default: + return false; } - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - return AP_PRODUCT_ID_PX4_V2; -#else - return AP_PRODUCT_ID_PX4; -#endif + return true; } /* @@ -89,109 +113,33 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz) } } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -// multi-device interface -bool AP_InertialSensor_PX4::get_gyro_health(uint8_t instance) const -{ - if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - if (instance >= _num_gyro_instances) { - return false; - } - - if ((_last_get_sample_timestamp - _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_accel_health(uint8_t k) const -{ - if (_sample_time_usec == 0 || _last_get_sample_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - if (k >= _num_accel_instances) { - return false; - } - - if ((_last_get_sample_timestamp - _last_accel_timestamp[k]) > 2*_sample_time_usec) { - // accels have not updated - return false; - } - if (fabsf(_accel[k].x) > 30 && fabsf(_accel[k].y) > 30 && fabsf(_accel[k].z) > 30 && - (_previous_accel[k] - _accel[k]).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::update(void) { - if (!wait_for_sample(100)) { - return false; - } - // get the latest sample from the sensor drivers _get_sample(); + uint32_t now = hal.scheduler->micros(); for (uint8_t k=0; k<_num_accel_instances; k++) { - _previous_accel[k] = _accel[k]; - _accel[k] = _accel_in[k]; - _accel[k].rotate(_board_orientation); - _accel[k].x *= _accel_scale[k].get().x; - _accel[k].y *= _accel_scale[k].get().y; - _accel[k].z *= _accel_scale[k].get().z; - _accel[k] -= _accel_offset[k]; + Vector3f accel = _accel_in[k]; + _rotate_and_offset_accel(_accel_instance[k], accel, now); + _last_accel_update_timestamp[k] = _last_accel_timestamp[k]; } for (uint8_t k=0; k<_num_gyro_instances; k++) { - _gyro[k] = _gyro_in[k]; - _gyro[k].rotate(_board_orientation); - _gyro[k] -= _gyro_offset[k]; + Vector3f gyro = _gyro_in[k]; + _rotate_and_offset_gyro(_gyro_instance[k], gyro, now); + _last_gyro_update_timestamp[k] = _last_gyro_timestamp[k]; } - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } - _have_sample_available = false; - return true; } -float AP_InertialSensor_PX4::get_delta_time(void) const -{ - return _sample_time_usec * 1.0e-6f; -} - -float AP_InertialSensor_PX4::get_gyro_drift_rate(void) -{ - // assume 0.5 degrees/second/minute - return ToRad(0.5/60); -} - void AP_InertialSensor_PX4::_get_sample(void) { for (uint8_t i=0; i<_num_accel_instances; i++) { @@ -215,59 +163,26 @@ void AP_InertialSensor_PX4::_get_sample(void) _last_get_sample_timestamp = hal.scheduler->micros64(); } -bool AP_InertialSensor_PX4::_sample_available(void) +bool AP_InertialSensor_PX4::gyro_sample_available(void) { - uint64_t tnow = hal.scheduler->micros64(); - while (tnow - _last_sample_timestamp > _sample_time_usec) { - _have_sample_available = true; - _last_sample_timestamp += _sample_time_usec; - } - return _have_sample_available; -} - -bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint64_t start = hal.scheduler->millis64(); - while ((hal.scheduler->millis64() - start) < timeout_ms) { - uint64_t tnow = hal.scheduler->micros64(); - // we spin for the last timing_lag microseconds. Before that - // we yield the CPU to allow IO to happen - const uint16_t timing_lag = 400; - 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()) { + _get_sample(); + for (uint8_t i=0; i<_num_gyro_instances; i++) { + if (_last_gyro_timestamp[i] != _last_gyro_update_timestamp[i]) { return true; } - } + } return false; } -/** - try to detect bad accel/gyro sensors - */ -bool AP_InertialSensor_PX4::healthy(void) const -{ - return get_gyro_health(0) && get_accel_health(0); -} - -uint8_t AP_InertialSensor_PX4::_get_primary_gyro(void) const -{ - for (uint8_t i=0; i<_num_gyro_instances; i++) { - if (get_gyro_health(i)) return i; - } - return 0; -} - -uint8_t AP_InertialSensor_PX4::get_primary_accel(void) const +bool AP_InertialSensor_PX4::accel_sample_available(void) { + _get_sample(); for (uint8_t i=0; i<_num_accel_instances; i++) { - if (get_accel_health(i)) return i; + if (_last_accel_timestamp[i] != _last_accel_update_timestamp[i]) { + return true; + } } - return 0; + return false; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index 1950910c4e..fe584aa050 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -13,47 +13,35 @@ #include #include -class AP_InertialSensor_PX4 : public AP_InertialSensor +class AP_InertialSensor_PX4 : public AP_InertialSensor_Backend { public: - AP_InertialSensor_PX4() : - AP_InertialSensor(), - _last_get_sample_timestamp(0), - _sample_time_usec(0) - { - } + AP_InertialSensor_PX4(AP_InertialSensor &imu); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); - bool healthy(void) const; + /* update accel and gyro state */ + bool update(); - // multi-device interface - bool get_gyro_health(uint8_t instance) const; - uint8_t get_gyro_count(void) const; + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, + AP_InertialSensor::Sample_rate sample_rate); - bool get_accel_health(uint8_t instance) const; - uint8_t get_accel_count(void) const; - - uint8_t get_primary_accel(void) const; + bool gyro_sample_available(void); + bool accel_sample_available(void); private: - uint8_t _get_primary_gyro(void) const; - - uint16_t _init_sensor( Sample_rate sample_rate ); + bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); void _get_sample(void); bool _sample_available(void); Vector3f _accel_in[INS_MAX_INSTANCES]; Vector3f _gyro_in[INS_MAX_INSTANCES]; uint64_t _last_accel_timestamp[INS_MAX_INSTANCES]; uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES]; + uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES]; uint64_t _last_get_sample_timestamp; uint64_t _last_sample_timestamp; uint32_t _sample_time_usec; - bool _have_sample_available; // support for updating filter at runtime uint8_t _last_filter_hz; @@ -64,8 +52,14 @@ private: // accelerometer and gyro driver handles uint8_t _num_accel_instances; uint8_t _num_gyro_instances; + int _accel_fd[INS_MAX_INSTANCES]; int _gyro_fd[INS_MAX_INSTANCES]; + + // indexes in frontend object. Note that these could be different + // from the backend indexes + uint8_t _accel_instance[INS_MAX_INSTANCES]; + uint8_t _gyro_instance[INS_MAX_INSTANCES]; }; #endif #endif // __AP_INERTIAL_SENSOR_PX4_H__