diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 43617189d4..f53572ba7b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -90,14 +90,126 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { AP_GROUPINFO("MPU6K_FILTER", 4, AP_InertialSensor, _mpu6000_filter, 0), #if INS_MAX_INSTANCES > 1 + // @Param: ACC2SCAL_X + // @DisplayName: Accelerometer2 scaling of X axis + // @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC2SCAL_Y + // @DisplayName: Accelerometer2 scaling of Y axis + // @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC2SCAL_Z + // @DisplayName: Accelerometer2 scaling of Z axis + // @Description: Accelerometer2 scaling of Z axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced AP_GROUPINFO("ACC2SCAL", 5, AP_InertialSensor, _accel_scale[1], 0), + + // @Param: ACC2OFFS_X + // @DisplayName: Accelerometer2 offsets of X axis + // @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC2OFFS_Y + // @DisplayName: Accelerometer2 offsets of Y axis + // @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC2OFFS_Z + // @DisplayName: Accelerometer2 offsets of Z axis + // @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced AP_GROUPINFO("ACC2OFFS", 6, AP_InertialSensor, _accel_offset[1], 0), + + // @Param: GYR2OFFS_X + // @DisplayName: Gyro2 offsets of X axis + // @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR2OFFS_Y + // @DisplayName: Gyro2 offsets of Y axis + // @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR2OFFS_Z + // @DisplayName: Gyro2 offsets of Z axis + // @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced AP_GROUPINFO("GYR2OFFS", 7, AP_InertialSensor, _gyro_offset[1], 0), #endif #if INS_MAX_INSTANCES > 2 + // @Param: ACC3SCAL_X + // @DisplayName: Accelerometer3 scaling of X axis + // @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC3SCAL_Y + // @DisplayName: Accelerometer3 scaling of Y axis + // @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced + + // @Param: ACC3SCAL_Z + // @DisplayName: Accelerometer3 scaling of Z axis + // @Description: Accelerometer3 scaling of Z axis Calculated during acceleration calibration routine + // @Range: 0.8 1.2 + // @User: Advanced AP_GROUPINFO("ACC3SCAL", 8, AP_InertialSensor, _accel_scale[2], 0), + + // @Param: ACC3OFFS_X + // @DisplayName: Accelerometer3 offsets of X axis + // @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC3OFFS_Y + // @DisplayName: Accelerometer3 offsets of Y axis + // @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced + + // @Param: ACC3OFFS_Z + // @DisplayName: Accelerometer3 offsets of Z axis + // @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations + // @Units: m/s/s + // @Range: -300 300 + // @User: Advanced AP_GROUPINFO("ACC3OFFS", 9, AP_InertialSensor, _accel_offset[2], 0), + + // @Param: GYR3OFFS_X + // @DisplayName: Gyro3 offsets of X axis + // @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR3OFFS_Y + // @DisplayName: Gyro3 offsets of Y axis + // @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced + + // @Param: GYR3OFFS_Z + // @DisplayName: Gyro3 offsets of Z axis + // @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations + // @Units: rad/s + // @User: Advanced AP_GROUPINFO("GYR3OFFS", 10, AP_InertialSensor, _gyro_offset[2], 0), #endif @@ -105,30 +217,154 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = { }; AP_InertialSensor::AP_InertialSensor() : + _gyro_count(0), + _accel_count(0), + _backend_count(0), _accel(), _gyro(), - _board_orientation(ROTATION_NONE) + _board_orientation(ROTATION_NONE), + _hil_mode(false), + _have_3D_calibration(false) { AP_Param::setup_object_defaults(this, var_info); + for (uint8_t i=0; ipanic(PSTR("Too many gyros")); + } + return _gyro_count++; +} + +/* + register a new accel instance + */ +uint8_t AP_InertialSensor::register_accel(void) +{ + if (_accel_count == INS_MAX_INSTANCES) { + hal.scheduler->panic(PSTR("Too many accels")); + } + return _accel_count++; } void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) { - _product_id = _init_sensor(sample_rate); + // remember the sample rate + _sample_rate = sample_rate; - // check scaling + if (_gyro_count == 0 && _accel_count == 0) { + // detect available backends. Only called once + _detect_backends(); + } + + _product_id = 0; // FIX + + // initialise accel scale if need be. This is needed as we can't + // give non-zero default values for vectors in AP_Param for (uint8_t i=0; ipanic(PSTR("Too many INS backends")); + } + _backends[_backend_count] = detect(*this); + if (_backends[_backend_count] != NULL) { + _backend_count++; + } +} + + +/* + detect available backends for this board + */ +void +AP_InertialSensor::_detect_backends(void) +{ +#if HAL_INS_DEFAULT == HAL_INS_HIL + _add_backend(AP_InertialSensor_HIL::detect); +#elif HAL_INS_DEFAULT == HAL_INS_MPU6000 + _add_backend(AP_InertialSensor_MPU6000::detect); +#elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN + _add_backend(AP_InertialSensor_PX4::detect); +#elif HAL_INS_DEFAULT == HAL_INS_OILPAN + _add_backend(AP_InertialSensor_Oilpan::detect); +#elif HAL_INS_DEFAULT == HAL_INS_MPU9250 + _add_backend(AP_InertialSensor_MPU9250::detect); +#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE + _add_backend(AP_InertialSensor_Flymaple::detect); +#else + #error Unrecognised HAL_INS_TYPE setting +#endif + +#if 0 // disabled due to broken hardware on some PXF capes +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + // the PXF also has a MPU6000 + _add_backend(AP_InertialSensor_MPU6000::detect); +#endif +#endif + + if (_backend_count == 0 || + _gyro_count == 0 || + _accel_count == 0) { + hal.scheduler->panic(PSTR("No INS backends available")); + } + + // set the product ID to the ID of the first backend + _product_id.set(_backends[0]->product_id()); } void @@ -138,6 +374,8 @@ AP_InertialSensor::init_accel() // save calibration _save_parameters(); + + check_3D_calibration(); } #if !defined( __AVR_ATmega1280__ ) @@ -213,10 +451,7 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact } uint8_t num_samples = 0; while (num_samples < 32) { - if (!wait_for_sample(1000)) { - interact->printf_P(PSTR("Failed to get INS sample\n")); - goto failed; - } + wait_for_sample(); // read samples from ins update(); // capture sample @@ -254,6 +489,8 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact } _save_parameters(); + check_3D_calibration(); + // calculate the trims as well from primary accels and pass back to caller _calculate_trim(samples[0][0], trim_roll, trim_pitch); @@ -271,18 +508,37 @@ failed: } #endif -/// calibrated - returns true if the accelerometers have been calibrated -/// @note this should not be called while flying because it reads from the eeprom which can be slow -bool AP_InertialSensor::calibrated() +/* + check if the accelerometers are calibrated in 3D. Called on startup + and any accel cal + */ +void AP_InertialSensor::check_3D_calibration() { + _have_3D_calibration = false; // check each accelerometer has offsets saved for (uint8_t i=0; iupdate(); + } + + // adjust health status if a sensor has a non-zero error count + // but another sensor doesn't. + bool have_zero_accel_error_count = false; + bool have_zero_gyro_error_count = false; + for (uint8_t i=0; imicros(); + + if (_next_sample_usec == 0 && _delta_time <= 0) { + // this is the first call to wait_for_sample() + _last_sample_usec = now - _sample_period_usec; + _next_sample_usec = now + _sample_period_usec; + goto check_sample; + } + + // see how long it is till the next sample is due + if (_next_sample_usec - now <=_sample_period_usec) { + // we're ahead on time, schedule next sample at expected period + uint32_t wait_usec = _next_sample_usec - now; + if (wait_usec > 200) { + hal.scheduler->delay_microseconds(wait_usec); + } + _next_sample_usec += _sample_period_usec; + } else if (now - _next_sample_usec < _sample_period_usec/8) { + // we've overshot, but only by a small amount, keep on + // schedule with no delay + _next_sample_usec += _sample_period_usec; + } else { + // we've overshot by a larger amount, re-zero scheduling with + // no delay + _next_sample_usec = now + _sample_period_usec; + } + +check_sample: + if (!_hil_mode) { + // we also wait for at least one backend to have a sample of both + // accel and gyro. This normally completes immediately. + bool gyro_available = false; + bool accel_available = false; + while (!gyro_available || !accel_available) { + for (uint8_t i=0; i<_backend_count; i++) { + gyro_available |= _backends[i]->gyro_sample_available(); + accel_available |= _backends[i]->accel_sample_available(); + } + if (!gyro_available || !accel_available) { + hal.scheduler->delay_microseconds(100); + } + } + } + + now = hal.scheduler->micros(); + _delta_time = (now - _last_sample_usec) * 1.0e-6f; + _last_sample_usec = now; + +#if 0 + { + static uint64_t delta_time_sum; + static uint16_t counter; + if (delta_time_sum == 0) { + delta_time_sum = _sample_period_usec; + } + delta_time_sum += _delta_time * 1.0e6f; + if (counter++ == 400) { + counter = 0; + hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n", + (unsigned long)now, + (unsigned long)delta_time_sum, + (long)(now - delta_time_sum)); + } + } +#endif + + _have_sample = true; +} + +/* + support for setting accel and gyro vectors, for use by HIL + */ +void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel) +{ + if (instance < INS_MAX_INSTANCES) { + _accel[instance] = accel; + _accel_healthy[instance] = true; + } +} + +void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro) +{ + if (instance < INS_MAX_INSTANCES) { + _gyro[instance] = gyro; + _gyro_healthy[instance] = true; + } +} + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 3eaba83755..12004e1ede 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -11,18 +11,22 @@ maximum number of INS instances available on this platform. If more than 1 then redundent sensors may be available */ -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL -#define INS_MAX_INSTANCES 3 -#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 #define INS_MAX_INSTANCES 3 +#define INS_MAX_BACKENDS 6 #else #define INS_MAX_INSTANCES 1 +#define INS_MAX_BACKENDS 1 #endif + #include #include #include #include "AP_InertialSensor_UserInteract.h" + +class AP_InertialSensor_Backend; + /* AP_InertialSensor is an abstraction for gyro and accel measurements * which are correctly aligned to the body axes and scaled to SI units. * @@ -32,12 +36,11 @@ */ class AP_InertialSensor { + friend class AP_InertialSensor_Backend; + public: AP_InertialSensor(); - // empty virtual destructor - virtual ~AP_InertialSensor() {} - enum Start_style { COLD_START = 0, WARM_START @@ -64,22 +67,28 @@ public: /// /// @param style The initialisation startup style. /// - virtual void init( Start_style style, - Sample_rate sample_rate); + void init( Start_style style, + Sample_rate sample_rate); /// Perform cold startup initialisation for just the accelerometers. /// /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work. /// - virtual void init_accel(); + void init_accel(); + + + /// Register a new gyro/accel driver, allocating an instance + /// number + uint8_t register_gyro(void); + uint8_t register_accel(void); #if !defined( __AVR_ATmega1280__ ) // perform accelerometer calibration including providing user instructions // and feedback - virtual bool calibrate_accel(AP_InertialSensor_UserInteract *interact, - float& trim_roll, - float& trim_pitch); + bool calibrate_accel(AP_InertialSensor_UserInteract *interact, + float& trim_roll, + float& trim_pitch); #endif /// calibrated - returns true if the accelerometers have been calibrated @@ -93,65 +102,66 @@ public: /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work /// - virtual void init_gyro(void); + void init_gyro(void); /// Fetch the current gyro values /// /// @returns vector of rotational rates in radians/sec /// const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; } - const Vector3f &get_gyro(void) const { return get_gyro(_get_primary_gyro()); } - virtual void set_gyro(uint8_t instance, const Vector3f &gyro) {} + const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); } + void set_gyro(uint8_t instance, const Vector3f &gyro); // set gyro offsets in radians/sec const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; } - const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_get_primary_gyro()); } + const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); } /// Fetch the current accelerometer values /// /// @returns vector of current accelerations in m/s/s /// const Vector3f &get_accel(uint8_t i) const { return _accel[i]; } - const Vector3f &get_accel(void) const { return get_accel(get_primary_accel()); } - virtual void set_accel(uint8_t instance, const Vector3f &accel) {} + const Vector3f &get_accel(void) const { return get_accel(_primary_accel); } + void set_accel(uint8_t instance, const Vector3f &accel); + + uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; } + uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; } // multi-device interface - virtual bool get_gyro_health(uint8_t instance) const { return true; } - bool get_gyro_health(void) const { return get_gyro_health(_get_primary_gyro()); } + bool get_gyro_health(uint8_t instance) const { return _gyro_healthy[instance]; } + bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); } bool get_gyro_health_all(void) const; - virtual uint8_t get_gyro_count(void) const { return 1; }; + uint8_t get_gyro_count(void) const { return _gyro_count; } bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; } bool gyro_calibrated_ok_all() const; - virtual bool get_accel_health(uint8_t instance) const { return true; } - bool get_accel_health(void) const { return get_accel_health(get_primary_accel()); } + bool get_accel_health(uint8_t instance) const { return _accel_healthy[instance]; } + bool get_accel_health(void) const { return get_accel_health(_primary_accel); } bool get_accel_health_all(void) const; - virtual uint8_t get_accel_count(void) const { return 1; }; + uint8_t get_accel_count(void) const { return _accel_count; }; // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; } - const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(get_primary_accel()); } + const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); } // get accel scale const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; } - const Vector3f &get_accel_scale(void) const { return get_accel_scale(get_primary_accel()); } - - /* Update the sensor data, so that getters are nonblocking. - * Returns a bool of whether data was updated or not. - */ - virtual bool update() = 0; + const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); } /* get_delta_time returns the time period in seconds * overwhich the sensor data was collected */ - virtual float get_delta_time() const = 0; + float get_delta_time() const { return _delta_time; } // return the maximum gyro drift rate in radians/s/s. This // depends on what gyro chips are being used - virtual float get_gyro_drift_rate(void) = 0; + float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); } - // wait for a sample to be available, with timeout in milliseconds - virtual bool wait_for_sample(uint16_t timeout_ms) = 0; + // update gyro and accel values from accumulated samples + void update(void); + + // wait for a sample to be available + void wait_for_sample(void); // class level parameters static const struct AP_Param::GroupInfo var_info[]; @@ -169,24 +179,28 @@ public: } // get_filter - return filter in hz - virtual uint8_t get_filter() const { return _mpu6000_filter.get(); } + uint8_t get_filter() const { return _mpu6000_filter.get(); } - virtual uint16_t error_count(void) const { return 0; } - virtual bool healthy(void) const { return get_gyro_health() && get_accel_health(); } + // return the selected sample rate + Sample_rate get_sample_rate(void) const { return _sample_rate; } - virtual uint8_t get_primary_accel(void) const { return 0; } + uint16_t error_count(void) const { return 0; } + bool healthy(void) const { return get_gyro_health() && get_accel_health(); } -protected: + uint8_t get_primary_accel(void) const { return 0; } - virtual uint8_t _get_primary_gyro(void) const { return 0; } + // enable HIL mode + void set_hil_mode(void) { _hil_mode = true; } - // sensor specific init to be overwritten by descendant classes - virtual uint16_t _init_sensor( Sample_rate sample_rate ) = 0; +private: - // no-save implementations of accel and gyro initialisation routines - virtual void _init_accel(); + // load backend drivers + void _add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor &)); + void _detect_backends(void); - virtual void _init_gyro(); + // accel and gyro initialisation + void _init_accel(); + void _init_gyro(); #if !defined( __AVR_ATmega1280__ ) // Calibration routines borrowed from Rolfe Schmidt @@ -194,53 +208,98 @@ protected: // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde // _calibrate_accel - perform low level accel calibration - virtual bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); - virtual void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); - virtual void _calibrate_reset_matrices(float dS[6], float JS[6][6]); - virtual void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); - virtual void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); + bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale); + void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]); + void _calibrate_reset_matrices(float dS[6], float JS[6][6]); + void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]); + void _calculate_trim(Vector3f accel_sample, float& trim_roll, float& trim_pitch); #endif + // check if we have 3D accel calibration + void check_3D_calibration(void); + // save parameters to eeprom void _save_parameters(); - // Most recent accelerometer reading obtained by ::update + // backend objects + AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS]; + + // number of gyros and accel drivers. Note that most backends + // provide both accel and gyro data, so will increment both + // counters on initialisation + uint8_t _gyro_count; + uint8_t _accel_count; + uint8_t _backend_count; + + // the selected sample rate + Sample_rate _sample_rate; + + // Most recent accelerometer reading Vector3f _accel[INS_MAX_INSTANCES]; - // previous accelerometer reading obtained by ::update - Vector3f _previous_accel[INS_MAX_INSTANCES]; - - // Most recent gyro reading obtained by ::update + // Most recent gyro reading Vector3f _gyro[INS_MAX_INSTANCES]; // product id AP_Int16 _product_id; // accelerometer scaling and offsets - AP_Vector3f _accel_scale[INS_MAX_INSTANCES]; - AP_Vector3f _accel_offset[INS_MAX_INSTANCES]; - AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]; + AP_Vector3f _accel_scale[INS_MAX_INSTANCES]; + AP_Vector3f _accel_offset[INS_MAX_INSTANCES]; + AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]; // filtering frequency (0 means default) - AP_Int8 _mpu6000_filter; + AP_Int8 _mpu6000_filter; // board orientation from AHRS - enum Rotation _board_orientation; + enum Rotation _board_orientation; // calibrated_ok flags - bool _gyro_cal_ok[INS_MAX_INSTANCES]; + bool _gyro_cal_ok[INS_MAX_INSTANCES]; + + // primary accel and gyro + uint8_t _primary_gyro; + uint8_t _primary_accel; + + // has wait_for_sample() found a sample? + bool _have_sample:1; + + // are we in HIL mode? + bool _hil_mode:1; + + // do we have offsets/scaling from a 3D calibration? + bool _have_3D_calibration:1; + + // the delta time in seconds for the last sample + float _delta_time; + + // last time a wait_for_sample() returned a sample + uint32_t _last_sample_usec; + + // target time for next wait_for_sample() return + uint32_t _next_sample_usec; + + // time between samples in microseconds + uint32_t _sample_period_usec; + + // health of gyros and accels + bool _gyro_healthy[INS_MAX_INSTANCES]; + bool _accel_healthy[INS_MAX_INSTANCES]; + + uint32_t _accel_error_count[INS_MAX_INSTANCES]; + uint32_t _gyro_error_count[INS_MAX_INSTANCES]; }; -#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" -#include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_VRBRAIN.h" +#include "AP_InertialSensor_Oilpan.h" +#include "AP_InertialSensor_MPU9250.h" +#include "AP_InertialSensor_L3G4200D.h" +#include "AP_InertialSensor_Flymaple.h" +#include "AP_InertialSensor_MPU9150.h" +#include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" -#include "AP_InertialSensor_Flymaple.h" -#include "AP_InertialSensor_L3G4200D.h" -#include "AP_InertialSensor_MPU9150.h" -#include "AP_InertialSensor_MPU9250.h" #endif // __AP_INERTIAL_SENSOR_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index ccc481c086..684cb327e9 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,25 +63,39 @@ 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) { - // 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; - _default_filter_hz = 10; - break; - case RATE_100HZ: - _sample_divider = raw_sample_rate_hz / 100; - _default_filter_hz = 20; - break; - case RATE_200HZ: - default: - _sample_divider = raw_sample_rate_hz / 200; - _default_filter_hz = 20; - break; + AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu); + if (sensor == NULL) { + return NULL; } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_InertialSensor_Flymaple::_init_sensor(void) +{ + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -146,12 +146,17 @@ 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(); + + _product_id = AP_PRODUCT_ID_FLYMAPLE; + + return true; } /* @@ -170,109 +175,46 @@ 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; - // 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); // 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); - 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 +227,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 +242,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 +259,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 +267,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..724e03dbb4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -6,39 +6,31 @@ #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); 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(void); + 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 +38,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__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index bbb5e8d8ee..50bfb4d6b6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -1,130 +1,46 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "AP_InertialSensor_HIL.h" #include +#include "AP_InertialSensor_HIL.h" + const extern AP_HAL::HAL& hal; -AP_InertialSensor_HIL::AP_InertialSensor_HIL() : - AP_InertialSensor(), - _sample_period_usec(0), - _last_sample_usec(0) +AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu) { - _accel[0] = Vector3f(0, 0, -GRAVITY_MSS); } -uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) { - switch (sample_rate) { - case RATE_50HZ: - _sample_period_usec = 20000; - break; - case RATE_100HZ: - _sample_period_usec = 10000; - break; - case RATE_200HZ: - _sample_period_usec = 5000; - break; - case RATE_400HZ: - _sample_period_usec = 2500; - break; +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu); + if (sensor == NULL) { + return NULL; } - return AP_PRODUCT_ID_NONE; + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ +bool AP_InertialSensor_HIL::_init_sensor(void) +{ + // grab the used instances + _imu.register_gyro(); + _imu.register_accel(); + + _product_id = AP_PRODUCT_ID_NONE; + _imu.set_hil_mode(); -bool AP_InertialSensor_HIL::update( void ) { - uint32_t now = hal.scheduler->micros(); - _last_sample_usec = now; return true; } -float AP_InertialSensor_HIL::get_delta_time() const { - return _sample_period_usec * 1.0e-6f; -} - -float AP_InertialSensor_HIL::get_gyro_drift_rate(void) { - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - -bool AP_InertialSensor_HIL::_sample_available() +bool AP_InertialSensor_HIL::update(void) { - uint32_t tnow = hal.scheduler->micros(); - bool have_sample = false; - while (tnow - _last_sample_usec > _sample_period_usec) { - have_sample = true; - _last_sample_usec += _sample_period_usec; - } - return have_sample; + // the data is stored directly in the frontend, so update() + // doesn't need to do anything + return true; } - -bool AP_InertialSensor_HIL::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) { - uint32_t tnow = hal.scheduler->micros(); - uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow; - if (tdelay < 100000) { - hal.scheduler->delay_microseconds(tdelay); - } - if (_sample_available()) { - return true; - } - } - return false; -} - -void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel) -{ - if (instance >= INS_MAX_INSTANCES) { - return; - } - _previous_accel[instance] = _accel[instance]; - _accel[instance] = accel; - _last_accel_usec[instance] = hal.scheduler->micros(); -} - -void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro) -{ - if (instance >= INS_MAX_INSTANCES) { - return; - } - _gyro[instance] = gyro; - _last_gyro_usec[instance] = hal.scheduler->micros(); -} - -bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const -{ - if (instance >= INS_MAX_INSTANCES) { - return false; - } - return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000; -} - -bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const -{ - if (instance >= INS_MAX_INSTANCES) { - return false; - } - return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000; -} - -uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const -{ - if (get_gyro_health(1)) { - return 2; - } - return 1; -} - -uint8_t AP_InertialSensor_HIL::get_accel_count(void) const -{ - if (get_accel_health(1)) { - return 2; - } - return 1; -} - diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index 55ae828ad8..9c1422c662 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -1,36 +1,26 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#ifndef __AP_INERTIAL_SENSOR_STUB_H__ -#define __AP_INERTIAL_SENSOR_STUB_H__ +#ifndef __AP_INERTIALSENSOR_HIL_H__ +#define __AP_INERTIALSENSOR_HIL_H__ -#include #include "AP_InertialSensor.h" -class AP_InertialSensor_HIL : public AP_InertialSensor +class AP_InertialSensor_HIL : public AP_InertialSensor_Backend { public: + AP_InertialSensor_HIL(AP_InertialSensor &imu); - AP_InertialSensor_HIL(); + /* 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); - void set_accel(uint8_t instance, const Vector3f &accel); - void set_gyro(uint8_t instance, const Vector3f &gyro); - bool get_gyro_health(uint8_t instance) const; - bool get_accel_health(uint8_t instance) const; - uint8_t get_gyro_count(void) const; - uint8_t get_accel_count(void) const; + bool gyro_sample_available(void) { return true; } + bool accel_sample_available(void) { return true; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - bool _sample_available(); - uint16_t _init_sensor( Sample_rate sample_rate ); - uint32_t _sample_period_usec; - uint32_t _last_sample_usec; - uint32_t _last_accel_usec[INS_MAX_INSTANCES]; - uint32_t _last_gyro_usec[INS_MAX_INSTANCES]; + bool _init_sensor(void); }; -#endif // __AP_INERTIAL_SENSOR_STUB_H__ +#endif // __AP_INERTIALSENSOR_HIL_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 60281bd559..e1e4c5ce25 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -16,9 +16,17 @@ /* This is an INS driver for the combination L3G4200D gyro and ADXL345 accelerometer. This combination is available as a cheap 10DOF sensor on ebay - */ -// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf -// L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf + + This sensor driver is an example only - it should not be used in any + serious autopilot as the latencies on I2C prevent good timing at + high sample rates. It is useful when doing an initial port of + ardupilot to a board where only I2C is available, and a cheap sensor + can be used. + +Datasheets: + ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf + L3G4200D gyro http://www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/CD00265057.pdf +*/ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX @@ -103,8 +111,10 @@ const extern AP_HAL::HAL& hal; #define L3G4200D_GYRO_SCALE_R_S (DEG_TO_RAD * 70.0f * 0.001f) // constructor -AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() : - AP_InertialSensor(), +AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_gyro_sample(false), + _have_accel_sample(false), _accel_filter_x(800, 10), _accel_filter_y(800, 10), _accel_filter_z(800, 10), @@ -113,27 +123,9 @@ AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D() : _gyro_filter_z(800, 10) {} -uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) +bool AP_InertialSensor_L3G4200D::_init_sensor(void) { - - switch (sample_rate) { - case RATE_50HZ: - _default_filter_hz = 10; - _sample_period_usec = (1000*1000) / 50; - _gyro_samples_needed = 16; - break; - case RATE_100HZ: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 100; - _gyro_samples_needed = 8; - break; - case RATE_200HZ: - default: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 200; - _gyro_samples_needed = 4; - break; - } + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -219,7 +211,7 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) // Set up the filter desired - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); // give back i2c semaphore i2c_sem->give(); @@ -227,15 +219,12 @@ uint16_t AP_InertialSensor_L3G4200D::_init_sensor( Sample_rate sample_rate ) // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_L3G4200D::_accumulate)); - clock_gettime(CLOCK_MONOTONIC, &_next_sample_ts); + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); - _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL; - if (_next_sample_ts.tv_nsec >= 1.0e9) { - _next_sample_ts.tv_nsec -= 1.0e9; - _next_sample_ts.tv_sec++; - } + _product_id = AP_PRODUCT_ID_L3G4200D; - return AP_PRODUCT_ID_L3G4200D; + return true; } /* @@ -254,58 +243,36 @@ void AP_InertialSensor_L3G4200D::_set_filter_frequency(uint8_t filter_hz) _gyro_filter_z.set_cutoff_frequency(800, filter_hz); } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - +/* + copy filtered data to the frontend + */ bool AP_InertialSensor_L3G4200D::update(void) { - Vector3f accel_scale = _accel_scale[0].get(); - - _previous_accel[0] = _accel[0]; + Vector3f accel, gyro; hal.scheduler->suspend_timer_procs(); - _accel[0] = _accel_filtered; - _gyro[0] = _gyro_filtered; - _delta_time = _gyro_samples_available * (1.0f/800); - _gyro_samples_available = 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] *= ADXL345_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 *= ADXL345_ACCELEROMETER_SCALE_M_S; + _rotate_and_offset_accel(_accel_instance, accel); // Adjust for chip scaling to get radians/sec - _gyro[0] *= L3G4200D_GYRO_SCALE_R_S; - _gyro[0] -= _gyro_offset[0]; + gyro *= L3G4200D_GYRO_SCALE_R_S; + _rotate_and_offset_gyro(_gyro_instance, gyro); - 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; } -float AP_InertialSensor_L3G4200D::get_delta_time(void) const -{ - return _delta_time; -} - -float AP_InertialSensor_L3G4200D::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute (a guess) - return ToRad(0.5/60); -} - // Accumulate values from accels and gyros void AP_InertialSensor_L3G4200D::_accumulate(void) { @@ -344,19 +311,17 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) _gyro_filtered = Vector3f(_gyro_filter_x.apply(buffer[i][0]), _gyro_filter_y.apply(-buffer[i][1]), _gyro_filter_z.apply(-buffer[i][2])); - _gyro_samples_available++; + _have_gyro_sample = true; } } } - // Read accelerometer FIFO to find out how many samples are available hal.i2c->readRegister(ADXL345_ACCELEROMETER_ADDRESS, ADXL345_ACCELEROMETER_ADXLREG_FIFO_STATUS, &fifo_status); num_samples_available = fifo_status & 0x3F; -#if 1 // read the samples and apply the filter if (num_samples_available > 0) { int16_t buffer[num_samples_available][3]; @@ -368,35 +333,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) _accel_filtered = Vector3f(_accel_filter_x.apply(buffer[i][0]), _accel_filter_y.apply(-buffer[i][1]), _accel_filter_z.apply(-buffer[i][2])); + _have_accel_sample = true; } } } -#endif // give back i2c semaphore i2c_sem->give(); } -bool AP_InertialSensor_L3G4200D::_sample_available(void) -{ - return (_gyro_samples_available >= _gyro_samples_needed); -} - -bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms) -{ - uint32_t start_us = hal.scheduler->micros(); - while (clock_nanosleep(CLOCK_MONOTONIC, - TIMER_ABSTIME, &_next_sample_ts, NULL) == -1 && errno == EINTR) ; - _next_sample_ts.tv_nsec += _sample_period_usec * 1000UL; - if (_next_sample_ts.tv_nsec >= 1.0e9) { - _next_sample_ts.tv_nsec -= 1.0e9; - _next_sample_ts.tv_sec++; - - } - //_accumulate(); - return true; - -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 5aa035a6aa..674dc87c15 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -6,40 +6,39 @@ #include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include #include "AP_InertialSensor.h" #include #include -class AP_InertialSensor_L3G4200D : public AP_InertialSensor +class AP_InertialSensor_L3G4200D : public AP_InertialSensor_Backend { public: - AP_InertialSensor_L3G4200D(); + AP_InertialSensor_L3G4200D(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); + /* update accel and gyro state */ + bool update(); + + bool gyro_sample_available(void) { return _have_gyro_sample; } + bool accel_sample_available(void) { return _have_accel_sample; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); + + // return product ID + int16_t product_id(void) const { return AP_PRODUCT_ID_L3G4200D; } private: - uint16_t _init_sensor( Sample_rate sample_rate ); - void _accumulate(void); - bool _sample_available(); - uint64_t _last_update_usec; + bool _init_sensor(void); + void _accumulate(void); Vector3f _accel_filtered; Vector3f _gyro_filtered; - uint32_t _sample_period_usec; - uint32_t _last_sample_time; - volatile uint32_t _gyro_samples_available; - volatile uint8_t _gyro_samples_needed; - float _delta_time; - struct timespec _next_sample_ts; + volatile bool _have_gyro_sample; + volatile bool _have_accel_sample; // support for updating filter at runtime uint8_t _last_filter_hz; - uint8_t _default_filter_hz; + uint8_t _default_filter_hz; void _set_filter_frequency(uint8_t filter_hz); @@ -50,6 +49,10 @@ private: LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_z; + + // gyro and accel instances + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif #endif // __AP_INERTIAL_SENSOR_L3G4200D_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index a79e01dd99..849851fb88 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -173,26 +173,51 @@ const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532f / 16.4f); * variants however */ -AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000() : - AP_InertialSensor(), +AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), _drdy_pin(NULL), _spi(NULL), _spi_sem(NULL), - _num_samples(0), - _last_sample_time_micros(0), - _initialised(false), - _mpu6000_product_id(AP_PRODUCT_ID_NONE), - _sample_shift(0), _last_filter_hz(0), - _error_count(0) + _error_count(0), +#if MPU6000_FAST_SAMPLING + _accel_filter_x(1000, 15), + _accel_filter_y(1000, 15), + _accel_filter_z(1000, 15), + _gyro_filter_x(1000, 15), + _gyro_filter_y(1000, 15), + _gyro_filter_z(1000, 15), +#else + _sample_count(0), + _accel_sum(), + _gyro_sum(), +#endif + _sum_count(0) { } -uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::detect(AP_InertialSensor &_imu) { - if (_initialised) return _mpu6000_product_id; - _initialised = true; + AP_InertialSensor_MPU6000 *sensor = new AP_InertialSensor_MPU6000(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +/* + initialise the sensor + */ +bool AP_InertialSensor_MPU6000::_init_sensor(void) +{ _spi = hal.spi->device(AP_HAL::SPIDevice_MPU6000); _spi_sem = _spi->get_semaphore(); @@ -205,103 +230,85 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) uint8_t tries = 0; do { - bool success = _hardware_init(sample_rate); + bool success = _hardware_init(); if (success) { hal.scheduler->delay(5+2); if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); + return false; } if (_data_ready()) { _spi_sem->give(); break; } else { - hal.console->println_P( - PSTR("MPU6000 startup failed: no data ready")); + return false; } _spi_sem->give(); } if (tries++ > 5) { - hal.scheduler->panic(PSTR("PANIC: failed to boot MPU6000 5 times")); + hal.console->print_P(PSTR("failed to boot MPU6000 5 times")); + return false; } } while (1); + // grab the used instances + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + hal.scheduler->resume_timer_procs(); - - /* read the first lot of data. - * _read_data_transaction requires the spi semaphore to be taken by - * its caller. */ - _last_sample_time_micros = hal.scheduler->micros(); - hal.scheduler->delay(10); - if (_spi_sem->take(100)) { - _read_data_transaction(); - _spi_sem->give(); - } - // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU6000::_poll_data)); #if MPU6000_DEBUG _dump_registers(); #endif - return _mpu6000_product_id; + + return true; } -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_MPU6000::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; -} +/* + process any + */ bool AP_InertialSensor_MPU6000::update( void ) -{ - // wait for at least 1 sample - if (!wait_for_sample(1000)) { +{ +#if !MPU6000_FAST_SAMPLING + if (_sum_count < _sample_count) { + // we don't have enough samples yet return false; } +#endif - _previous_accel[0] = _accel[0]; + // we have a full set of samples + uint16_t num_samples; + Vector3f accel, gyro; - // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); - _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); - _num_samples = _sum_count; +#if MPU6000_FAST_SAMPLING + gyro = _gyro_filtered; + accel = _accel_filtered; + num_samples = 1; +#else + 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(); +#endif _sum_count = 0; hal.scheduler->resume_timer_procs(); - _gyro[0].rotate(_board_orientation); - _gyro[0] *= _gyro_scale / _num_samples; - _gyro[0] -= _gyro_offset[0]; + gyro *= _gyro_scale / num_samples; + _rotate_and_offset_gyro(_gyro_instance, gyro); - _accel[0].rotate(_board_orientation); - _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; + accel *= MPU6000_ACCEL_SCALE_1G / num_samples; + _rotate_and_offset_accel(_accel_instance, accel); - Vector3f accel_scale = _accel_scale[0].get(); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; - - if (_last_filter_hz != _mpu6000_filter) { + if (_last_filter_hz != _imu.get_filter()) { if (_spi_sem->take(10)) { _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - _set_filter_register(_mpu6000_filter, 0); + _set_filter_register(_imu.get_filter()); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _error_count = 0; _spi_sem->give(); } } @@ -331,35 +338,13 @@ bool AP_InertialSensor_MPU6000::_data_ready() */ void AP_InertialSensor_MPU6000::_poll_data(void) { - if (hal.scheduler->in_timerprocess()) { - if (!_spi_sem->take_nonblocking()) { - /* - the semaphore being busy is an expected condition when the - mainline code is calling wait_for_sample() which will - grab the semaphore. We return now and rely on the mainline - code grabbing the latest sample. - */ - return; - } - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - /* Synchronous read - take semaphore */ - if (_spi_sem->take(10)) { - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - hal.scheduler->panic( - PSTR("PANIC: AP_InertialSensor_MPU6000::_poll_data " - "failed to take SPI semaphore synchronously")); - } + if (!_spi_sem->take_nonblocking()) { + return; + } + if (_data_ready()) { + _read_data_transaction(); } + _spi_sem->give(); } @@ -390,19 +375,31 @@ void AP_InertialSensor_MPU6000::_read_data_transaction() { } #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) +#if MPU6000_FAST_SAMPLING + _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), + _accel_filter_y.apply(int16_val(rx.v, 0)), + _accel_filter_z.apply(-int16_val(rx.v, 2))); + + _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), + _gyro_filter_y.apply(int16_val(rx.v, 4)), + _gyro_filter_z.apply(-int16_val(rx.v, 6))); +#else _accel_sum.x += int16_val(rx.v, 1); _accel_sum.y += int16_val(rx.v, 0); _accel_sum.z -= int16_val(rx.v, 2); _gyro_sum.x += int16_val(rx.v, 5); _gyro_sum.y += int16_val(rx.v, 4); _gyro_sum.z -= int16_val(rx.v, 6); +#endif _sum_count++; +#if !MPU6000_FAST_SAMPLING if (_sum_count == 0) { // rollover - v unlikely _accel_sum.zero(); _gyro_sum.zero(); } +#endif } uint8_t AP_InertialSensor_MPU6000::_register_read( uint8_t reg ) @@ -448,36 +445,30 @@ void AP_InertialSensor_MPU6000::_register_write_check(uint8_t reg, uint8_t val) /* set the DLPF filter frequency. Assumes caller has taken semaphore */ -void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz) { - uint8_t filter = default_filter; + if (filter_hz == 0) { + filter_hz = _default_filter(); + } + uint8_t filter; // choose filtering frequency - switch (filter_hz) { - case 5: + if (filter_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; - break; - case 10: + } else if (filter_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; - break; - case 20: + } else if (filter_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; - break; - case 42: + } else if (filter_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; - break; - case 98: + } else { filter = BITS_DLPF_CFG_98HZ; - break; - } - - if (filter != 0) { - _last_filter_hz = filter_hz; - _register_write(MPUREG_CONFIG, filter); } + _last_filter_hz = filter_hz; + _register_write(MPUREG_CONFIG, filter); } -bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) +bool AP_InertialSensor_MPU6000::_hardware_init(void) { if (!_spi_sem->take(100)) { hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore")); @@ -519,46 +510,53 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); hal.scheduler->delay(1); - uint8_t default_filter; - +#if MPU6000_FAST_SAMPLING + _sample_count = 1; +#else // sample rate and filtering // to minimise the effects of aliasing we choose a filter // that is less than half of the sample rate - switch (sample_rate) { - case RATE_50HZ: + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: // this is used for plane and rover, where noise resistance is // more important than update rate. Tests on an aerobatic plane // show that 10Hz is fine, and makes it very noise resistant - default_filter = BITS_DLPF_CFG_10HZ; - _sample_shift = 2; + _sample_count = 4; break; - case RATE_100HZ: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 1; + case AP_InertialSensor::RATE_100HZ: + _sample_count = 2; + break; + case AP_InertialSensor::RATE_200HZ: + _sample_count = 1; break; - case RATE_200HZ: default: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 0; - break; + return false; } +#endif - _set_filter_register(_mpu6000_filter, default_filter); + _set_filter_register(_imu.get_filter()); +#if MPU6000_FAST_SAMPLING + // set sample rate to 1000Hz and apply a software filter + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ); +#else // set sample rate to 200Hz, and use _sample_divider to give // the requested rate to the application _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); +#endif hal.scheduler->delay(1); _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000ยบ/s hal.scheduler->delay(1); // read the product ID rev c has 1/2 the sensitivity of rev d - _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); + _product_id = _register_read(MPUREG_PRODUCT_ID); //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || - (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { + if ((_product_id == MPU6000ES_REV_C4) || + (_product_id == MPU6000ES_REV_C5) || + (_product_id == MPU6000_REV_C4) || + (_product_id == MPU6000_REV_C5)) { // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D _register_write(MPUREG_ACCEL_CONFIG,1<<3); @@ -585,22 +583,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(Sample_rate sample_rate) return true; } -// return the MPU6k gyro drift rate in radian/s/s -// note that this is much better than the oilpan gyros -float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - -// return true if a sample is available -bool AP_InertialSensor_MPU6000::_sample_available() -{ - _poll_data(); - return (_sum_count >> _sample_shift) > 0; -} - - #if MPU6000_DEBUG // dump all config registers - used for debug void AP_InertialSensor_MPU6000::_dump_registers(void) @@ -619,11 +601,3 @@ void AP_InertialSensor_MPU6000::_dump_registers(void) } } #endif - - -// get_delta_time returns the time period in seconds overwhich the sensor data was collected -float AP_InertialSensor_MPU6000::get_delta_time() const -{ - // the sensor runs at 200Hz - return 0.005 * _num_samples; -} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index f6567d849d..3fa58d04c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -12,33 +12,44 @@ // enable debug to see a register dump on startup #define MPU6000_DEBUG 0 -class AP_InertialSensor_MPU6000 : public AP_InertialSensor +// on fast CPUs we sample at 1kHz and use a software filter +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 +#define MPU6000_FAST_SAMPLING 1 +#else +#define MPU6000_FAST_SAMPLING 0 +#endif + +#if MPU6000_FAST_SAMPLING +#include +#include +#endif + +class AP_InertialSensor_MPU6000 : public AP_InertialSensor_Backend { public: + AP_InertialSensor_MPU6000(AP_InertialSensor &imu); - AP_InertialSensor_MPU6000(); + /* update accel and gyro state */ + bool update(); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_gyro_drift_rate(); + bool gyro_sample_available(void) { return _sum_count >= _sample_count; } + bool accel_sample_available(void) { return _sum_count >= _sample_count; } - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); - - // get_delta_time returns the time period in seconds overwhich the sensor data was collected - float get_delta_time() const; - - uint16_t error_count(void) const { return _error_count; } - bool healthy(void) const { return _error_count <= 4; } - bool get_gyro_health(uint8_t instance) const { return healthy(); } - bool get_accel_health(uint8_t instance) const { return healthy(); } - -protected: - uint16_t _init_sensor( Sample_rate sample_rate ); + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: +#if MPU6000_DEBUG + void _dump_registers(void); +#endif + + // instance numbers of accel and gyro data + uint8_t _gyro_instance; + uint8_t _accel_instance; + AP_HAL::DigitalSource *_drdy_pin; + bool _init_sensor(void); bool _sample_available(); void _read_data_transaction(); bool _data_ready(); @@ -46,41 +57,42 @@ private: uint8_t _register_read( uint8_t reg ); void _register_write( uint8_t reg, uint8_t val ); void _register_write_check(uint8_t reg, uint8_t val); - bool _hardware_init(Sample_rate sample_rate); + bool _hardware_init(void); AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; - uint16_t _num_samples; static const float _gyro_scale; - uint32_t _last_sample_time_micros; - - // ensure we can't initialise twice - bool _initialised; - int16_t _mpu6000_product_id; - - // how many hardware samples before we report a sample to the caller - uint8_t _sample_shift; - // support for updating filter at runtime uint8_t _last_filter_hz; - void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + void _set_filter_register(uint8_t filter_hz); + // count of bus errors uint16_t _error_count; + // how many hardware samples before we report a sample to the caller + uint8_t _sample_count; + +#if MPU6000_FAST_SAMPLING + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + + // Low Pass filters for gyro and accel + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; +#else // accumulation in timer - must be read with timer disabled // the sum of the values since last read Vector3l _accel_sum; Vector3l _gyro_sum; - volatile int16_t _sum_count; - -public: - -#if MPU6000_DEBUG - void _dump_registers(void); #endif + volatile uint16_t _sum_count; }; #endif // __AP_INERTIAL_SENSOR_MPU6000_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp index 44cd2b117a..95130f12eb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp @@ -19,6 +19,10 @@ Please check the following links for datasheets and documentation: - http://www.invensense.com/mems/gyro/documents/PS-MPU-9150A-00v4_3.pdf - http://www.invensense.com/mems/gyro/documents/RM-MPU-9150A-00v4_2.pdf + + Note that this is an experimental driver. It is not used by any + actively maintained board and should be considered untested and + unmaintained */ #include @@ -320,19 +324,33 @@ static struct gyro_state_s st = { /** * @brief Constructor */ -AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() : - AP_InertialSensor(), +AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _have_sample_available(false), _accel_filter_x(800, 10), _accel_filter_y(800, 10), _accel_filter_z(800, 10), _gyro_filter_x(800, 10), _gyro_filter_y(800, 10), - _gyro_filter_z(800, 10) - // _mag_filter_x(800, 10), - // _mag_filter_y(800, 10), - // _mag_filter_z(800, 10) + _gyro_filter_z(800, 10) { +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU9150::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_MPU9150 *sensor = new AP_InertialSensor_MPU9150(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; } /* @@ -352,41 +370,21 @@ void AP_InertialSensor_MPU9150::_set_filter_frequency(uint8_t filter_hz) } /** - * @brief Init method - * @param[in] Sample_rate The sample rate, check the struct def. - * @return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE if successful. + * Init method */ -uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) +bool AP_InertialSensor_MPU9150::_init_sensor(void) { // Sensors pushed to the FIFO. uint8_t sensors; - switch (sample_rate) { - case RATE_50HZ: - _default_filter_hz = 10; - _sample_period_usec = (1000*1000) / 50; - break; - case RATE_100HZ: - _default_filter_hz = 20; - _sample_period_usec = (1000*1000) / 100; - break; - case RATE_200HZ: - _default_filter_hz = 20; - _sample_period_usec = 5000; - break; - case RATE_400HZ: - default: - _default_filter_hz = 20; - _sample_period_usec = 2500; - break; - } + _default_filter_hz = _default_filter(); // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); // take i2c bus sempahore if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)){ - return -1; + return false; } // Init the sensor @@ -405,8 +403,8 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) // This registers are not documented in the register map. uint8_t buff[6]; if (hal.i2c->readRegisters(st.hw->addr, st.reg->accel_offs, 6, buff) != 0) { - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision")); - goto failed; + hal.console->printf("AP_InertialSensor_MPU9150: couldn't read the registers to determine revision"); + goto failed; } uint8_t rev; rev = ((buff[5] & 0x01) << 2) | ((buff[3] & 0x01) << 1) | @@ -432,28 +430,28 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) // Set gyro full-scale range [250, 500, 1000, 2000] if (mpu_set_gyro_fsr(2000)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_gyro_fsr.\n"); goto failed; } // Set the accel full-scale range if (mpu_set_accel_fsr(2)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_accel_fsr.\n"); goto failed; } // Set digital low pass filter (using _default_filter_hz, 20 for 100 Hz of sample rate) if (mpu_set_lpf(_default_filter_hz)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_lpf.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_lpf.\n"); goto failed; } // Set sampling rate (value must be between 4Hz and 1KHz) if (mpu_set_sample_rate(800)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"); goto failed; } // Select which sensors are pushed to FIFO. sensors = INV_XYZ_ACCEL| INV_XYZ_GYRO; if (mpu_configure_fifo(sensors)){ - hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n")); + hal.console->printf("AP_InertialSensor_MPU9150: mpu_configure_fifo.\n"); goto failed; } @@ -467,18 +465,23 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate ) mpu_set_sensors(sensors); // Set the filter frecuency (_mpu6000_filter configured to the default value, check AP_InertialSensor.cpp) - _set_filter_frequency(_mpu6000_filter); + _set_filter_frequency(_imu.get_filter()); // give back i2c semaphore i2c_sem->give(); + + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9150::_accumulate)); - return AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE; - failed: - // give back i2c semaphore - i2c_sem->give(); - return -1; + return true; + +failed: + // give back i2c semaphore + i2c_sem->give(); + return false; } /** @@ -1017,9 +1020,9 @@ int16_t AP_InertialSensor_MPU9150::mpu_read_fifo(int16_t *gyro, int16_t *accel, * @brief Accumulate values from accels and gyros. * * This method is called periodically by the scheduler. - * */ -void AP_InertialSensor_MPU9150::_accumulate(void){ +void AP_InertialSensor_MPU9150::_accumulate(void) +{ // get pointer to i2c bus semaphore AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); @@ -1094,102 +1097,36 @@ void AP_InertialSensor_MPU9150::_accumulate(void){ _gyro_filter_y.apply(gyro_y), _gyro_filter_z.apply(gyro_z)); - _gyro_samples_available++; + _have_sample_available = true; } // give back i2c semaphore i2c_sem->give(); } -bool AP_InertialSensor_MPU9150::_sample_available(void) -{ - uint64_t tnow = hal.scheduler->micros(); - while (tnow - _last_sample_timestamp > _sample_period_usec) { - _have_sample_available = true; - _last_sample_timestamp += _sample_period_usec; - } - return _have_sample_available; -} - -bool AP_InertialSensor_MPU9150::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) { - uint64_t tnow = hal.scheduler->micros(); - // 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_period_usec > tnow+timing_lag) { - hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag)); - } - if (_sample_available()) { - return true; - } - } - return false; -} - bool AP_InertialSensor_MPU9150::update(void) { - if (!wait_for_sample(1000)) { - return false; - } - Vector3f accel_scale = _accel_scale[0].get(); - - _previous_accel[0] = _accel[0]; - - // hal.scheduler->suspend_timer_procs(); - _accel[0] = _accel_filtered; - _gyro[0] = _gyro_filtered; - // 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] *= MPU9150_ACCEL_SCALE_2G/_gyro_samples_available; - - // 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); - - // Adjust for chip scaling to get radians/sec - _gyro[0] *= MPU9150_GYRO_SCALE_2000 / _gyro_samples_available; - _gyro[0] -= _gyro_offset[0]; - //////////////////////////////////////////////// - - _gyro_samples_available = 0; - - if (_last_filter_hz != _mpu6000_filter) { - _set_filter_frequency(_mpu6000_filter); - _last_filter_hz = _mpu6000_filter; - } + Vector3f accel, gyro; + hal.scheduler->suspend_timer_procs(); + accel = _accel_filtered; + gyro = _gyro_filtered; _have_sample_available = false; + hal.scheduler->resume_timer_procs(); + + accel *= MPU9150_ACCEL_SCALE_2G; + _rotate_and_offset_accel(_accel_instance, accel); + + gyro *= MPU9150_GYRO_SCALE_2000; + _rotate_and_offset_gyro(_gyro_instance, gyro); + + if (_last_filter_hz != _imu.get_filter()) { + _set_filter_frequency(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); + } return true; } -// TODO review to make sure it matches -float AP_InertialSensor_MPU9150::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute (a guess) - return ToRad(0.5/60); -} - -// TODO review to make sure it matches -float AP_InertialSensor_MPU9150::get_delta_time(void) const -{ - return _sample_period_usec * 1.0e-6f; -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h index a1d31fadab..52de4f9d71 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h @@ -12,33 +12,30 @@ #include -class AP_InertialSensor_MPU9150 : public AP_InertialSensor +class AP_InertialSensor_MPU9150 : public AP_InertialSensor_Backend { public: + AP_InertialSensor_MPU9150(AP_InertialSensor &imu); - AP_InertialSensor_MPU9150(); + /* update accel and gyro state */ + bool update(); - /* 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 gyro_sample_available(void) { return _have_sample_available; } + bool accel_sample_available(void) { return _have_sample_available; } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - uint16_t _init_sensor( Sample_rate sample_rate ); + bool _init_sensor(); void _accumulate(void); - bool _sample_available(); - // uint64_t _last_update_usec; Vector3f _accel_filtered; Vector3f _gyro_filtered; - uint32_t _sample_period_usec; - volatile uint32_t _gyro_samples_available; - uint64_t _last_sample_timestamp; - bool _have_sample_available; + bool _have_sample_available; // // support for updating filter at runtime uint8_t _last_filter_hz; - uint8_t _default_filter_hz; + uint8_t _default_filter_hz; int16_t mpu_set_gyro_fsr(uint16_t fsr); int16_t mpu_set_accel_fsr(uint8_t fsr); @@ -52,7 +49,6 @@ private: int16_t mpu_set_int_latched(uint8_t enable); int16_t mpu_read_fifo(int16_t *gyro, int16_t *accel, uint32_t timestamp, uint8_t *sensors, uint8_t *more); - // Filter (specify which one) void _set_filter_frequency(uint8_t filter_hz); // Low Pass filters for gyro and accel @@ -62,11 +58,9 @@ private: LowPassFilter2p _gyro_filter_x; LowPassFilter2p _gyro_filter_y; LowPassFilter2p _gyro_filter_z; - // LowPassFilter2p _mag_filter_x; - // LowPassFilter2p _mag_filter_y; - // LowPassFilter2p _mag_filter_z; - + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif #endif // __AP_INERTIAL_SENSOR_MPU9150_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 6eefd8f2cd..0c8cd32112 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -21,7 +21,6 @@ #include "AP_InertialSensor_MPU9250.h" #include "../AP_HAL_Linux/GPIO.h" -#include extern const AP_HAL::HAL& hal; @@ -158,29 +157,11 @@ extern const AP_HAL::HAL& hal; #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 #define BITS_DLPF_CFG_MASK 0x07 - -// // TODO Remove -// // Product ID Description for MPU6000 -// // high 4 bits low 4 bits -// // Product Name Product Revision -// #define MPU6000ES_REV_C4 0x14 // 0001 0100 -// #define MPU6000ES_REV_C5 0x15 // 0001 0101 -// #define MPU6000ES_REV_D6 0x16 // 0001 0110 -// #define MPU6000ES_REV_D7 0x17 // 0001 0111 -// #define MPU6000ES_REV_D8 0x18 // 0001 1000 -// #define MPU6000_REV_C4 0x54 // 0101 0100 -// #define MPU6000_REV_C5 0x55 // 0101 0101 -// #define MPU6000_REV_D6 0x56 // 0101 0110 -// #define MPU6000_REV_D7 0x57 // 0101 0111 -// #define MPU6000_REV_D8 0x58 // 0101 1000 -// #define MPU6000_REV_D9 0x59 // 0101 1001 - - /* * PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) */ -const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); +#define GYRO_SCALE (0.0174532f / 16.4f) /* * PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of @@ -190,27 +171,48 @@ const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); * variants however */ -AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() : - AP_InertialSensor(), - _drdy_pin(NULL), - _initialised(false), - _mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE) +AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu), + _last_filter_hz(-1), + _shared_data_idx(0), + _accel_filter_x(1000, 15), + _accel_filter_y(1000, 15), + _accel_filter_z(1000, 15), + _gyro_filter_x(1000, 15), + _gyro_filter_y(1000, 15), + _gyro_filter_z(1000, 15), + _have_sample_available(false) { } -uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) -{ - if (_initialised) return _mpu9250_product_id; - _initialised = true; +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + + return sensor; +} + +/* + initialise the sensor + */ +bool AP_InertialSensor_MPU9250::_init_sensor(void) +{ _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); _spi_sem = _spi->get_semaphore(); -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE - _drdy_pin = hal.gpio->channel(BBB_P8_7); - _drdy_pin->mode(HAL_GPIO_INPUT); -#endif - + // we need to suspend timers to prevent other SPI drivers grabbing + // the bus while we do the long initialisation hal.scheduler->suspend_timer_procs(); uint8_t whoami = _register_read(MPUREG_WHOAMI); @@ -218,43 +220,36 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) // TODO: we should probably accept multiple chip // revisions. This is the one on the PXF hal.console->printf("MPU9250: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - hal.scheduler->panic("MPU9250: bad WHOAMI"); + return false; } uint8_t tries = 0; do { - bool success = _hardware_init(sample_rate); + bool success = _hardware_init(); if (success) { - hal.scheduler->delay(5+2); + hal.scheduler->delay(10); if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); + hal.console->printf("MPU9250: Unable to get semaphore"); + return false; } - if (_data_ready()) { + uint8_t status = _register_read(MPUREG_INT_STATUS); + if ((status & BIT_RAW_RDY_INT) != 0) { _spi_sem->give(); break; - } else { - hal.console->println_P( - PSTR("MPU9250 startup failed: no data ready")); } _spi_sem->give(); } if (tries++ > 5) { - hal.scheduler->panic(PSTR("PANIC: failed to boot MPU9250 5 times")); + return false; } } while (1); hal.scheduler->resume_timer_procs(); + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); - /* read the first lot of data. - * _read_data_transaction requires the spi semaphore to be taken by - * its caller. */ - _last_sample_time_micros = hal.scheduler->micros(); - hal.scheduler->delay(10); - if (_spi_sem->take(100)) { - _read_data_transaction(); - _spi_sem->give(); - } + _product_id = AP_PRODUCT_ID_MPU9250; // start the timer process to read samples hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_MPU9250::_poll_data)); @@ -262,70 +257,45 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) #if MPU9250_DEBUG _dump_registers(); #endif - return _mpu9250_product_id; -} - -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_MPU9250::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; + return true; } +/* + update the accel and gyro vectors + */ bool AP_InertialSensor_MPU9250::update( void ) { - // wait for at least 1 sample - if (!wait_for_sample(1000)) { - return false; - } + // pull the data from the timer shared data buffer + uint8_t idx = _shared_data_idx; + Vector3f gyro = _shared_data[idx]._gyro_filtered; + Vector3f accel = _shared_data[idx]._accel_filtered; - _previous_accel[0] = _accel[0]; + _have_sample_available = false; - // disable timer procs for mininum time - hal.scheduler->suspend_timer_procs(); - _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel[0] = Vector3f(_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[0].rotate(_board_orientation); - _gyro[0] *= _gyro_scale / _num_samples; - _gyro[0] -= _gyro_offset[0]; - - _accel[0].rotate(_board_orientation); - _accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples; + accel *= MPU9250_ACCEL_SCALE_1G; + gyro *= GYRO_SCALE; // rotate for bbone default - _accel[0].rotate(ROTATION_ROLL_180_YAW_90); - _gyro[0].rotate(ROTATION_ROLL_180_YAW_90); + accel.rotate(ROTATION_ROLL_180_YAW_90); + gyro.rotate(ROTATION_ROLL_180_YAW_90); - Vector3f accel_scale = _accel_scale[0].get(); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] -= _accel_offset[0]; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + // PXF has an additional YAW 180 + accel.rotate(ROTATION_YAW_180); + gyro.rotate(ROTATION_YAW_180); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO + // NavIO has different orientation, assuming RaspberryPi is right + // way up, and PWM pins on NavIO are at the back of the aircraft + accel.rotate(ROTATION_ROLL_180_YAW_90); + gyro.rotate(ROTATION_ROLL_180_YAW_90); +#endif - if (_last_filter_hz != _mpu6000_filter) { - if (_spi_sem->take(10)) { - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - _set_filter_register(_mpu6000_filter, 0); - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _error_count = 0; - _spi_sem->give(); - } + _rotate_and_offset_gyro(_gyro_instance, gyro); + _rotate_and_offset_accel(_accel_instance, accel); + + if (_last_filter_hz != _imu.get_filter()) { + _set_filter(_imu.get_filter()); + _last_filter_hz = _imu.get_filter(); } return true; @@ -333,59 +303,30 @@ bool AP_InertialSensor_MPU9250::update( void ) /*================ HARDWARE FUNCTIONS ==================== */ -/** - * Return true if the MPU9250 has new data available for reading. - * - * We use the data ready pin if it is available. Otherwise, read the - * status register. - */ -bool AP_InertialSensor_MPU9250::_data_ready() -{ - if (_drdy_pin) { - return _drdy_pin->read() != 0; - } - uint8_t status = _register_read(MPUREG_INT_STATUS); - return (status & BIT_RAW_RDY_INT) != 0; -} - /** * Timer process to poll for new data from the MPU9250. */ void AP_InertialSensor_MPU9250::_poll_data(void) { - if (hal.scheduler->in_timerprocess()) { - if (!_spi_sem->take_nonblocking()) { - /* - the semaphore being busy is an expected condition when the - mainline code is calling wait_for_sample() which will - grab the semaphore. We return now and rely on the mainline - code grabbing the latest sample. - */ - return; - } - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - /* Synchronous read - take semaphore */ - if (_spi_sem->take(10)) { - if (_data_ready()) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction(); - } - _spi_sem->give(); - } else { - hal.scheduler->panic( - PSTR("PANIC: AP_InertialSensor_MPU9250::_poll_data " - "failed to take SPI semaphore synchronously")); - } + if (!_spi_sem->take_nonblocking()) { + /* + the semaphore being busy is an expected condition when the + mainline code is calling wait_for_sample() which will + grab the semaphore. We return now and rely on the mainline + code grabbing the latest sample. + */ + return; } + _read_data_transaction(); + _spi_sem->give(); } -void AP_InertialSensor_MPU9250::_read_data_transaction() { +/* + read from the data registers and update filtered data + */ +void AP_InertialSensor_MPU9250::_read_data_transaction() +{ /* one resister address followed by seven 2-byte registers */ struct PACKED { uint8_t cmd; @@ -395,45 +336,27 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() { _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); - if (_drdy_pin) { - if (_drdy_pin->read() != 0) { - // data ready should have gone low after a read - printf("MPU9250: DRDY didn't go low\n"); - } - } - - /* - detect a bad SPI bus transaction by looking for all 14 bytes - zero, or the wrong INT_STATUS register value. This is used to - detect a too high SPI bus speed. - */ - uint8_t i; - for (i=0; i<14; i++) { - if (rx.v[i] != 0) break; - } - if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) { - // likely a bad bus transaction - if (++_error_count > 4) { - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - } - } - #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) - _accel_sum.x += int16_val(rx.v, 1); - _accel_sum.y += int16_val(rx.v, 0); - _accel_sum.z -= int16_val(rx.v, 2); - _gyro_sum.x += int16_val(rx.v, 5); - _gyro_sum.y += int16_val(rx.v, 4); - _gyro_sum.z -= int16_val(rx.v, 6); - _sum_count++; - if (_sum_count == 0) { - // rollover - v unlikely - _accel_sum.zero(); - _gyro_sum.zero(); - } + Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)), + _accel_filter_y.apply(int16_val(rx.v, 0)), + _accel_filter_z.apply(-int16_val(rx.v, 2))); + + Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), + _gyro_filter_y.apply(int16_val(rx.v, 4)), + _gyro_filter_z.apply(-int16_val(rx.v, 6))); + // update the shared buffer + uint8_t idx = _shared_data_idx ^ 1; + _shared_data[idx]._accel_filtered = _accel_filtered; + _shared_data[idx]._gyro_filtered = _gyro_filtered; + _shared_data_idx = idx; + + _have_sample_available = true; } +/* + read an 8 bit register + */ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) { uint8_t addr = reg | 0x80; // Set most significant bit @@ -444,10 +367,12 @@ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) tx[0] = addr; tx[1] = 0; _spi->transaction(tx, rx, 2); - return rx[1]; } +/* + write an 8 bit register + */ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) { uint8_t tx[2]; @@ -459,42 +384,32 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) } /* - set the DLPF filter frequency. Assumes caller has taken semaphore + set the accel/gyro filter frequency */ -void AP_InertialSensor_MPU9250::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz) { - uint8_t filter = default_filter; - // choose filtering frequency - switch (filter_hz) { - case 5: - filter = BITS_DLPF_CFG_5HZ; - break; - case 10: - filter = BITS_DLPF_CFG_10HZ; - break; - case 20: - filter = BITS_DLPF_CFG_20HZ; - break; - case 42: - filter = BITS_DLPF_CFG_42HZ; - break; - case 98: - filter = BITS_DLPF_CFG_98HZ; - break; + if (filter_hz == 0) { + filter_hz = _default_filter_hz; } - if (filter != 0) { - _last_filter_hz = filter_hz; + _accel_filter_x.set_cutoff_frequency(1000, filter_hz); + _accel_filter_y.set_cutoff_frequency(1000, filter_hz); + _accel_filter_z.set_cutoff_frequency(1000, filter_hz); - _register_write(MPUREG_CONFIG, filter); - } + _gyro_filter_x.set_cutoff_frequency(1000, filter_hz); + _gyro_filter_y.set_cutoff_frequency(1000, filter_hz); + _gyro_filter_z.set_cutoff_frequency(1000, filter_hz); } -bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) +/* + initialise the sensor configuration registers + */ +bool AP_InertialSensor_MPU9250::_hardware_init(void) { if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); + hal.console->printf("MPU9250: Unable to get semaphore"); + return false; } // initially run the bus at low speed @@ -503,7 +418,11 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) // Chip reset uint8_t tries; for (tries = 0; tries<5; tries++) { +#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 + /* Prevent reseting if internal AK8963 is selected, because it may corrupt + * AK8963's initialisation. */ _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); +#endif hal.scheduler->delay(100); // Wake up device and select GyroZ clock. Note that the @@ -527,68 +446,30 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) } _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode - hal.scheduler->delay(1); // Disable I2C bus (recommended on datasheet) +#if HAL_COMPASS_DEFAULT != HAL_COMPASS_AK8963 + /* Prevent disabling if internal AK8963 is selected. If internal AK8963 is not used + * it's ok to disable I2C slaves*/ _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); - hal.scheduler->delay(1); +#endif - uint8_t default_filter; + _default_filter_hz = _default_filter(); - // sample rate and filtering - // to minimise the effects of aliasing we choose a filter - // that is less than half of the sample rate - switch (sample_rate) { - case RATE_50HZ: - // this is used for plane and rover, where noise resistance is - // more important than update rate. Tests on an aerobatic plane - // show that 10Hz is fine, and makes it very noise resistant - default_filter = BITS_DLPF_CFG_10HZ; - _sample_shift = 2; - break; - case RATE_100HZ: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 1; - break; - case RATE_200HZ: - default: - default_filter = BITS_DLPF_CFG_20HZ; - _sample_shift = 0; - break; - } - - _set_filter_register(_mpu6000_filter, default_filter); - - // set sample rate to 200Hz, and use _sample_divider to give - // the requested rate to the application - _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); - hal.scheduler->delay(1); + // used a fixed filter of 42Hz on the sensor, then filter using + // the 2-pole software filter + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); + // set sample rate to 1kHz, and use the 2 pole filter to give the + // desired rate + _register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ); _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000ยบ/s - hal.scheduler->delay(1); - - // // read the product ID rev c has 1/2 the sensitivity of rev d - // _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); - // //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - - // if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || - // (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { - // // Accel scale 8g (4096 LSB/g) - // // Rev C has different scaling than rev D - // _register_write(MPUREG_ACCEL_CONFIG,1<<3); - // } else { - // // Accel scale 8g (4096 LSB/g) - // _register_write(MPUREG_ACCEL_CONFIG,2<<3); - // } // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g _register_write(MPUREG_ACCEL_CONFIG,2<<3); - hal.scheduler->delay(1); - // configure interrupt to fire when new data arrives _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); - hal.scheduler->delay(1); // clear interrupt on any read, and hold the data ready pin high // until we clear the interrupt @@ -603,22 +484,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) return true; } -// return the MPUXk gyro drift rate in radian/s/s -// note that this is much better than the oilpan gyros -float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute - return ToRad(0.5/60); -} - -// return true if a sample is available -bool AP_InertialSensor_MPU9250::_sample_available() -{ - _poll_data(); - return (_sum_count >> _sample_shift) > 0; -} - - #if MPU9250_DEBUG // dump all config registers - used for debug void AP_InertialSensor_MPU9250::_dump_registers(void) @@ -636,12 +501,4 @@ void AP_InertialSensor_MPU9250::_dump_registers(void) #endif -// get_delta_time returns the time period in seconds overwhich the sensor data was collected -float AP_InertialSensor_MPU9250::get_delta_time() const -{ - // the sensor runs at 200Hz - return 0.005 * _num_samples; - -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index aeb1dd998c..1ce5b1a67f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -7,75 +7,75 @@ #include #include #include +#include +#include #include "AP_InertialSensor.h" // enable debug to see a register dump on startup #define MPU9250_DEBUG 0 -class AP_InertialSensor_MPU9250 : public AP_InertialSensor +class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend { public: - AP_InertialSensor_MPU9250(); + AP_InertialSensor_MPU9250(AP_InertialSensor &imu); - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_gyro_drift_rate(); + /* update accel and gyro state */ + bool update(); - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); + bool gyro_sample_available(void) { return _have_sample_available; } + bool accel_sample_available(void) { return _have_sample_available; } - // get_delta_time returns the time period in seconds overwhich the sensor data was collected - float get_delta_time() const; - - uint16_t error_count(void) const { return _error_count; } - bool healthy(void) const { return _error_count <= 4; } - bool get_gyro_health(uint8_t instance) const { return healthy(); } - bool get_accel_health(uint8_t instance) const { return healthy(); } - -protected: - uint16_t _init_sensor( Sample_rate sample_rate ); + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - AP_HAL::DigitalSource *_drdy_pin; + bool _init_sensor(void); - bool _sample_available(); void _read_data_transaction(); bool _data_ready(); void _poll_data(void); uint8_t _register_read( uint8_t reg ); void _register_write( uint8_t reg, uint8_t val ); - bool _hardware_init(Sample_rate sample_rate); + bool _hardware_init(void); + bool _sample_available(); AP_HAL::SPIDeviceDriver *_spi; AP_HAL::Semaphore *_spi_sem; - uint16_t _num_samples; - static const float _gyro_scale; - - uint32_t _last_sample_time_micros; - - // ensure we can't initialise twice - bool _initialised; - int16_t _mpu9250_product_id; - - // how many hardware samples before we report a sample to the caller - uint8_t _sample_shift; - // support for updating filter at runtime - uint8_t _last_filter_hz; + int16_t _last_filter_hz; - void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + // change the filter frequency + void _set_filter(uint8_t filter_hz); - uint16_t _error_count; + // This structure is used to pass data from the timer which reads + // the sensor to the main thread. The _shared_data_idx is used to + // prevent race conditions by ensuring the data is fully updated + // before being used by the consumer + struct { + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + } _shared_data[2]; + volatile uint8_t _shared_data_idx; - // accumulation in timer - must be read with timer disabled - // the sum of the values since last read - Vector3l _accel_sum; - Vector3l _gyro_sum; - volatile int16_t _sum_count; + // Low Pass filters for gyro and accel + LowPassFilter2p _accel_filter_x; + LowPassFilter2p _accel_filter_y; + LowPassFilter2p _accel_filter_z; + LowPassFilter2p _gyro_filter_x; + LowPassFilter2p _gyro_filter_y; + LowPassFilter2p _gyro_filter_z; -public: + // do we currently have a sample pending? + bool _have_sample_available; + + // default filter frequency when set to zero + uint8_t _default_filter_hz; + + // gyro and accel instances + uint8_t _gyro_instance; + uint8_t _accel_instance; #if MPU9250_DEBUG void _dump_registers(void); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 7070ff8e87..df56b44746 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -1,11 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include + #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #include "AP_InertialSensor_Oilpan.h" +#include const extern AP_HAL::HAL& hal; +// this driver assumes an AP_ADC object has been declared globally +extern AP_ADC_ADS7844 apm1_adc; + // ADC channel mappings on for the APM Oilpan // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; @@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = { 1, -1, -1, 1, -1, -1 }; -// ADC channel reading the gyro temperature -const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3; - // Maximum possible value returned by an offset-corrected sensor channel const float AP_InertialSensor_Oilpan::_adc_constraint = 900; @@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900; #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) #define OILPAN_RAW_GYRO_OFFSET 1658.0f -#define ToRad(x) radians(x) // *pi/180 // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 -const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f); -const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f); -const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f); +const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f); /* ------ Public functions -------------------------------------------*/ -AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : - AP_InertialSensor(), - _adc(adc) +AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu) { } -uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate) -{ - _adc->Init(); - - switch (sample_rate) { - case RATE_50HZ: - _sample_threshold = 20; - break; - case RATE_100HZ: - _sample_threshold = 10; - break; - case RATE_200HZ: - _sample_threshold = 5; - break; - } - -#if defined(__AVR_ATmega1280__) - return AP_PRODUCT_ID_APM1_1280; -#else - return AP_PRODUCT_ID_APM1_2560; -#endif -} - -bool AP_InertialSensor_Oilpan::update() -{ - if (!wait_for_sample(100)) { - return false; - } - float adc_values[6]; - Vector3f gyro_offset = _gyro_offset[0].get(); - Vector3f accel_scale = _accel_scale[0].get(); - Vector3f accel_offset = _accel_offset[0].get(); - - _delta_time_micros = _adc->Ch6(_sensors, adc_values); - _temp = _adc->Ch(_gyro_temp_ch); - - _gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET )); - _gyro[0].rotate(_board_orientation); - _gyro[0].x *= _gyro_gain_x; - _gyro[0].y *= _gyro_gain_y; - _gyro[0].z *= _gyro_gain_z; - _gyro[0] -= gyro_offset; - - _previous_accel[0] = _accel[0]; - - _accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); - _accel[0].rotate(_board_orientation); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] *= OILPAN_ACCEL_SCALE_1G; - _accel[0] -= accel_offset; - /* - * X = 1619.30 to 2445.69 - * Y = 1609.45 to 2435.42 - * Z = 1627.44 to 2434.82 + detect the sensor */ +AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + + return sensor; +} + +bool AP_InertialSensor_Oilpan::_init_sensor(void) +{ + apm1_adc.Init(); + + switch (_imu.get_sample_rate()) { + case AP_InertialSensor::RATE_50HZ: + _sample_threshold = 20; + break; + case AP_InertialSensor::RATE_100HZ: + _sample_threshold = 10; + break; + case AP_InertialSensor::RATE_200HZ: + _sample_threshold = 5; + break; + default: + // can't do this speed + return false; + } + + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + + _product_id = AP_PRODUCT_ID_APM1_2560; return true; } -float AP_InertialSensor_Oilpan::get_delta_time() const { - return _delta_time_micros * 1.0e-6; -} - -/* ------ Private functions -------------------------------------------*/ - -// return the oilpan gyro drift rate in radian/s/s -float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) +/* + copy data from ADC to frontend + */ +bool AP_InertialSensor_Oilpan::update() { - // 3.0 degrees/second/minute - return ToRad(3.0/60); + float adc_values[6]; + + apm1_adc.Ch6(_sensors, adc_values); + + // copy gyros to frontend + Vector3f v; + v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x, + _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y, + _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z); + _rotate_and_offset_gyro(_gyro_instance, v); + + // copy accels to frontend + v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); + v *= OILPAN_ACCEL_SCALE_1G; + _rotate_and_offset_accel(_accel_instance, v); + + return true; } // return true if a new sample is available -bool AP_InertialSensor_Oilpan::_sample_available() +bool AP_InertialSensor_Oilpan::_sample_available() const { - return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; -} - -bool AP_InertialSensor_Oilpan::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; + return apm1_adc.num_samples_available(_sensors) >= _sample_threshold; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index bffb4f2114..e00169424c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -3,50 +3,35 @@ #ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ #define __AP_INERTIAL_SENSOR_OILPAN_H__ -#include - -#include -#include +#include #include "AP_InertialSensor.h" -class AP_InertialSensor_Oilpan : public AP_InertialSensor +class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend { public: + AP_InertialSensor_Oilpan(AP_InertialSensor &imu); - AP_InertialSensor_Oilpan( AP_ADC * adc ); + /* 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 gyro_sample_available(void) { return _sample_available(); } + bool accel_sample_available(void) { return _sample_available(); } - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); - -protected: - uint16_t _init_sensor(Sample_rate sample_rate); + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); private: - - bool _sample_available(); - - AP_ADC * _adc; - - float _temp; - - uint32_t _delta_time_micros; - + bool _init_sensor(void); + bool _sample_available() const; static const uint8_t _sensors[6]; static const int8_t _sensor_signs[6]; - static const uint8_t _gyro_temp_ch; - static const float _gyro_gain_x; static const float _gyro_gain_y; static const float _gyro_gain_z; - static const float _adc_constraint; - uint8_t _sample_threshold; + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif // __AP_INERTIAL_SENSOR_OILPAN_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 4f473f5637..c36e126414 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -1,7 +1,8 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN + #include "AP_InertialSensor_PX4.h" const extern AP_HAL::HAL& hal; @@ -15,11 +16,33 @@ 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) { - // assumes max 2 instances +} + +/* + detect the sensor + */ +AP_InertialSensor_Backend *AP_InertialSensor_PX4::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_PX4 *sensor = new AP_InertialSensor_PX4(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_InertialSensor_PX4::_init_sensor(void) +{ + // 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 +55,30 @@ 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: - _default_filter_hz = 15; - _sample_time_usec = 20000; - break; - case RATE_100HZ: - _default_filter_hz = 30; - _sample_time_usec = 10000; - break; - case RATE_200HZ: - _default_filter_hz = 30; - _sample_time_usec = 5000; - break; - case RATE_400HZ: - default: - _default_filter_hz = 30; - _sample_time_usec = 2500; - break; - } - - _set_filter_frequency(_mpu6000_filter); + _default_filter_hz = _default_filter(); + _set_filter_frequency(_imu.get_filter()); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - return AP_PRODUCT_ID_PX4_V2; + _product_id = AP_PRODUCT_ID_PX4_V2; #else - return AP_PRODUCT_ID_PX4; + _product_id = AP_PRODUCT_ID_PX4; #endif + + return true; } /* @@ -89,109 +97,39 @@ 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(); - 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]; + // calling _rotate_and_offset_accel sets the sensor healthy, + // so we only want to do this if we have new data from it + if (_last_accel_timestamp[k] != _last_accel_update_timestamp[k]) { + _rotate_and_offset_accel(_accel_instance[k], accel); + _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]; + // calling _rotate_and_offset_accel sets the sensor healthy, + // so we only want to do this if we have new data from it + if (_last_gyro_timestamp[k] != _last_gyro_update_timestamp[k]) { + _rotate_and_offset_gyro(_gyro_instance[k], gyro); + _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++) { @@ -201,6 +139,7 @@ void AP_InertialSensor_PX4::_get_sample(void) accel_report.timestamp != _last_accel_timestamp[i]) { _accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z); _last_accel_timestamp[i] = accel_report.timestamp; + _set_accel_error_count(_accel_instance[i], accel_report.error_count); } } for (uint8_t i=0; i<_num_gyro_instances; i++) { @@ -210,64 +149,32 @@ void AP_InertialSensor_PX4::_get_sample(void) 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; + _set_gyro_error_count(_gyro_instance[i], gyro_report.error_count); } } - _last_get_sample_timestamp = hrt_absolute_time(); + _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 = hrt_absolute_time(); - 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; - } - uint32_t start = hal.scheduler->millis(); - while ((hal.scheduler->millis() - start) < timeout_ms) { - uint64_t tnow = hrt_absolute_time(); - // 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..262608baaa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -4,7 +4,7 @@ #define __AP_INERTIAL_SENSOR_PX4_H__ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #include #include "AP_InertialSensor.h" @@ -13,47 +13,33 @@ #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); - 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(void); 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 +50,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 // CONFIG_HAL_BOARD #endif // __AP_INERTIAL_SENSOR_PX4_H__ diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde index c28dc8e7d2..81b0e7bcc0 100644 --- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde +++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.pde @@ -36,28 +36,12 @@ #include #include #include +#include +#include const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -#define CONFIG_INS_TYPE HAL_INS_DEFAULT - -#if CONFIG_INS_TYPE == HAL_INS_MPU6000 -AP_InertialSensor_MPU6000 ins; -#elif CONFIG_INS_TYPE == HAL_INS_PX4 -AP_InertialSensor_PX4 ins; -#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN -AP_InertialSensor_VRBRAIN ins; -#elif CONFIG_INS_TYPE == HAL_INS_HIL -AP_InertialSensor_HIL ins; -#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE -AP_InertialSensor_Flymaple ins; -#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D -AP_InertialSensor_L3G4200D ins; -#elif CONFIG_INS_TYPE == HAL_INS_MPU9250 -AP_InertialSensor_MPU9250 ins; -#else - #error Unrecognised CONFIG_INS_TYPE setting. -#endif // CONFIG_INS_TYPE +AP_InertialSensor ins; void setup(void) { @@ -208,7 +192,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - ins.wait_for_sample(1000); + ins.wait_for_sample(); // read samples from ins ins.update();