src/main/tracker/gps_estimation.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 "gps_estimation.h"
  21
  22 #include <stdint.h>
  23 #include <stdbool.h>
  24 #include "Arduino.h"
  25 #include "telemetry.h"
  26 #include "interpolation.h"
  27
  28 #define earthRadius 6378137.0f
  29
  30 float speed;
  31 uint16_t vartime;
  32 uint32_t estimatedTime;
  33 float estimatedDistance;
  34 float estimatedAccDistance;
  35 float estimatedHeading;
  36 float estimatedSpeed;
  37 uint16_t positionIndex=0;
  38
  39 bool interpolationOn;
  40
  41 #define PVQ_ELEMENTS 20
  42 pvQElement_t epsVectors[PVQ_ELEMENTS];
  43 int PVQ_SIZE;
  44 uint8_t pvQIn;
  45 uint8_t pvQOut;
  46
  47 void pvInit(void)
  48 {
  49         PVQ_SIZE = PVQ_ELEMENTS;
  50         pvQIn = 0;
  51         pvQOut = 0;
  52 }
  53
  54 bool pvFull(){
  55         return (pvQIn == (( pvQOut - 1 + PVQ_SIZE) % PVQ_SIZE));
  56 }
  57 bool pvEmpty(){
  58         return (pvQIn == pvQOut);
  59 }
  60 bool pvPut(epsVector_t *pvector, uint8_t vectorType){
  61     if(pvFull())
  62     {
  63          return true;
  64     }
  65
  66     epsVectors[pvQIn].heading = pvector->heading;
  67     epsVectors[pvQIn].distance = pvector->distance;
  68     epsVectors[pvQIn].speed = pvector->speed;
  69     epsVectors[pvQIn].lat_a = pvector->lat_a;
  70     epsVectors[pvQIn].lat_b = pvector->lat_b;
  71     epsVectors[pvQIn].lon_a = pvector->lon_a;
  72     epsVectors[pvQIn].lon_b = pvector->lon_b;
  73     epsVectors[pvQIn].lat_sgn = pvector->lat_sgn;
  74     epsVectors[pvQIn].lon_sgn = pvector->lon_sgn;
  75     epsVectors[pvQIn].time = pvector->time;
  76     epsVectors[pvQIn].index = pvector->index;
  77     epsVectors[pvQIn].type = vectorType;
  78
  79     pvQIn = (pvQIn + 1) % PVQ_SIZE;
  80
  81     return false;
  82 }
  83
  84 pvQElement_t pvGet(void){
  85
  86         pvQElement_t pvector = {0,0,0,0,0,0,0,0};
  87
  88     if(pvEmpty())
  89     {
  90         return pvector;
  91     }
  92
  93     pvector = epsVectors[pvQOut];
  94
  95     pvQOut = (pvQOut + 1) % PVQ_SIZE;
  96
  97     return pvector;
  98 }
  99
 100 float epsVectorSpeed(uint32_t last_time,uint32_t currentTime, float distance){
 101         if(last_time == currentTime || distance == 0)
 102                 return 0;
 103         uint16_t vartime;
 104         float speed;
 105         vartime = currentTime - last_time;
 106         speed = distance / (vartime / 1000.0f);
 107         return speed;
 108 }
 109
 110 void epsVectorAddPoint(epsVector_t *last, epsVector_t *current){
 111
 112                 iPoint_t delta;
 113
 114
 115                 delta.speed = current->speed - last->speed;
 116                 delta.heading = current->heading - last->heading;
 117
 118                 if(current->heading < last->heading)
 119                         delta.heading = delta.heading + 360;
 120
 121                 if (delta.heading > 180)
 122                         delta.heading -= 360.0f;
 123                 else if (delta.heading < -180)
 124                         delta.heading += 360.0f;
 125
 126                 iPutPoint(current->time,delta.heading,delta.speed);
 127 }
 128
 129 void epsVectorEstimate(epsVector_t *last, epsVector_t *current, epsVector_t *estimated,epsVectorGain_t gain, uint32_t eps_frequency,uint8_t eps_mode){
 130         float angularDistance;
 131         float headingRadians;
 132         iPoint_t delta;
 133         float lat2;
 134         float lon2;
 135         float x;
 136         float y;
 137
 138         estimatedHeading = current->heading;
 139         estimatedSpeed = current->speed;
 140         estimatedTime = millis();
 141
 142         if(eps_mode > 1) {
 143                 vartime = estimatedTime - current->time;
 144                 estimatedDistance = current->speed * (vartime / 1000.0f) *(gain.distance / 100.0f);
 145                 if(interpolationOn) {
 146                         delta.heading = 0;
 147                         delta.speed = 0;
 148                         if(iFull()) {
 149                                 delta = iEval(estimatedTime);
 150                                 estimatedHeading = current->heading + delta.heading * (gain.heading / 100.0f);
 151                                 if(estimatedHeading < 0)
 152                                         estimatedHeading += 360;
 153                                 if(estimatedHeading > 360)
 154                                         estimatedHeading -= 360;
 155                                 //estimatedHeading = fmod(estimatedHeading,360.0f);
 156                                 estimatedSpeed = current->speed + delta.speed * (gain.speed / 100.0f);
 157                                 estimatedDistance = estimatedSpeed * (vartime / 1000.0f);
 158                         }
 159                 }
 160         } else if(eps_mode == 1) {
 161                 estimatedDistance = (current->distance * gain.distance / 100.0);
 162         }
 163
 164         estimatedAccDistance += estimatedDistance;
 165
 166         angularDistance = estimatedDistance / earthRadius;
 167         //
 168         headingRadians = radians(estimatedHeading);
 169         //
 170         lat2 = asin(sin(radians(current->lat/TELEMETRY_LATLON_DIVIDER_F))*cos(angularDistance) + cos(radians(current->lat/TELEMETRY_LATLON_DIVIDER_F))*sin(angularDistance)*cos(headingRadians));
 171         x = cos(angularDistance) - sin(radians(current->lat/TELEMETRY_LATLON_DIVIDER_F)) * sin(lat2);
 172         y = sin(headingRadians) * sin(angularDistance) * cos(radians(current->lat/TELEMETRY_LATLON_DIVIDER_F));
 173         lon2 = radians(current->lon/TELEMETRY_LATLON_DIVIDER_F) + atan2(y, x);
 174         lat2 = degrees(lat2);
 175         lon2 = fmod(degrees(lon2) + 540,360)-180;
 176         //
 177         estimated->lat = lat2 * TELEMETRY_LATLON_DIVIDER_I;
 178         estimated->lon = lon2 * TELEMETRY_LATLON_DIVIDER_I;
 179         estimated->lat_a = abs(estimated->lat / TELEMETRY_LATLON_DIVIDER_I);
 180         estimated->lat_b = abs(estimated->lat % TELEMETRY_LATLON_DIVIDER_I);
 181         estimated->lat_sgn = (estimated->lat < 0) ? -1 : 1;
 182         estimated->lon_a = abs(estimated->lon / TELEMETRY_LATLON_DIVIDER_I);
 183         estimated->lon_b = abs(estimated->lon % TELEMETRY_LATLON_DIVIDER_I);
 184         estimated->lon_sgn = (estimated->lon < 0) ? -1 : 1;
 185         estimated->time = estimatedTime;
 186         estimated->heading = estimatedHeading;
 187         estimated->speed = estimatedSpeed;
 188         estimated->distance = estimatedDistance;
 189         positionIndex++;
 190         estimated->index = positionIndex;
 191
 192         if(!pvFull())
 193                 pvPut(estimated,2);
 194 }
 195
 196 void epsVectorsInit(epsVector_t *last, epsVector_t *current, epsVector_t *estimated, uint8_t interpolation,uint8_t points){
 197
 198         positionIndex=0;
 199
 200         last->heading = 0;
 201         last->speed = 0;
 202         last->distance = 0;
 203         last->speed = 0;
 204         last->lat = 0;
 205         last->lon = 0;
 206         last->time = 0;
 207         last->index = 0;
 208
 209         current->heading = 0;
 210         current->speed = 0;
 211         current->distance = 0;
 212         current->speed = 0;
 213         current->lat = 0;
 214         current->lon = 0;
 215         current->time = 0;
 216         current->index = 0;
 217
 218         estimated->heading = 0;
 219         estimated->speed = 0;
 220         estimated->distance = 0;
 221         estimated->speed = 0;
 222         estimated->lat = 0;
 223         estimated->lon = 0;
 224         estimated->time = 0;
 225         estimated->index = 0;
 226
 227         pvInit();
 228
 229         if(interpolation == 1) {
 230                 iInit(points);
 231                 interpolationOn = true;
 232         }
 233 }
 234
 235 void epsVectorCurrentToLast(epsVector_t *current,epsVector_t *last){
 236         last->heading = current->heading;
 237         last->speed = current->speed;
 238         last->distance = current->distance;
 239         last->speed = current->speed;
 240         last->lat = current->lat;
 241         last->lon = current->lon;
 242         last->time = current->time;
 243         last->lat_a = current->lat_a;
 244         last->lat_b = current->lat_b;
 245         last->lat_sgn = current->lat_sgn;
 246         last->lon_a = current->lon_a;
 247         last->lon_b = current->lon_b;
 248         last->lon_sgn = current->lon_sgn;
 249 }
 250
 251 void epsVectorLoad(epsVector_t *current,int32_t lat,int32_t lon,float distance, uint32_t last_time, uint32_t currentTime){
 252
 253         current->lat = lat;
 254         current->lon = lon;
 255         current->time = currentTime;
 256         current->distance = distance;
 257         current->speed = epsVectorSpeed(last_time,currentTime,distance);
 258         current->lat_a = abs(current->lat / TELEMETRY_LATLON_DIVIDER_I);
 259         current->lat_b = abs(current->lat % TELEMETRY_LATLON_DIVIDER_I);
 260         current->lat_sgn = (current->lat < 0) ? -1 : 1;
 261         current->lon_a = abs(current->lon / TELEMETRY_LATLON_DIVIDER_I);
 262         current->lon_b = abs(current->lon % TELEMETRY_LATLON_DIVIDER_I);
 263         current->lon_sgn = (current->lon < 0) ? -1 : 1;
 264         positionIndex++;
 265         current->index = positionIndex;
 266
 267 }
 268
 269 uint16_t getPositionVectorIndex(void){
 270         return positionIndex++;
 271 }