Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             : */
      15             : #pragma once
      16             : 
      17             : #include "AP_InertialSensor.h"
      18             : 
      19             : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
      20             : 
      21             : #define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8     // gyro buffer size for rate loop
      22             : 
      23             : #include <AP_HAL/AP_HAL_Boards.h>
      24             : #include <AP_HAL/utility/RingBuffer.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : #include <AP_HAL/Semaphores.h>
      27             : 
      28             : class FastRateBuffer
      29             : {
      30             :     friend class AP_InertialSensor;
      31             : public:
      32             :     bool get_next_gyro_sample(Vector3f& gyro);
      33      288220 :     uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); }
      34      291840 :     void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; }
      35             :     // whether or not to push the current gyro sample
      36      799229 :     bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; }
      37             :     bool gyro_samples_available() { return  _rate_loop_gyro_window.available() > 0; }
      38             :     void reset();
      39             : 
      40             : private:
      41             :     /*
      42             :       binary semaphore for rate loop to use to start a rate loop when
      43             :       we hav finished filtering the primary IMU
      44             :      */
      45             :     ObjectBuffer<Vector3f> _rate_loop_gyro_window{AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE};
      46             :     uint8_t rate_decimation; // 0 means off
      47             :     uint8_t rate_decimation_count;
      48             :     HAL_BinarySemaphore _notifier;
      49             :     HAL_Semaphore _mutex;
      50             : };
      51             : #endif