diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 30e60bec6c231..446a46f2855a9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -9,6 +9,8 @@ #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + #include #include @@ -34,6 +36,7 @@ class AP_InertialSensor_Backend; class AuxiliaryBus; class AP_AHRS; +class FastRateBuffer; /* forward declare AP_Logger class. We can't include logger.h @@ -51,6 +54,7 @@ class AP_Logger; class AP_InertialSensor : AP_AccelCal_Client { friend class AP_InertialSensor_Backend; + friend class FastRateBuffer; public: AP_InertialSensor(); @@ -161,12 +165,12 @@ class AP_InertialSensor : AP_AccelCal_Client const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; } FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; } FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_first_usable_gyro, axis); } - uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } - uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED bool has_fft_notch() const; #endif #endif + uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; } + uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); } bool set_gyro_window_size(uint16_t size); // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); } @@ -263,6 +267,7 @@ class AP_InertialSensor : AP_AccelCal_Client AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance); void detect_backends(void); + void update_backends(); // accel peak hold detector void set_accel_peak_hold(uint8_t instance, const Vector3f &accel); @@ -798,6 +803,30 @@ class AP_InertialSensor : AP_AccelCal_Client bool raw_logging_option_set(RAW_LOGGING_OPTION option) const { return (raw_logging_options.get() & int32_t(option)) != 0; } + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + // Support for the fast rate thread in copter + FastRateBuffer* fast_rate_buffer; + +public: + // enable the fast rate buffer and start pushing samples to it + void enable_fast_rate_buffer(); + // disable the fast rate buffer and stop pushing samples to it + void disable_fast_rate_buffer(); + // get the next available gyro sample from the fast rate buffer + bool get_next_gyro_sample(Vector3f& gyro); + // get the number of available gyro samples in the fast rate buffer + uint32_t get_num_gyro_samples(); + // set the rate at which samples are collected, unused samples are dropped + void set_rate_decimation(uint8_t rdec); + // are gyro samples being sourced from the rate loop buffer + bool use_rate_loop_gyro_samples() const; + // push a new gyro sample into the fast rate buffer + bool push_next_gyro_sample(const Vector3f& gyro); + // run the filter parmeter update code. + void update_backend_filters(); + // are rate loop samples enabled for this instance? + bool is_rate_loop_gyro_enabled(uint8_t instance) const; + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED }; namespace AP { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 944b20661a131..30d06b02b16c1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -2,6 +2,7 @@ #include #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" #include @@ -254,9 +255,21 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const notch.filter[instance].reset(); } #endif + gyro_filtered = _imu._gyro_filtered[instance]; + } + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + if (_imu.is_rate_loop_gyro_enabled(instance)) { + if (_imu.push_next_gyro_sample(gyro_filtered)) { + // if we used the value, record it for publication to the front-end + _imu._gyro_filtered[instance] = gyro_filtered; + } } else { _imu._gyro_filtered[instance] = gyro_filtered; } +#else + _imu._gyro_filtered[instance] = gyro_filtered; +#endif } void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, @@ -772,6 +785,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ if (has_been_killed(instance)) { return; } + if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); #if HAL_GYROFFT_ENABLED diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 00d149e149223..051ac1346489c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -53,6 +53,15 @@ class AP_InertialSensor_Backend */ virtual bool update() = 0; /* front end */ + // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* + * Update the filter parameters. Called by the frontend to propagate + * filter parameters to the frontend structure via the + * update_gyro_filters() and update_accel_filters() functions + */ + void update_filters() __RAMFUNC__; /* front end */ + // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + /* * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h new file mode 100644 index 0000000000000..8bf45fecab7f0 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h @@ -0,0 +1,9 @@ +#pragma once + +#include +#include +#include + +#ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter)) +#endif diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp new file mode 100644 index 0000000000000..6617915fecd62 --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -0,0 +1,124 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include +#include "AP_InertialSensor_rate_config.h" +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED +#include "FastRateBuffer.h" +#include + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +// hal.console can be accessed from bus threads on ChibiOS +#define debug(fmt, args ...) do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0) +#else +#define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) +#endif + +void AP_InertialSensor::enable_fast_rate_buffer() +{ + fast_rate_buffer = NEW_NOTHROW FastRateBuffer(); +} + +void AP_InertialSensor::disable_fast_rate_buffer() +{ + delete fast_rate_buffer; + fast_rate_buffer = nullptr; +} + +uint32_t AP_InertialSensor::get_num_gyro_samples() +{ + return fast_rate_buffer->get_num_gyro_samples(); +} + +void AP_InertialSensor::set_rate_decimation(uint8_t rdec) +{ + fast_rate_buffer->set_rate_decimation(rdec); +} + +// are gyro samples being sourced from the rate loop buffer +bool AP_InertialSensor::use_rate_loop_gyro_samples() const +{ + return fast_rate_buffer != nullptr; +} + +// whether or not to push the current gyro sample +bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const +{ + return use_rate_loop_gyro_samples() && fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index(); +} + +bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + return fast_rate_buffer->get_next_gyro_sample(gyro); +} + +bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro) +{ + if (!use_rate_loop_gyro_samples()) { + return false; + } + + if (_rate_loop_gyro_window.available() == 0) { + _notifier.wait_blocking(); + } + + WITH_SEMAPHORE(_mutex); + + return _rate_loop_gyro_window.pop(gyro); +} + +bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro) +{ + if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) { + return false; + } + /* + tell the rate thread we have a new sample + */ + WITH_SEMAPHORE(fast_rate_buffer->_mutex); + + if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) { + debug("dropped rate loop sample"); + } + fast_rate_buffer->rate_decimation_count = 0; + fast_rate_buffer->_notifier.signal(); + return true; +} + +void AP_InertialSensor::update_backend_filters() +{ + for (uint8_t i=0; i<_backend_count; i++) { + _backends[i]->update_filters(); + } +} + +void AP_InertialSensor_Backend::update_filters() +{ + WITH_SEMAPHORE(_sem); + + update_accel_filters(accel_instance); + update_gyro_filters(gyro_instance); +} + +#endif // AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED diff --git a/libraries/AP_InertialSensor/FastRateBuffer.h b/libraries/AP_InertialSensor/FastRateBuffer.h new file mode 100644 index 0000000000000..712500fb8d9bb --- /dev/null +++ b/libraries/AP_InertialSensor/FastRateBuffer.h @@ -0,0 +1,50 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_InertialSensor.h" + +#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED + +#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop + +#include +#include +#include +#include + +class FastRateBuffer +{ + friend class AP_InertialSensor; +public: + bool get_next_gyro_sample(Vector3f& gyro); + uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); } + void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; } + // whether or not to push the current gyro sample + bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; } + bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } + +private: + /* + binary semaphore for rate loop to use to start a rate loop when + we hav finished filtering the primary IMU + */ + ObjectBuffer _rate_loop_gyro_window{AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE}; + uint8_t rate_decimation; // 0 means off + uint8_t rate_decimation_count; + HAL_BinarySemaphore _notifier; + HAL_Semaphore _mutex; +}; +#endif \ No newline at end of file