From 14636e1cdffbf626e67e32c793e6f0e9eb5fb4e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 27 Sep 2013 10:33:08 +1000 Subject: [PATCH] AP_InertialSensor: changed num_samples_available() to sample_available() this makes the interface clearer. It also fixes a 3D accel cal bug. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 22 +++++++++++-------- .../AP_InertialSensor/AP_InertialSensor.h | 4 ++-- .../AP_InertialSensor_Flymaple.cpp | 6 ++--- .../AP_InertialSensor_Flymaple.h | 2 +- .../AP_InertialSensor_MPU6000.cpp | 10 ++++----- .../AP_InertialSensor_MPU6000.h | 4 ++-- .../AP_InertialSensor_Oilpan.cpp | 6 ++--- .../AP_InertialSensor_Oilpan.h | 4 ++-- .../AP_InertialSensor_PX4.cpp | 4 ++-- .../AP_InertialSensor/AP_InertialSensor_PX4.h | 2 +- .../AP_InertialSensor_Stub.cpp | 5 +++-- .../AP_InertialSensor_Stub.h | 2 +- .../examples/MPU6000/MPU6000.pde | 2 +- .../examples/OilPan/OilPan.pde | 2 +- .../AP_InertialSensor/examples/PX4/PX4.pde | 2 +- 15 files changed, 41 insertions(+), 36 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 54761dbc04..c5e8848fc6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -360,16 +360,20 @@ bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact // clear out any existing samples from ins update(); - // wait until we have 32 samples - while( num_samples_available() < 32 * SAMPLE_UNIT ) { - hal.scheduler->delay(10); + // average 32 samples + samples[i] = Vector3f(); + uint8_t num_samples = 0; + while (num_samples < 32) { + if (sample_available()) { + // read samples from ins + update(); + // capture sample + samples[i] += get_accel(); + hal.scheduler->delay(10); + num_samples++; + } } - - // read samples from ins - update(); - - // capture sample - samples[i] = get_accel(); + samples[i] /= num_samples; } // run the calibration routine diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 31c634afa6..f6f3995506 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -118,8 +118,8 @@ public: // depends on what gyro chips are being used virtual float get_gyro_drift_rate(void) = 0; - // get number of samples read from the sensors - virtual uint16_t num_samples_available() = 0; + // true if a new sample is available from the sensors + virtual bool sample_available() = 0; // class level parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index bb4e92f187..299f4613e5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -120,7 +120,7 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz) bool AP_InertialSensor_Flymaple::update(void) { - while (num_samples_available() == 0) { + while (sample_available() == false) { hal.scheduler->delay(1); } Vector3f accel_scale = _accel_scale.get(); @@ -221,10 +221,10 @@ void AP_InertialSensor_Flymaple::_ins_timer(uint32_t now) _accumulate(); } -uint16_t AP_InertialSensor_Flymaple::num_samples_available(void) +bool AP_InertialSensor_Flymaple::sample_available(void) { _accumulate(); - return min(_accel_sum_count, _gyro_sum_count) / _sample_divider; + return (min(_accel_sum_count, _gyro_sum_count) / _sample_divider) > 0; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h index c09c6f5dca..98553da1d8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h @@ -20,7 +20,7 @@ public: float get_delta_time(); uint32_t get_last_sample_time_micros(); float get_gyro_drift_rate(); - uint16_t num_samples_available(); + bool sample_available(); private: uint16_t _init_sensor( Sample_rate sample_rate ); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 33b87b00b3..2bf2e47166 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -279,7 +279,7 @@ static volatile uint16_t _count; void AP_InertialSensor_MPU6000::wait_for_sample() { uint32_t tstart = hal.scheduler->micros(); - while (num_samples_available() == 0) { + while (sample_available() == false) { uint32_t now = hal.scheduler->micros(); uint32_t dt = now - tstart; if (dt > 50000) { @@ -417,7 +417,7 @@ void AP_InertialSensor_MPU6000::_read_data_from_timerprocess() if (!_spi_sem->take_nonblocking()) { /* the semaphore being busy is an expected condition when the - mainline code is calling num_samples_available() which will + mainline code is calling sample_available() which will grab the semaphore. We return now and rely on the mainline code grabbing the latest sample. */ @@ -629,11 +629,11 @@ float AP_InertialSensor_MPU6000::get_gyro_drift_rate(void) return ToRad(0.5/60); } -// get number of samples read from the sensors -uint16_t AP_InertialSensor_MPU6000::num_samples_available() +// return true if a sample is available +bool AP_InertialSensor_MPU6000::sample_available() { _poll_data(0); - return _count >> _sample_shift; + return (_count >> _sample_shift) > 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 060d43cb03..8c0d5bcad0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -39,8 +39,8 @@ public: void push_accel_offsets_to_dmp(); void set_dmp_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ); - // num_samples_available - get number of samples read from the sensors - uint16_t num_samples_available(); + // sample_available - true when a new sample is available + bool sample_available(); // get_delta_time returns the time period in seconds overwhich the sensor data was collected float get_delta_time(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 12cf9f5209..0b9fbd385e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -121,10 +121,10 @@ float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) return ToRad(3.0/60); } -// get number of samples read from the sensors -uint16_t AP_InertialSensor_Oilpan::num_samples_available() +// return true if a new sample is available +bool AP_InertialSensor_Oilpan::sample_available() { - return _adc->num_samples_available(_sensors) / _sample_threshold; + return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index f6678c94b4..e6338ec1eb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -21,8 +21,8 @@ public: //uint32_t get_last_sample_time_micros(); // last_sample_time - get time (in microseconds) that last sample was captured float get_gyro_drift_rate(); - // get number of samples read from the sensors - uint16_t num_samples_available(); + // sample_available() - true when a new sample is available + bool sample_available(); protected: uint16_t _init_sensor(Sample_rate sample_rate); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp index 2009f92485..37ea6401de 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -142,14 +142,14 @@ void AP_InertialSensor_PX4::_get_sample(void) } } -uint16_t AP_InertialSensor_PX4::num_samples_available(void) +bool AP_InertialSensor_PX4::sample_available(void) { uint64_t tnow = hrt_absolute_time(); if (tnow - _last_sample_timestamp > _sample_time_usec) { _num_samples_available++; _last_sample_timestamp = tnow; } - return _num_samples_available; + return _num_samples_available > 0; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h index c0904ae292..f06fca44ef 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -23,7 +23,7 @@ public: bool update(); float get_delta_time(); float get_gyro_drift_rate(); - uint16_t num_samples_available(); + bool sample_available(); private: uint16_t _init_sensor( Sample_rate sample_rate ); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp index 4909f5ea0c..a4063e6f2c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp @@ -44,10 +44,11 @@ float AP_InertialSensor_Stub::get_gyro_drift_rate(void) { // 0.5 degrees/second/minute return ToRad(0.5/60); } -uint16_t AP_InertialSensor_Stub::num_samples_available() + +bool AP_InertialSensor_Stub::sample_available() { uint16_t ret = (hal.scheduler->millis() - _last_update_ms) / _sample_period_ms; - return ret; + return ret > 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h index 7b3d8cae4a..9faf1964ff 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h @@ -17,7 +17,7 @@ public: float get_delta_time(); uint32_t get_last_sample_time_micros(); float get_gyro_drift_rate(); - uint16_t num_samples_available(); + bool sample_available(); protected: uint16_t _init_sensor( Sample_rate sample_rate ); diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index 9445ea5ee5..263a6e9e64 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.num_samples_available() == 0) /* noop */ ; + while (ins.sample_available() == false) /* noop */ ; // 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 0c445c2a78..26e756eed1 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.num_samples_available() == 0) /* noop */ ; + while (ins.sample_available() == false) /* noop */ ; // 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 d77bdff233..a39879baf5 100644 --- a/libraries/AP_InertialSensor/examples/PX4/PX4.pde +++ b/libraries/AP_InertialSensor/examples/PX4/PX4.pde @@ -164,7 +164,7 @@ void run_test() while( !hal.console->available() ) { // wait until we have a sample - while (ins.num_samples_available() == 0) { + while (ins.sample_available() == false) { hal.scheduler->delay(1); }