1
0

gpx.c 5.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213
  1. #include <string.h>
  2. #include <avr/pgmspace.h>
  3. #include "xprintf.h"
  4. #include "math.h"
  5. #include "main.h"
  6. #include "gpx.h"
  7. #include "ff.h"
  8. #define KALMAN_Q 8.5e-6
  9. #define KALMAN_R 4e-5
  10. #define KALMAN_ERR_MAX 6e-4
  11. __flash const char xml_header[] = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
  12. "<gpx xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns=\"http://www.topografix.com/GPX/1/1\" xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\" version=\"1.1\" creator=\"k4be\">\n"
  13. "\t<trk>\n"
  14. "\t\t<trkseg>\n";
  15. FIL gpx_file;
  16. static char buf[sizeof(xml_header)+1];
  17. struct kalman_s {
  18. unsigned char initialized;
  19. float x_est_last;
  20. float P_last;
  21. float Q;
  22. float R;
  23. float K;
  24. };
  25. #define PREV_POINTS_LENGTH 4
  26. #define AVG_COUNT 3
  27. #define MIN_DIST_DELTA 2.0
  28. struct {
  29. struct location_s data[PREV_POINTS_LENGTH];
  30. unsigned char start;
  31. unsigned char count;
  32. } prev_points;
  33. struct location_s last_saved;
  34. static struct kalman_s kalman[2];
  35. #if GPX_POINTS_PROCESSING_ENABLE == 1
  36. static struct {
  37. float lat;
  38. float lon;
  39. time_t time;
  40. } avg_store;
  41. #endif
  42. static unsigned char avg_count;
  43. float kalman_predict(struct kalman_s *k, float data);
  44. void kalman_init(struct kalman_s *k);
  45. float distance(struct location_s *pos1, struct location_s *pos2);
  46. void prev_points_append(struct location_s *new){
  47. prev_points.data[(prev_points.start + prev_points.count)%PREV_POINTS_LENGTH] = *new;
  48. if(++prev_points.count > PREV_POINTS_LENGTH){
  49. prev_points.count--;
  50. prev_points.start++;
  51. prev_points.start %= PREV_POINTS_LENGTH;
  52. }
  53. }
  54. struct location_s *prev_points_get(unsigned char index){
  55. unsigned char i, addr = prev_points.start;
  56. for(i=0; i<index; i++){
  57. addr++;
  58. addr %= PREV_POINTS_LENGTH;
  59. }
  60. return &prev_points.data[addr];
  61. }
  62. unsigned char gpx_init(FIL *file) {
  63. unsigned int bw;
  64. kalman_init(&kalman[0]);
  65. kalman_init(&kalman[1]);
  66. prev_points.count = 0;
  67. avg_count = 0;
  68. last_saved.lon = 0;
  69. last_saved.lat = 0;
  70. last_saved.time = 0;
  71. strcpy_P(buf, xml_header);
  72. return f_write(file, buf, strlen(buf), &bw);
  73. }
  74. unsigned char gpx_write(struct location_s *loc, FIL *file) {
  75. unsigned int bw;
  76. const char *time;
  77. time = get_iso_time(loc->time, 0);
  78. xsprintf(buf, PSTR("\t\t\t<trkpt lat=\"%.8f\" lon=\"%.8f\">\n\t\t\t\t<time>%s</time>\n"), loc->lat, loc->lon, time);
  79. /* alt */
  80. strcat_P(buf, PSTR("\t\t\t</trkpt>\n"));
  81. return f_write(file, buf, strlen(buf), &bw);
  82. }
  83. unsigned char gpx_close(FIL *file) {
  84. unsigned int bw;
  85. strcpy_P(buf, PSTR("\t\t</trkseg>\n\t</trk>\n</gpx>\n"));
  86. f_write(file, buf, strlen(buf), &bw);
  87. return f_close(file);
  88. }
  89. void gpx_process_point(struct location_s *loc, FIL *file){
  90. #if GPX_POINTS_PROCESSING_ENABLE == 1
  91. float lon_est, lon_err, lat_est, lat_err;
  92. struct location_s *ptr;
  93. struct location_s nloc;
  94. lat_est = kalman_predict(&kalman[0], loc->lat);
  95. lon_est = kalman_predict(&kalman[1], loc->lon);
  96. lat_err = fabs(loc->lat - lat_est);
  97. lon_err = fabs(loc->lon - lon_est);
  98. // xprintf(PSTR("lat_err: %e, lon_err: %e, limit: %e\r\n"), lat_err, lon_err, (float)KALMAN_ERR_MAX);
  99. if(lat_err > KALMAN_ERR_MAX || lon_err > KALMAN_ERR_MAX){
  100. xputs_P(PSTR("KALMAN REJECT\r\n"));
  101. return;
  102. }
  103. loc->lat = lat_est;
  104. loc->lon = lon_est;
  105. prev_points_append(loc);
  106. if(prev_points.count == PREV_POINTS_LENGTH){
  107. float dist12 = distance(prev_points_get(0), prev_points_get(1));
  108. float dist34 = distance(prev_points_get(2), prev_points_get(3));
  109. float dist32 = distance(prev_points_get(2), prev_points_get(1));
  110. xprintf(PSTR("New distance: %fm\r\n"), dist32);
  111. if(dist34 > dist12 && dist32 > dist12){
  112. xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
  113. return;
  114. }
  115. ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
  116. } else {
  117. if(prev_points.count >= PREV_POINTS_LENGTH-2){
  118. ptr = prev_points_get(prev_points.count - 2);
  119. xputs_P(PSTR("NEW\r\n"));
  120. } else {
  121. return;
  122. }
  123. }
  124. if(distance(&last_saved, ptr) < MIN_DIST_DELTA){
  125. xputs_P(PSTR("Too small position change REJECT\r\n"));
  126. return;
  127. }
  128. xputs_P(PSTR("ACCEPT\r\n"));
  129. avg_store.lat += ptr->lat;
  130. avg_store.lon += ptr->lon;
  131. if(avg_count == AVG_COUNT/2)
  132. avg_store.time = ptr->time;
  133. if(++avg_count == AVG_COUNT){
  134. nloc.lat = avg_store.lat / AVG_COUNT;
  135. nloc.lon = avg_store.lon / AVG_COUNT;
  136. nloc.time = avg_store.time;
  137. avg_count = 0;
  138. avg_store.lat = 0;
  139. avg_store.lon = 0;
  140. avg_store.time = 0;
  141. last_saved = nloc;
  142. gpx_write(&nloc, file);
  143. return;
  144. }
  145. #else
  146. xputs_P(PSTR("ACCEPT\r\n"));
  147. gpx_write(loc, file);
  148. #endif
  149. }
  150. void kalman_init(struct kalman_s *k){
  151. k->initialized = 0;
  152. k->P_last = 0;
  153. //the noise in the system
  154. k->Q = KALMAN_Q; // process variance
  155. k->R = KALMAN_R; // measurement variance
  156. k->K = 0;
  157. }
  158. float kalman_predict(struct kalman_s *k, float data){
  159. if(!k->initialized){
  160. //initial values for the kalman filter
  161. k->x_est_last = data;
  162. k->initialized = 1;
  163. return data;
  164. }
  165. //do a prediction
  166. float x_temp_est = k->x_est_last;
  167. float P_temp = k->P_last + k->Q;
  168. //calculate the Kalman gain
  169. k->K = P_temp * (1.0/(P_temp + k->R));
  170. //correct
  171. float x_est = x_temp_est + k->K * (data - x_temp_est);
  172. k->P_last = (1 - k->K) * P_temp;
  173. k->x_est_last = x_est;
  174. return x_est;
  175. }
  176. float distance(struct location_s *pos1, struct location_s *pos2){
  177. float lon_delta = fabs(pos1->lon - pos2->lon) * 111139.0;
  178. float lat_delta = fabs(pos1->lat - pos2->lat) * 111139.0;
  179. // xprintf(PSTR("lat1=%f; lat2=%f; lon1=%f; lon2=%f; lat_delta=%f; lon_delta=%f\r\n"), pos1->lat, pos2->lat, pos1->lon, pos2->lon, lon_delta, lat_delta);
  180. return sqrtf(lon_delta * lon_delta + lat_delta * lat_delta);
  181. }