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             : 
      16             : #include <AP_AHRS/AP_AHRS.h>
      17             : #include "AP_InertialSensor_rate_config.h"
      18             : #include "AP_InertialSensor.h"
      19             : #include "AP_InertialSensor_Backend.h"
      20             : 
      21             : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
      22             : #include "FastRateBuffer.h"
      23             : #include <stdio.h>
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
      28             : // hal.console can be accessed from bus threads on ChibiOS
      29             : #define debug(fmt, args ...)  do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0)
      30             : #else
      31             : #define debug(fmt, args ...)  do {printf("IMU: " fmt "\n", ## args); } while(0)
      32             : #endif
      33             : 
      34             : // enable the fast rate buffer and start pushing samples to it
      35           3 : void AP_InertialSensor::enable_fast_rate_buffer()
      36             : {
      37           3 :     if (fast_rate_buffer_enabled) {
      38           0 :         return;
      39             :     }
      40           3 :     if (fast_rate_buffer == nullptr) {
      41           3 :         fast_rate_buffer = NEW_NOTHROW FastRateBuffer();
      42             :     }
      43           3 :     fast_rate_buffer_enabled = fast_rate_buffer != nullptr;
      44             : }
      45             : 
      46             : // disable the fast rate buffer and stop pushing samples to it
      47           1 : void AP_InertialSensor::disable_fast_rate_buffer()
      48             : {
      49           1 :     fast_rate_buffer_enabled = false;
      50           1 :     if (fast_rate_buffer != nullptr) {
      51           1 :         fast_rate_buffer->reset();
      52             :     }
      53           1 : }
      54             : 
      55             : // get the number of available gyro samples in the fast rate buffer
      56      288220 : uint32_t AP_InertialSensor::get_num_gyro_samples()
      57             : {
      58      288220 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
      59           0 :         return 0;
      60             :     }
      61      288220 :     return fast_rate_buffer->get_num_gyro_samples();
      62             : }
      63             : 
      64             : // set the rate at which samples are collected, unused samples are dropped
      65      291840 : void AP_InertialSensor::set_rate_decimation(uint8_t rdec)
      66             : {
      67      291840 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
      68           0 :         return;
      69             :     }
      70      291840 :     fast_rate_buffer->set_rate_decimation(rdec);
      71             : }
      72             : 
      73             : // whether or not to push the current gyro sample
      74   135288617 : bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
      75             : {
      76   135288617 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
      77   134781209 :         return false;
      78             :     }
      79      507408 :     return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
      80             : }
      81             : 
      82             : // whether or not to use the dynamic fifo
      83           0 : bool AP_InertialSensor::is_dynamic_fifo_enabled(uint8_t instance) const
      84             : {
      85           0 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
      86           0 :         return false;
      87             :     }
      88           0 :     return (_fast_sampling_mask & (1U<<instance)) != 0
      89           0 :             && fast_rate_buffer->use_rate_loop_gyro_samples();
      90             : }
      91             : 
      92             : // get the next available gyro sample from the fast rate buffer
      93      291840 : bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
      94             : {
      95      291840 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
      96           0 :         return false;
      97             :     }
      98             : 
      99      291840 :     return fast_rate_buffer->get_next_gyro_sample(gyro);
     100             : }
     101             : 
     102             : 
     103      291840 : bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro)
     104             : {
     105      291840 :     if (!use_rate_loop_gyro_samples()) {
     106           0 :         return false;
     107             :     }
     108             : 
     109      291840 :     if (_rate_loop_gyro_window.available() == 0) {
     110      287161 :         _notifier.wait_blocking();
     111             :     }
     112             : 
     113      291838 :     WITH_SEMAPHORE(_mutex);
     114             : 
     115      291838 :     return _rate_loop_gyro_window.pop(gyro);
     116      291838 : }
     117             : 
     118           1 : void FastRateBuffer::reset()
     119             : {
     120           1 :     _rate_loop_gyro_window.clear();
     121           1 : }
     122             : 
     123      288225 : bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro)
     124             : {
     125      288225 :     if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
     126           0 :         return false;
     127             :     }
     128             : 
     129      288225 :     if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) {
     130           0 :         return false;
     131             :     }
     132             :     /*
     133             :         tell the rate thread we have a new sample
     134             :     */
     135      288225 :     WITH_SEMAPHORE(fast_rate_buffer->_mutex);
     136             : 
     137      288225 :     if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) {
     138           4 :         debug("dropped rate loop sample");
     139             :     }
     140      288225 :     fast_rate_buffer->rate_decimation_count = 0;
     141      288225 :     fast_rate_buffer->_notifier.signal();
     142      288225 :     return true;
     143      288225 : }
     144             : 
     145      144109 : void AP_InertialSensor::update_backend_filters()
     146             : {
     147      432327 :     for (uint8_t i=0; i<_backend_count; i++) {
     148      288218 :         _backends[i]->update_filters();
     149             :     }
     150      144109 : }
     151             : 
     152      288218 : void AP_InertialSensor_Backend::update_filters()
     153             : {
     154      288218 :     WITH_SEMAPHORE(_sem);
     155             : 
     156      288218 :     update_accel_filters(accel_instance);
     157      288218 :     update_gyro_filters(gyro_instance);
     158      288218 : }
     159             : 
     160             : #endif // AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED