src/main/common/sensor_alignment_impl.h

   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 #pragma once
  22
  23 //
  24 // alignment bitmasks
  25 //
  26
  27 #define ALIGNMENT_ROTATION_WIDTH 2
  28
  29 #define ALIGNMENT_YAW_ROTATIONS_POSITION   (0 * ALIGNMENT_ROTATION_WIDTH)
  30 #define ALIGNMENT_PITCH_ROTATIONS_POSITION (1 * ALIGNMENT_ROTATION_WIDTH)
  31 #define ALIGNMENT_ROLL_ROTATIONS_POSITION  (2 * ALIGNMENT_ROTATION_WIDTH)
  32
  33 #define ALIGNMENT_YAW_ROTATIONS_MASK (0x3 << ALIGNMENT_YAW_ROTATIONS_POSITION)
  34 #define ALIGNMENT_PITCH_ROTATIONS_MASK (0x3 << ALIGNMENT_PITCH_ROTATIONS_POSITION)
  35 #define ALIGNMENT_ROLL_ROTATIONS_MASK (0x3 << ALIGNMENT_ROLL_ROTATIONS_POSITION)
  36
  37 #define ALIGNMENT_AXIS_ROTATIONS_MASK(axis) (0x3 << ((FD_YAW - axis) * ALIGNMENT_ROTATION_WIDTH))
  38
  39 #define ALIGNMENT_YAW_ROTATIONS(bits) ((bits & ALIGNMENT_YAW_ROTATIONS_MASK) >> ALIGNMENT_YAW_ROTATIONS_POSITION)
  40 #define ALIGNMENT_PITCH_ROTATIONS(bits) ((bits & ALIGNMENT_PITCH_ROTATIONS_MASK) >> ALIGNMENT_PITCH_ROTATIONS_POSITION)
  41 #define ALIGNMENT_ROLL_ROTATIONS(bits) ((bits & ALIGNMENT_ROLL_ROTATIONS_MASK) >> ALIGNMENT_ROLL_ROTATIONS_POSITION)
  42
  43 #define ALIGNMENT_AXIS_ROTATIONS(bits, axis) ((bits & ALIGNMENT_AXIS_ROTATIONS_MASK(axis)) >> ((FD_YAW - axis) * ALIGNMENT_ROTATION_WIDTH))
  44
  45 // [1:0] count of 90 degree rotations from 0
  46 // [3:2] indicates 90 degree rotations on pitch
  47 // [5:4] indicates 90 degree rotations on roll
  48 #define ALIGNMENT_TO_BITMASK(alignment) ((alignment - CW0_DEG) & 0x3) | (((alignment - CW0_DEG) & 0x4) << 1)