From 2753449e75fd53270f6a14ba14e78feb5584416b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:43:53 +1100 Subject: [PATCH] AP_InertialSensor: added support for multiple accel/gyro devices this makes it possible to ask for the gyro and accel vectors from secondary INS devices. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 37 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 14 +++++-- .../AP_InertialSensor_Flymaple.cpp | 6 +-- .../AP_InertialSensor_Flymaple.h | 2 +- .../AP_InertialSensor_HIL.cpp | 8 ++-- .../AP_InertialSensor/AP_InertialSensor_HIL.h | 6 +-- .../AP_InertialSensor_L3G4200D.cpp | 6 +-- .../AP_InertialSensor_L3G4200D.h | 2 +- .../AP_InertialSensor_MPU6000.cpp | 8 ++-- .../AP_InertialSensor_MPU6000.h | 6 +-- .../AP_InertialSensor_Oilpan.cpp | 6 +-- .../AP_InertialSensor_Oilpan.h | 5 +-- .../examples/Flymaple/Flymaple.pde | 2 +- .../examples/L3G4200D/L3G4200D.pde | 2 +- .../examples/MPU6000/MPU6000.pde | 2 +- .../examples/OilPan/OilPan.pde | 2 +- .../AP_InertialSensor/examples/PX4/PX4.pde | 4 +- 17 files changed, 78 insertions(+), 40 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 69f5a6e13d..32b8b29ba1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -588,5 +588,42 @@ void AP_InertialSensor::_calculate_trim(Vector3f accel_sample, float& trim_roll, } } +/** + default versions of multi-device accessor functions + */ +bool AP_InertialSensor::get_gyro_instance_health(uint8_t instance) const +{ + if (instance != 0) { + return false; + } + return healthy(); +} + +bool AP_InertialSensor::get_gyro_instance(uint8_t instance, Vector3f &gyro) const +{ + if (instance != 0) { + return false; + } + gyro = get_gyro(); + return true; +} + +bool AP_InertialSensor::get_accel_instance_health(uint8_t instance) const +{ + if (instance != 0) { + return false; + } + return healthy(); +} + +bool AP_InertialSensor::get_accel_instance(uint8_t instance, Vector3f &accel) const +{ + if (instance != 0) { + return false; + } + accel = get_accel(); + return true; +} + #endif // __AVR_ATmega1280__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 95946ebced..6dfcf375ed 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -97,6 +97,15 @@ public: const Vector3f &get_accel(void) const { return _accel; } virtual void set_accel(const Vector3f &accel) {} + // multi-device interface + virtual bool get_gyro_instance_health(uint8_t instance) const; + virtual uint8_t get_gyro_count(void) const { return 1; }; + virtual bool get_gyro_instance(uint8_t instance, Vector3f &gyro) const; + + virtual bool get_accel_instance_health(uint8_t instance) const; + virtual uint8_t get_accel_count(void) const { return 1; }; + virtual bool get_accel_instance(uint8_t instance, Vector3f &accel) const; + // get accel offsets in m/s/s Vector3f get_accel_offsets() { return _accel_offset; } void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); } @@ -118,9 +127,6 @@ public: // depends on what gyro chips are being used virtual float get_gyro_drift_rate(void) = 0; - // true if a new sample is available from the sensors - virtual bool sample_available() = 0; - // wait for a sample to be available, with timeout in milliseconds virtual bool wait_for_sample(uint16_t timeout_ms) = 0; @@ -140,7 +146,7 @@ public: } virtual uint16_t error_count(void) const { return 0; } - virtual bool healthy(void) { return true; } + virtual bool healthy(void) const { return true; } protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index dfe7e7d61c..0f0d5ec7b2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -295,7 +295,7 @@ void AP_InertialSensor_Flymaple::_accumulate(void) i2c_sem->give(); } -bool AP_InertialSensor_Flymaple::sample_available(void) +bool AP_InertialSensor_Flymaple::_sample_available(void) { _accumulate(); return min(_accel_samples, _gyro_samples) / _sample_divider > 0; @@ -303,13 +303,13 @@ bool AP_InertialSensor_Flymaple::sample_available(void) bool AP_InertialSensor_Flymaple::wait_for_sample(uint16_t timeout_ms) { - if (sample_available()) { + 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()) { + if (_sample_available()) { return true; } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index e24ccdbc5e..8c247a3f30 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -21,12 +21,12 @@ public: bool update(); float get_delta_time(); float get_gyro_drift_rate(); - bool sample_available(); bool wait_for_sample(uint16_t timeout_ms); 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index faa1381673..6b311920df 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp @@ -41,7 +41,7 @@ float AP_InertialSensor_HIL::get_gyro_drift_rate(void) { return ToRad(0.5/60); } -bool AP_InertialSensor_HIL::sample_available() +bool AP_InertialSensor_HIL::_sample_available() { uint16_t ret = (hal.scheduler->millis() - _last_update_ms) / _sample_period_ms; @@ -51,13 +51,13 @@ bool AP_InertialSensor_HIL::sample_available() bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms) { - if (sample_available()) { + if (_sample_available()) { return true; } uint32_t start = hal.scheduler->millis(); while ((hal.scheduler->millis() - start) < timeout_ms) { hal.scheduler->delay(1); - if (sample_available()) { + if (_sample_available()) { return true; } } @@ -80,7 +80,7 @@ void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro) /** try to detect bad accel/gyro sensors */ -bool AP_InertialSensor_HIL::healthy(void) +bool AP_InertialSensor_HIL::healthy(void) const { uint32_t tnow = hal.scheduler->micros(); if ((tnow - _last_accel_usec) > 40000) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index 5870ce0344..f97eda8881 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h @@ -16,13 +16,13 @@ public: bool update(); float get_delta_time(); float get_gyro_drift_rate(); - bool sample_available(); bool wait_for_sample(uint16_t timeout_ms); void set_accel(const Vector3f &accel); void set_gyro(const Vector3f &gyro); - bool healthy(void); + bool healthy(void) const; -protected: +private: + bool _sample_available(); uint16_t _init_sensor( Sample_rate sample_rate ); uint32_t _sample_period_ms; uint32_t _last_update_ms; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index e78903737b..8b5b9014f2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -344,14 +344,14 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) i2c_sem->give(); } -bool AP_InertialSensor_L3G4200D::sample_available(void) +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) { - if (sample_available()) { + if (_sample_available()) { _last_sample_time = hal.scheduler->micros(); return true; } @@ -359,7 +359,7 @@ bool AP_InertialSensor_L3G4200D::wait_for_sample(uint16_t timeout_ms) while ((hal.scheduler->millis() - start) < timeout_ms) { hal.scheduler->delay_microseconds(100); _accumulate(); - if (sample_available()) { + if (_sample_available()) { _last_sample_time = hal.scheduler->micros(); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index 2bea5f9de3..978a415009 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -21,12 +21,12 @@ public: bool update(); float get_delta_time(); float get_gyro_drift_rate(); - bool sample_available(); bool wait_for_sample(uint16_t timeout_ms); private: uint16_t _init_sensor( Sample_rate sample_rate ); void _accumulate(void); + bool _sample_available(); uint64_t _last_update_usec; Vector3f _accel_filtered; Vector3f _gyro_filtered; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 7ca7f455b4..856cd83df1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -235,13 +235,13 @@ uint16_t AP_InertialSensor_MPU6000::_init_sensor( Sample_rate sample_rate ) bool AP_InertialSensor_MPU6000::wait_for_sample(uint16_t timeout_ms) { - if (sample_available()) { + 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()) { + if (_sample_available()) { return true; } } @@ -319,7 +319,7 @@ void AP_InertialSensor_MPU6000::_poll_data(void) if (!_spi_sem->take_nonblocking()) { /* the semaphore being busy is an expected condition when the - mainline code is calling sample_available() which will + 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. */ @@ -563,7 +563,7 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) } // return true if a sample is available -bool AP_InertialSensor_MPU6000::sample_available() +bool AP_InertialSensor_MPU6000::_sample_available() { _poll_data(); return (_sum_count >> _sample_shift) > 0; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index a164be13d0..7bf4534763 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -24,9 +24,6 @@ public: bool update(); float get_gyro_drift_rate(); - // sample_available - true when a new sample is available - bool sample_available(); - // wait for a sample to be available, with timeout in milliseconds bool wait_for_sample(uint16_t timeout_ms); @@ -34,7 +31,7 @@ public: float get_delta_time(); uint16_t error_count(void) const { return _error_count; } - bool healthy(void) { return _error_count <= 4; } + bool healthy(void) const { return _error_count <= 4; } protected: uint16_t _init_sensor( Sample_rate sample_rate ); @@ -42,6 +39,7 @@ protected: private: AP_HAL::DigitalSource *_drdy_pin; + bool _sample_available(); void _read_data_transaction(); bool _data_ready(); void _poll_data(void); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 4f706c39a8..d8ab8121d6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -124,20 +124,20 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) } // return true if a new sample is available -bool AP_InertialSensor_Oilpan::sample_available() +bool AP_InertialSensor_Oilpan::_sample_available() { return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; } bool AP_InertialSensor_Oilpan::wait_for_sample(uint16_t timeout_ms) { - if (sample_available()) { + 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()) { + if (_sample_available()) { return true; } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index 63136a3d69..f8be696381 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -20,9 +20,6 @@ public: float get_delta_time(); // get_delta_time returns the time period in seconds overwhich the sensor data was collected float get_gyro_drift_rate(); - // sample_available() - true when a new sample is available - bool sample_available(); - // wait for a sample to be available, with timeout in milliseconds bool wait_for_sample(uint16_t timeout_ms); @@ -31,6 +28,8 @@ protected: private: + bool _sample_available(); + AP_ADC * _adc; float _temp; diff --git a/libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde b/libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde index a485bd3fb2..21f3ddeb0a 100644 --- a/libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde +++ b/libraries/AP_InertialSensor/examples/Flymaple/Flymaple.pde @@ -165,7 +165,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.sample_available() == false) /* noop */ ; + ins.wait_for_sample(); // read samples from ins ins.update(); diff --git a/libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde b/libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde index 772a25e9a2..f5d792ca17 100644 --- a/libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde +++ b/libraries/AP_InertialSensor/examples/L3G4200D/L3G4200D.pde @@ -166,7 +166,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.sample_available() == false) hal.scheduler->delay(1); + ins.wait_for_sample(); // read samples from ins ins.update(); diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index 263a6e9e64..291dafbf33 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -170,7 +170,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.sample_available() == false) /* noop */ ; + ins.wait_for_sample(); // read samples from ins ins.update(); diff --git a/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde b/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde index 26e756eed1..f624eb6819 100644 --- a/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde +++ b/libraries/AP_InertialSensor/examples/OilPan/OilPan.pde @@ -174,7 +174,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.sample_available() == false) /* noop */ ; + ins.wait_for_sample(); // read samples from ins ins.update(); diff --git a/libraries/AP_InertialSensor/examples/PX4/PX4.pde b/libraries/AP_InertialSensor/examples/PX4/PX4.pde index a39879baf5..91740283dc 100644 --- a/libraries/AP_InertialSensor/examples/PX4/PX4.pde +++ b/libraries/AP_InertialSensor/examples/PX4/PX4.pde @@ -164,9 +164,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.sample_available() == false) { - hal.scheduler->delay(1); - } + ins.wait_for_sample(); // read samples from ins ins.update();