src/main/tracker/interpolation.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * u360gts is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * u360gts is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  17  *
  18  */
  19
  20 #include "interpolation.h"
  21
  22 #define INT_ELEMENTS    10
  23
  24 iPoint_t iPoints[INT_ELEMENTS];
  25
  26 uint8_t INT_QSIZE;
  27 uint8_t iQIn;
  28 uint8_t iQOut;
  29
  30 void iInit(uint8_t n)
  31 {
  32         INT_QSIZE = n;
  33         iQIn = 0;
  34         iQOut = 0;
  35 }
  36
  37 bool iPutPoint(uint32_t time,float heading,float speed){
  38         iPoint_t point;
  39         point.time = time;
  40         point.heading = heading;
  41         point.speed = speed;
  42         if(iPut(point)) {
  43                 iGet();
  44                 iPut(point);
  45         }
  46         return iFull();
  47 }
  48
  49 bool iFull(){
  50         return (iQIn == (( iQOut - 1 + INT_QSIZE) % INT_QSIZE));
  51 }
  52
  53 bool iPut(iPoint_t point){
  54     if(iFull())
  55     {
  56          return true; /* Full */
  57     }
  58
  59     iPoints[iQIn].time = point.time;
  60     iPoints[iQIn].heading = point.heading;
  61     iPoints[iQIn].speed = point.speed;
  62
  63     iQIn = (iQIn + 1) % INT_QSIZE;
  64
  65     return false; // No errors
  66 }
  67
  68 bool iGet(){
  69     if(iQIn == iQOut)
  70     {
  71         return true; /* Queue Empty - nothing to get*/
  72     }
  73
  74     iQOut = (iQOut + 1) % INT_QSIZE;
  75
  76     return false; // No errors
  77 }
  78
  79 iPoint_t iEval(float A){
  80
  81         int i;
  82         int j;
  83         int index_i = iQIn - 1;
  84         int index_j = index_i;
  85         iPoint_t l;
  86         iPoint_t v;
  87         v.heading = 0;
  88         v.speed = 0;
  89         for(i=1; i<INT_QSIZE + 1; i++)
  90         {
  91                 index_i = (index_i + 1) % INT_QSIZE;
  92                 index_j = iQIn - 1;;
  93                 l.heading = iPoints[index_i].heading;
  94                 l.speed = iPoints[index_i].speed;
  95                 for(j=1; j<INT_QSIZE + 1; j++)
  96                 {
  97                         index_j = (index_j + 1) % INT_QSIZE;
  98
  99                         if(index_j!=index_i)
 100                         {
 101                                 l.heading = (l.heading * (A - iPoints[index_j].time * 1.0f))/(iPoints[index_i].time * 1.0f - iPoints[index_j].time * 1.0f);
 102                                 l.speed = (l.speed * (A - iPoints[index_j].time))/(iPoints[index_i].time - iPoints[index_j].time);
 103                                 //printf("%d %d %d %d %.2f %d %d %d\n",i,j,index_i,index_j,A,iPoints[index_i].time,iPoints[index_j].time);
 104                         }
 105                 }
 106                 v.heading = v.heading + l.heading;
 107                 v.speed = v.speed + l.speed;
 108         }
 109         v.time = A;
 110         return v;
 111 }