Subversion Repositories chibiosIgnition

Rev

Blame | Last modification | View Log | Download | RSS feed

  1. /*
  2.  * ap_math.c
  3.  
  4.  *
  5.  *  Created on: 31 Jan 2016
  6.  *      Author: Mike
  7.  */
  8. #include "ch.h"
  9. #include "hal.h"
  10. #include "math.h"
  11.  
  12. #include "ap_math.h"
  13. /* this is an approximate maths library where all the answers are accurate enough */
  14. static uint16_t sintab[90];
  15.  
  16. void ap_init(void)
  17. {
  18.         uint8_t i;
  19.         for(i=0;i<90;i++)
  20.         {
  21.           sintab[i]=sin(i/180.0*3.14159)*AP_K;
  22.         }
  23. }
  24.  
  25. /* returns 255 * sin(ang) where ang is in degrees */
  26. int ap_sin(int ang)
  27. {
  28.         /* wrap into range */
  29.         while(ang<0)
  30.         {
  31.                 ang+= 360;
  32.         }
  33. while(ang>=360)
  34.         {
  35.                 ang-= 360;
  36.         }
  37.  
  38.         if(ang>=0 && ang<90)
  39.         {
  40.                 return(sintab[ang]);
  41.         }
  42.         else
  43.         if(ang==90)
  44.         {
  45.                 return AP_K;
  46.         }
  47.         else
  48.         if(ang>90 && ang<=180)
  49.         {
  50.                 return(sintab[180-ang]);
  51.         }
  52.         else
  53.         if(ang==270)
  54.         {
  55.             return -AP_K;
  56.         }
  57.         if(ang>180 && ang< 270)
  58.         {
  59.                 return(-sintab[ang-180]);
  60.         }
  61.         else
  62.     /* ang > 270 and ang < 360 */
  63.         {
  64.         return(-sintab[360-ang]);
  65.         }
  66. }
  67.  
  68. /* returns AP_K * cos(ang) where ang is in degrees */
  69. int ap_cos(int ang)
  70. {
  71.         return ap_sin(90-ang);
  72. }
  73.  
  74.  
  75.