src/main/drivers/accgyro/accgyro.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #if !defined(SITL_BUILD)
  26 #include "build/atomic.h"
  27 #endif
  28
  29 #include "build/build_config.h"
  30
  31 #include "common/log.h"
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34
  35 #include "drivers/bus.h"
  36 #include "drivers/io.h"
  37 #include "drivers/nvic.h"
  38 #include "drivers/sensor.h"
  39 #include "drivers/system.h"
  40 #include "drivers/time.h"
  41
  42 #include "drivers/sensor.h"
  43 #include "drivers/accgyro/accgyro.h"
  44
  45 const gyroFilterAndRateConfig_t * chooseGyroConfig(uint8_t desiredLpf, uint16_t desiredRateHz, const gyroFilterAndRateConfig_t * configs, int count)
  46 {
  47     int i;
  48     int8_t selectedLpf = configs[0].gyroLpf;
  49     const gyroFilterAndRateConfig_t * candidate = &configs[0];
  50
  51     // Choose closest supported LPF value
  52     for (i = 1; i < count; i++) {
  53         if (ABS(desiredLpf - configs[i].gyroLpf) < ABS(desiredLpf - selectedLpf)) {
  54             selectedLpf = configs[i].gyroLpf;
  55             candidate = &configs[i];
  56         }
  57     }
  58
  59     // Now find the closest update rate
  60     for (i = 0; i < count; i++) {
  61         if ((configs[i].gyroLpf == selectedLpf) && (ABS(desiredRateHz - candidate->gyroRateHz) > ABS(desiredRateHz - configs[i].gyroRateHz))) {
  62             candidate = &configs[i];
  63         }
  64     }
  65
  66     LOG_VERBOSE(GYRO, "GYRO CONFIG { %d, %d } -> { %d, %d}; regs 0x%02X, 0x%02X",
  67                 desiredLpf, desiredRateHz,
  68                 candidate->gyroLpf, candidate->gyroRateHz,
  69                 candidate->gyroConfigValues[0], candidate->gyroConfigValues[1]);
  70
  71     return candidate;
  72 }
  73
  74 bool gyroCheckDataReady(gyroDev_t* gyro)
  75 {
  76     bool ret;
  77     if (gyro->dataReady) {
  78         ret = true;
  79         gyro->dataReady = false;
  80     } else {
  81         ret = false;
  82     }
  83     return ret;
  84 }