src/main/target/SITL/sim/simHelper.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdbool.h>
  26 #include <stdio.h>
  27 #include <math.h>
  28
  29 #include "common/quaternion.h"
  30
  31 #include "target/SITL/sim/simHelper.h"
  32
  33 inline int16_t constrainToInt16(float value)
  34 {
  35     return (int16_t)round(constrain(value, INT16_MIN, INT16_MAX));
  36 }
  37
  38 // Move to quaternion.h ?
  39 void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
  40 {
  41     if (initialRoll > 1800) initialRoll -= 3600;
  42     if (initialPitch > 1800) initialPitch -= 3600;
  43     if (initialYaw > 1800) initialYaw -= 3600;
  44
  45     const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
  46     const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
  47
  48     const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
  49     const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
  50
  51     const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
  52     const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
  53
  54     quat->q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
  55     quat->q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
  56     quat->q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
  57     quat->q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
  58 }
  59
  60 void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat)
  61 {
  62     // HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)
  63     v->y = -v->y;
  64
  65     // From earth frame to body frame
  66     quaternionRotateVector(v, v, quat);
  67 }