flight/libraries/math/sin_lookup.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilot Math Utilities
   4  * @{
   5  * @addtogroup Sine and cosine methods that use a cached lookup table
   6  * @{
   7  *
   8  * @file       sin_lookup.c
   9  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  10  * @brief      Sine lookup table from flash with 1 degree resolution
  11  *
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "openpilot.h"
  32 #include "math.h"
  33 #include "stdbool.h"
  34 #include "stdint.h"
  35 #include <pios_math.h>
  36
  37 #define FLASH_TABLE
  38 #ifdef FLASH_TABLE
  39 /** Version of the code which precomputes the lookup table in flash **/
  40
  41 // This is a precomputed sin lookup table over 90 degrees that
  42 const float sin_table[] = {
  43     0.000000f, 0.017452f, 0.034899f, 0.052336f, 0.069756f, 0.087156f, 0.104528f, 0.121869f, 0.139173f, 0.156434f,
  44     0.173648f, 0.190809f, 0.207912f, 0.224951f, 0.241922f, 0.258819f, 0.275637f, 0.292372f, 0.309017f, 0.325568f,
  45     0.342020f, 0.358368f, 0.374607f, 0.390731f, 0.406737f, 0.422618f, 0.438371f, 0.453990f, 0.469472f, 0.484810f,
  46     0.500000f, 0.515038f, 0.529919f, 0.544639f, 0.559193f, 0.573576f, 0.587785f, 0.601815f, 0.615661f, 0.629320f,
  47     0.642788f, 0.656059f, 0.669131f, 0.681998f, 0.694658f, 0.707107f, 0.719340f, 0.731354f, 0.743145f, 0.754710f,
  48     0.766044f, 0.777146f, 0.788011f, 0.798636f, 0.809017f, 0.819152f, 0.829038f, 0.838671f, 0.848048f, 0.857167f,
  49     0.866025f, 0.874620f, 0.882948f, 0.891007f, 0.898794f, 0.906308f, 0.913545f, 0.920505f, 0.927184f, 0.933580f,
  50     0.939693f, 0.945519f, 0.951057f, 0.956305f, 0.961262f, 0.965926f, 0.970296f, 0.974370f, 0.978148f, 0.981627f,
  51     0.984808f, 0.987688f, 0.990268f, 0.992546f, 0.994522f, 0.996195f, 0.997564f, 0.998630f, 0.999391f, 0.999848f,
  52     1.000000f, 0.999848f, 0.999391f, 0.998630f, 0.997564f, 0.996195f, 0.994522f, 0.992546f, 0.990268f, 0.987688f,
  53     0.984808f, 0.981627f, 0.978148f, 0.974370f, 0.970296f, 0.965926f, 0.961262f, 0.956305f, 0.951057f, 0.945519f,
  54     0.939693f, 0.933580f, 0.927184f, 0.920505f, 0.913545f, 0.906308f, 0.898794f, 0.891007f, 0.882948f, 0.874620f,
  55     0.866025f, 0.857167f, 0.848048f, 0.838671f, 0.829038f, 0.819152f, 0.809017f, 0.798636f, 0.788011f, 0.777146f,
  56     0.766044f, 0.754710f, 0.743145f, 0.731354f, 0.719340f, 0.707107f, 0.694658f, 0.681998f, 0.669131f, 0.656059f,
  57     0.642788f, 0.629320f, 0.615661f, 0.601815f, 0.587785f, 0.573576f, 0.559193f, 0.544639f, 0.529919f, 0.515038f,
  58     0.500000f, 0.484810f, 0.469472f, 0.453990f, 0.438371f, 0.422618f, 0.406737f, 0.390731f, 0.374607f, 0.358368f,
  59     0.342020f, 0.325568f, 0.309017f, 0.292372f, 0.275637f, 0.258819f, 0.241922f, 0.224951f, 0.207912f, 0.190809f,
  60     0.173648f, 0.156434f, 0.139173f, 0.121869f, 0.104528f, 0.087156f, 0.069756f, 0.052336f, 0.034899f, 0.017452f
  61 };
  62
  63 int sin_lookup_initalize()
  64 {
  65     return 0;
  66 }
  67
  68 #else /* ifdef FLASH_TABLE */
  69 /** Version of the code which allocates the lookup table in heap **/
  70
  71 const int SIN_RESOLUTION = 180;
  72
  73 static float *sin_table;
  74 int sin_lookup_initalize()
  75 {
  76     // Previously initialized
  77     if (sin_table) {
  78         return 0;
  79     }
  80
  81     sin_table = (float *)pios_malloc(sizeof(float) * SIN_RESOLUTION);
  82     if (sin_table == NULL) {
  83         return -1;
  84     }
  85
  86     for (uint32_t i = 0; i < 180; i++) {
  87         sin_table[i] = sinf(DEG2RAD((float)i));
  88     }
  89
  90     return 0;
  91 }
  92
  93 #endif /* ifdef FLASH_TABLE */
  94
  95 /**
  96  * Use the lookup table to return sine(angle) where angle is in radians
  97  * to save flash this cheats and uses trig functions to only save
  98  * 90 values
  99  * @param[in] angle Angle in degrees
 100  * @returns sin(angle)
 101  */
 102 float sin_lookup_deg(float angle)
 103 {
 104     if (sin_table == NULL) {
 105         return 0;
 106     }
 107
 108     // <bug, was> int i_ang = ((int32_t)angle) % 360;
 109     // 1073741760 is a multiple of 360 that is close to 0x3fffffff
 110     // so angle can be a very large number of positive or negative rotations
 111     // this is the fastest fix (no tests, one integer addition)
 112     // and has the largest range since float mantissas are 23-4 bit
 113     // we could halve the lookup table size
 114     // we could interpolate for greater accuracy
 115     int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360;
 116     if (i_ang >= 180) { // for 180 to 360 deg
 117         return -sin_table[i_ang - 180];
 118     } else { // for 0 to 179 deg
 119         return sin_table[i_ang];
 120     }
 121
 122     return 0;
 123 }
 124
 125 /**
 126  * Get cos(angle) using the sine lookup table
 127  * @param[in] angle Angle in degrees
 128  * @returns cos(angle)
 129  */
 130 float cos_lookup_deg(float angle)
 131 {
 132     return sin_lookup_deg(angle + 90);
 133 }
 134
 135 /**
 136  * Use the lookup table to return sine(angle) where angle is in radians
 137  * @param[in] angle Angle in radians
 138  * @returns sin(angle)
 139  */
 140 float sin_lookup_rad(float angle)
 141 {
 142     int degrees = RAD2DEG(angle);
 143
 144     return sin_lookup_deg(degrees);
 145 }
 146
 147 /**
 148  * Use the lookup table to return sine(angle) where angle is in radians
 149  * @param[in] angle Angle in radians
 150  * @returns cos(angle)
 151  */
 152 float cos_lookup_rad(float angle)
 153 {
 154     int degrees = RAD2DEG(angle);
 155
 156     return cos_lookup_deg(degrees);
 157 }