diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 3a783bd183..129d2cda40 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -316,6 +316,8 @@ AP_InertialSensor::_detect_backends(Sample_rate sample_rate) _backends[0] = AP_InertialSensor_Oilpan::detect(*this, sample_rate); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250 _backends[0] = AP_InertialSensor_MPU9250::detect(*this, sample_rate); +#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE + _backends[0] = AP_InertialSensor_Flymaple::detect(*this, sample_rate); #else #error Unrecognised HAL_INS_TYPE setting #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 5842a76121..74190af255 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -277,6 +277,7 @@ private: #include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_L3G4200D.h" +#include "AP_InertialSensor_Flymaple.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_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index ccc481c086..cc0d5a097e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -14,7 +14,7 @@ along with this program. If not, see . */ /* - Flymaple port by Mike McCauley + Flymaple IMU driver by Mike McCauley */ // Interface to the Flymaple sensors: @@ -28,20 +28,6 @@ const extern AP_HAL::HAL& hal; -/// Statics -Vector3f AP_InertialSensor_Flymaple::_accel_filtered; -uint32_t AP_InertialSensor_Flymaple::_accel_samples; -Vector3f AP_InertialSensor_Flymaple::_gyro_filtered; -uint32_t AP_InertialSensor_Flymaple::_gyro_samples; -uint64_t AP_InertialSensor_Flymaple::_last_accel_timestamp; -uint64_t AP_InertialSensor_Flymaple::_last_gyro_timestamp; -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_x(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_y(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_accel_filter_z(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_x(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_y(800, 10); -LowPassFilter2p AP_InertialSensor_Flymaple::_gyro_filter_z(800, 10); - // This is how often we wish to make raw samples of the sensors in Hz const uint32_t raw_sample_rate_hz = 800; // And the equivalent time between samples in microseconds @@ -77,24 +63,56 @@ const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz); // Result wil be radians/sec #define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f) -uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) +AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_gyro_sample(false), + _have_accel_sample(false), + _accel_filter_x(raw_sample_rate_hz, 10), + _accel_filter_y(raw_sample_rate_hz, 10), + _accel_filter_z(raw_sample_rate_hz, 10), + _gyro_filter_x(raw_sample_rate_hz, 10), + _gyro_filter_y(raw_sample_rate_hz, 10), + _gyro_filter_z(raw_sample_rate_hz, 10), + _last_gyro_timestamp(0), + _last_accel_timestamp(0) +{} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu, + AP_InertialSensor::Sample_rate sample_rate) +{ + AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor(sample_rate)) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_InertialSensor_Flymaple::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) { // Sensors are raw sampled at 800Hz. // Here we figure the divider to get the rate that update should be called switch (sample_rate) { - case RATE_50HZ: - _sample_divider = raw_sample_rate_hz / 50; + case AP_InertialSensor::RATE_50HZ: _default_filter_hz = 10; break; - case RATE_100HZ: - _sample_divider = raw_sample_rate_hz / 100; + case AP_InertialSensor::RATE_100HZ: _default_filter_hz = 20; break; - case RATE_200HZ: + case AP_InertialSensor::RATE_200HZ: + _default_filter_hz = 20; + break; + case AP_InertialSensor::RATE_400HZ: + _default_filter_hz = 30; + break; default: - _sample_divider = raw_sample_rate_hz / 200; - _default_filter_hz = 20; - break; + return false; } // get pointer to i2c bus semaphore @@ -146,12 +164,15 @@ uint16_t AP_InertialSensor_Flymaple::_init_sensor( Sample_rate sample_rate ) hal.scheduler->delay(1); // Set up the filter desired - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); - // give back i2c semaphore + // give back i2c semaphore i2c_sem->give(); - return AP_PRODUCT_ID_FLYMAPLE; + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + + return true; } /* @@ -170,109 +191,47 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz) _gyro_filter_z.set_cutoff_frequency(raw_sample_rate_hz, filter_hz); } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ // This takes about 20us to run bool AP_InertialSensor_Flymaple::update(void) { - if (!wait_for_sample(100)) { - return false; - } - Vector3f accel_scale = _accel_scale[0].get(); + Vector3f accel, gyro; + uint32_t now = hal.scheduler->micros(); - // Not really needed since Flymaple _accumulate runs in the main thread hal.scheduler->suspend_timer_procs(); - - // base the time on the gyro timestamp, as that is what is - // multiplied by time to integrate in DCM - _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f; - _last_update_usec = _last_gyro_timestamp; - - _previous_accel[0] = _accel[0]; - - _accel[0] = _accel_filtered; - _accel_samples = 0; - - _gyro[0] = _gyro_filtered; - _gyro_samples = 0; - + accel = _accel_filtered; + gyro = _gyro_filtered; + _have_gyro_sample = false; + _have_accel_sample = false; hal.scheduler->resume_timer_procs(); - // add offsets and rotation - _accel[0].rotate(_board_orientation); - // Adjust for chip scaling to get m/s/s - _accel[0] *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; - - // Now the calibration scale factor - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - _gyro[0].rotate(_board_orientation); + accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; + _rotate_and_offset_accel(_accel_instance, accel, now); // Adjust for chip scaling to get radians/sec - _gyro[0] *= FLYMAPLE_GYRO_SCALE_R_S; - _gyro[0] -= _gyro_offset[0]; + gyro *= FLYMAPLE_GYRO_SCALE_R_S; + _rotate_and_offset_gyro(_gyro_instance, gyro, now); - 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(); } return true; } -bool AP_InertialSensor_Flymaple::get_gyro_health(void) const -{ - if (_last_gyro_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - uint64_t now = hal.scheduler->micros(); - if ((now - _last_gyro_timestamp) >= (2 * raw_sample_interval_us)) { - // gyros have not updated - return false; - } - return true; -} - -bool AP_InertialSensor_Flymaple::get_accel_health(void) const -{ - if (_last_accel_timestamp == 0) { - // not initialised yet, show as healthy to prevent scary GCS - // warnings - return true; - } - uint64_t now = hal.scheduler->micros(); - if ((now - _last_accel_timestamp) >= (2 * raw_sample_interval_us)) { - // gyros have not updated - return false; - } - return true; -} - -float AP_InertialSensor_Flymaple::get_delta_time(void) const -{ - return _delta_time; -} - -float AP_InertialSensor_Flymaple::get_gyro_drift_rate(void) -{ - // Dont really know this for the ITG-3200. - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - // This needs to get called as often as possible. // Its job is to accumulate samples as fast as is reasonable for the accel and gyro // sensors. -// Cant call this from within the system timers, since the long I2C reads (up to 1ms) -// with interrupts disabled breaks lots of things -// Therefore must call this as often as possible from -// within the mainline and thropttle the reads here to suit the sensors +// Note that this is called from gyro_sample_available() and +// accel_sample_available(), which is really not good enough for +// 800Hz, as it is common for the main loop to take more than 1.5ms +// before wait_for_sample() is called again. We can't just call this +// from a timer as timers run with interrupts disabled, and the I2C +// operations take too long +// So we are stuck with a suboptimal solution. The results are not so +// good in terms of timing. It may be better with the FIFOs enabled void AP_InertialSensor_Flymaple::_accumulate(void) { // get pointer to i2c bus semaphore @@ -285,7 +244,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) // Read accelerometer // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used uint8_t buffer[6]; - uint64_t now = hal.scheduler->micros(); + uint32_t now = hal.scheduler->micros(); // This takes about 250us at 400kHz I2C speed if ((now - _last_accel_timestamp) >= raw_sample_interval_us && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0) @@ -300,7 +259,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) _accel_filtered = Vector3f(_accel_filter_x.apply(x), _accel_filter_y.apply(y), _accel_filter_z.apply(z)); - _accel_samples++; + _have_accel_sample = true; _last_accel_timestamp = now; } @@ -317,7 +276,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) _gyro_filtered = Vector3f(_gyro_filter_x.apply(x), _gyro_filter_y.apply(y), _gyro_filter_z.apply(z)); - _gyro_samples++; + _have_gyro_sample = true; _last_gyro_timestamp = now; } @@ -325,26 +284,4 @@ void AP_InertialSensor_Flymaple::_accumulate(void) i2c_sem->give(); } -bool AP_InertialSensor_Flymaple::_sample_available(void) -{ - _accumulate(); - return min(_accel_samples, _gyro_samples) / _sample_divider > 0; -} - -bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms) -{ - if (_sample_available()) { - return true; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; -} - #endif // CONFIG_HAL_BOARD - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index b08546dd64..5e56f696a3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -6,39 +6,32 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE -#include #include "AP_InertialSensor.h" #include #include -class AP_InertialSensor_Flymaple : public AP_InertialSensor +class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend { public: + AP_InertialSensor_Flymaple(AP_InertialSensor &imu); - AP_InertialSensor_Flymaple() : AP_InertialSensor() {} + /* update accel and gyro state */ + bool update(); - /* 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 get_gyro_health(void) const; - bool get_accel_health(void) const; - bool healthy(void) const { return get_gyro_health() && get_accel_health(); } + bool gyro_sample_available(void) { _accumulate(); return _have_gyro_sample; } + bool accel_sample_available(void) { _accumulate(); return _have_accel_sample; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, + AP_InertialSensor::Sample_rate sample_rate); private: - uint16_t _init_sensor( Sample_rate sample_rate ); - static void _accumulate(void); - bool _sample_available(); - uint64_t _last_update_usec; - float _delta_time; - static Vector3f _accel_filtered; - static uint32_t _accel_samples; - static Vector3f _gyro_filtered; - static uint32_t _gyro_samples; - static uint64_t _last_accel_timestamp; - static uint64_t _last_gyro_timestamp; - uint8_t _sample_divider; + bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); + void _accumulate(void); + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + bool _have_gyro_sample; + bool _have_accel_sample; // support for updating filter at runtime uint8_t _last_filter_hz; @@ -46,12 +39,18 @@ private: void _set_filter_frequency(uint8_t filter_hz); // Low Pass filters for gyro and accel - static LowPassFilter2p _accel_filter_x; - static LowPassFilter2p _accel_filter_y; - static LowPassFilter2p _accel_filter_z; - static LowPassFilter2p _gyro_filter_x; - static LowPassFilter2p _gyro_filter_y; - static LowPassFilter2p _gyro_filter_z; + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; + + uint8_t _gyro_instance; + uint8_t _accel_instance; + + uint32_t _last_gyro_timestamp; + uint32_t _last_accel_timestamp; }; #endif #endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__