src/main/common/sensor_alignment.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 <string.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/build_config.h"
  27
  28 #include "common/sensor_alignment.h"
  29 #include "common/sensor_alignment_impl.h"
  30 #include "common/vector.h"
  31
  32 void buildRotationMatrixFromAngles(matrix33_t *rm, const sensorAlignment_t *rpy)
  33 {
  34     fp_angles_t rotationAngles;
  35     rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(rpy->roll);
  36     rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(rpy->pitch);
  37     rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(rpy->yaw);
  38
  39     buildRotationMatrix(rm, &rotationAngles);
  40 }
  41
  42 void buildAlignmentFromStandardAlignment(sensorAlignment_t* rpy, sensor_align_e stdAlignment)
  43 {
  44     if (stdAlignment == ALIGN_CUSTOM || stdAlignment == ALIGN_DEFAULT) {
  45         return;
  46     }
  47
  48     uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(stdAlignment);
  49
  50     memset(rpy, 0x00, sizeof(sensorAlignment_t));
  51
  52     for (int axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
  53         rpy->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
  54     }
  55 }