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
  31 void buildRotationMatrixFromAlignment(const sensorAlignment_t* sensorAlignment, fp_rotationMatrix_t* rm)
  32 {
  33     fp_angles_t rotationAngles;
  34     rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(sensorAlignment->roll);
  35     rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(sensorAlignment->pitch);
  36     rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(sensorAlignment->yaw);
  37
  38     buildRotationMatrix(&rotationAngles, rm);
  39 }
  40
  41
  42 void buildAlignmentFromStandardAlignment(sensorAlignment_t* sensorAlignment, sensor_align_e alignment)
  43 {
  44     if (alignment == ALIGN_CUSTOM || alignment == ALIGN_DEFAULT) {
  45         return;
  46     }
  47
  48     uint8_t alignmentBits = ALIGNMENT_TO_BITMASK(alignment);
  49
  50     memset(sensorAlignment, 0x00, sizeof(sensorAlignment_t));
  51
  52     for (int axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
  53         sensorAlignment->raw[axis] = DEGREES_TO_DECIDEGREES(90) * ALIGNMENT_AXIS_ROTATIONS(alignmentBits, axis);
  54     }
  55 }