src/main/flight/imu.h

   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 #pragma once
  19
  20 #include "common/axis.h"
  21 #include "common/maths.h"
  22 #include "common/time.h"
  23
  24 #include "sensors/acceleration.h"
  25
  26 // Exported symbols
  27 extern uint32_t accTimeSum;
  28 extern int accSumCount;
  29 extern float accVelScale;
  30 extern int32_t accSum[XYZ_AXIS_COUNT];
  31
  32
  33 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
  34 #define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
  35 #define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
  36 #define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
  37
  38 typedef union {
  39     int16_t raw[XYZ_AXIS_COUNT];
  40     struct {
  41         // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
  42         int16_t roll;
  43         int16_t pitch;
  44         int16_t yaw;
  45     } values;
  46 } attitudeEulerAngles_t;
  47
  48 extern attitudeEulerAngles_t attitude;
  49
  50 typedef struct accDeadband_s {
  51     uint8_t xy;                 // set the acc deadband for xy-Axis
  52     uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
  53 } accDeadband_t;
  54
  55 typedef struct throttleCorrectionConfig_s {
  56     uint16_t throttle_correction_angle;     // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
  57     uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
  58 } throttleCorrectionConfig_t;
  59
  60 typedef struct imuConfig_s {
  61     uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
  62     uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
  63     uint8_t small_angle;
  64     uint8_t acc_unarmedcal;                 // turn automatic acc compensation on/off
  65     accDeadband_t accDeadband;
  66 } imuConfig_t;
  67
  68 typedef struct imuRuntimeConfig_s {
  69     float dcm_ki;
  70     float dcm_kp;
  71     uint8_t acc_unarmedcal;
  72     uint8_t small_angle;
  73     accDeadband_t accDeadband;
  74 } imuRuntimeConfig_t;
  75
  76 typedef enum {
  77     ACCPROC_READ = 0,
  78     ACCPROC_CHUNK_1,
  79     ACCPROC_CHUNK_2,
  80     ACCPROC_CHUNK_3,
  81     ACCPROC_CHUNK_4,
  82     ACCPROC_CHUNK_5,
  83     ACCPROC_CHUNK_6,
  84     ACCPROC_CHUNK_7,
  85     ACCPROC_COPY
  86 } accProcessorState_e;
  87
  88 typedef struct accProcessor_s {
  89     accProcessorState_e state;
  90 } accProcessor_t;
  91
  92 struct pidProfile_s;
  93 void imuConfigure(
  94     imuConfig_t *imuConfig,
  95     struct pidProfile_s *initialPidProfile,
  96     uint16_t throttle_correction_angle
  97 );
  98
  99 float getCosTiltAngle(void);
 100 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 101 void imuUpdateAttitude(timeUs_t currentTimeUs);
 102 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 103 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 104 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz);
 105
 106 union u_fp_vector;
 107 int16_t imuCalculateHeading(union u_fp_vector *vec);
 108
 109 void imuResetAccelerationSum(void);
 110 void imuInit(void);