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

   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 <stdint.h>
  26 #include "common/maths.h"
  27 #include "common/quaternion.h"
  28
  29 #define EARTH_RADIUS (6378.137f)
  30 #define PWM_TO_FLOAT_0_1(x) ((float)(((int)x - 1000) / 1000.0f))
  31 #define PWM_TO_FLOAT_MINUS_1_1(x) ((float)(((int)x - 1500) / 500.0f))
  32 #define FLOAT_0_1_TO_PWM(x) ((uint16_t)(x * 1000.0f) + 1000.0f)
  33 #define FLOAT_MINUS_1_1_TO_PWM(x) ((float)((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f))
  34
  35 int16_t constrainToInt16(float value);
  36 void transformVectorEarthToBody(fpVector3_t *v, const fpQuaternion_t *quat);
  37 void computeQuaternionFromRPY(fpQuaternion_t *quat, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);