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__