src/main/sensors/boardalignment.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <math.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #include "common/utils.h"
  29 #include "common/maths.h"
  30 #include "common/axis.h"
  31 #include "common/sensor_alignment.h"
  32
  33 #include "pg/pg.h"
  34 #include "pg/pg_ids.h"
  35
  36 #include "drivers/sensor.h"
  37
  38 #include "boardalignment.h"
  39
  40 static bool standardBoardAlignment = true;     // board orientation correction
  41 static fp_rotationMatrix_t boardRotation;
  42
  43 // no template required since defaults are zero
  44 PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
  45
  46 static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
  47 {
  48     return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees;
  49 }
  50
  51 void initBoardAlignment(const boardAlignment_t *boardAlignment)
  52 {
  53     if (isBoardAlignmentStandard(boardAlignment)) {
  54         return;
  55     }
  56
  57     standardBoardAlignment = false;
  58
  59     fp_angles_t rotationAngles;
  60     rotationAngles.angles.roll  = degreesToRadians(boardAlignment->rollDegrees );
  61     rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
  62     rotationAngles.angles.yaw   = degreesToRadians(boardAlignment->yawDegrees  );
  63
  64     buildRotationMatrix(&rotationAngles, &boardRotation);
  65 }
  66
  67 static FAST_CODE void alignBoard(float *vec)
  68 {
  69     applyRotation(vec, &boardRotation);
  70 }
  71
  72 FAST_CODE_NOINLINE void alignSensorViaMatrix(float *dest, fp_rotationMatrix_t* sensorRotationMatrix)
  73 {
  74     applyRotation(dest, sensorRotationMatrix);
  75
  76     if (!standardBoardAlignment) {
  77         alignBoard(dest);
  78     }
  79 }
  80
  81 FAST_CODE void alignSensorViaRotation(float *dest, uint8_t rotation)
  82 {
  83     const float x = dest[X];
  84     const float y = dest[Y];
  85     const float z = dest[Z];
  86
  87     switch (rotation) {
  88     default:
  89     case CW0_DEG:
  90         dest[X] = x;
  91         dest[Y] = y;
  92         dest[Z] = z;
  93         break;
  94     case CW90_DEG:
  95         dest[X] = y;
  96         dest[Y] = -x;
  97         dest[Z] = z;
  98         break;
  99     case CW180_DEG:
 100         dest[X] = -x;
 101         dest[Y] = -y;
 102         dest[Z] = z;
 103         break;
 104     case CW270_DEG:
 105         dest[X] = -y;
 106         dest[Y] = x;
 107         dest[Z] = z;
 108         break;
 109     case CW0_DEG_FLIP:
 110         dest[X] = -x;
 111         dest[Y] = y;
 112         dest[Z] = -z;
 113         break;
 114     case CW90_DEG_FLIP:
 115         dest[X] = y;
 116         dest[Y] = x;
 117         dest[Z] = -z;
 118         break;
 119     case CW180_DEG_FLIP:
 120         dest[X] = x;
 121         dest[Y] = -y;
 122         dest[Z] = -z;
 123         break;
 124     case CW270_DEG_FLIP:
 125         dest[X] = -y;
 126         dest[Y] = -x;
 127         dest[Z] = -z;
 128         break;
 129     }
 130
 131     if (!standardBoardAlignment) {
 132         alignBoard(dest);
 133     }
 134 }