gpx.c 5.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205
  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. static struct {
  36. float lat;
  37. float lon;
  38. time_t time;
  39. } avg_store;
  40. static unsigned char avg_count;
  41. float kalman_predict(struct kalman_s *k, float data);
  42. void kalman_init(struct kalman_s *k);
  43. float distance(struct location_s *pos1, struct location_s *pos2);
  44. void prev_points_append(struct location_s *new){
  45. prev_points.data[(prev_points.start + prev_points.count)%PREV_POINTS_LENGTH] = *new;
  46. if(++prev_points.count > PREV_POINTS_LENGTH){
  47. prev_points.count--;
  48. prev_points.start++;
  49. prev_points.start %= PREV_POINTS_LENGTH;
  50. }
  51. }
  52. struct location_s *prev_points_get(unsigned char index){
  53. unsigned char i, addr = prev_points.start;
  54. for(i=0; i<index; i++){
  55. addr++;
  56. addr %= PREV_POINTS_LENGTH;
  57. }
  58. return &prev_points.data[addr];
  59. }
  60. unsigned char gpx_init(FIL *file) {
  61. unsigned int bw;
  62. kalman_init(&kalman[0]);
  63. kalman_init(&kalman[1]);
  64. prev_points.count = 0;
  65. avg_count = 0;
  66. last_saved.lon = 0;
  67. last_saved.lat = 0;
  68. last_saved.time = 0;
  69. strcpy_P(buf, xml_header);
  70. return f_write(file, buf, strlen(buf), &bw);
  71. }
  72. unsigned char gpx_write(struct location_s *loc, FIL *file) {
  73. unsigned int bw;
  74. const char *time;
  75. time = get_iso_time(loc->time);
  76. 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);
  77. /* alt */
  78. strcat_P(buf, PSTR("\t\t\t</trkpt>\n"));
  79. return f_write(file, buf, strlen(buf), &bw);
  80. }
  81. unsigned char gpx_close(FIL *file) {
  82. unsigned int bw;
  83. strcpy_P(buf, PSTR("\t\t</trkseg>\n\t</trk>\n</gpx>\n"));
  84. f_write(file, buf, strlen(buf), &bw);
  85. return f_close(file);
  86. }
  87. void gpx_process_point(struct location_s *loc, FIL *file){
  88. float lon_est, lon_err, lat_est, lat_err;
  89. struct location_s *ptr;
  90. struct location_s nloc;
  91. lat_est = kalman_predict(&kalman[0], loc->lat);
  92. lon_est = kalman_predict(&kalman[1], loc->lon);
  93. lat_err = fabs(loc->lat - lat_est);
  94. lon_err = fabs(loc->lon - lon_est);
  95. // xprintf(PSTR("lat_err: %e, lon_err: %e, limit: %e\r\n"), lat_err, lon_err, (float)KALMAN_ERR_MAX);
  96. if(lat_err > KALMAN_ERR_MAX || lon_err > KALMAN_ERR_MAX){
  97. xputs_P(PSTR("KALMAN REJECT\r\n"));
  98. return;
  99. }
  100. loc->lat = lat_est;
  101. loc->lon = lon_est;
  102. prev_points_append(loc);
  103. if(prev_points.count == PREV_POINTS_LENGTH){
  104. float dist12 = distance(prev_points_get(0), prev_points_get(1));
  105. float dist34 = distance(prev_points_get(2), prev_points_get(3));
  106. float dist32 = distance(prev_points_get(2), prev_points_get(1));
  107. xprintf(PSTR("New distance: %fm\r\n"), dist32);
  108. if(dist34 > dist12 && dist32 > dist12){
  109. xputs_P(PSTR("DISTANCE DIFF REJECT\r\n"));
  110. return;
  111. }
  112. ptr = prev_points_get(PREV_POINTS_LENGTH - 2);
  113. } else {
  114. if(prev_points.count >= PREV_POINTS_LENGTH-2){
  115. ptr = prev_points_get(prev_points.count - 2);
  116. xputs_P(PSTR("NEW\r\n"));
  117. } else {
  118. return;
  119. }
  120. }
  121. if(distance(&last_saved, ptr) < MIN_DIST_DELTA){
  122. xputs_P(PSTR("Too small position change REJECT\r\n"));
  123. return;
  124. }
  125. xputs_P(PSTR("ACCEPT\r\n"));
  126. avg_store.lat += ptr->lat;
  127. avg_store.lon += ptr->lon;
  128. if(avg_count == AVG_COUNT/2)
  129. avg_store.time = ptr->time;
  130. if(++avg_count == AVG_COUNT){
  131. nloc.lat = avg_store.lat / AVG_COUNT;
  132. nloc.lon = avg_store.lon / AVG_COUNT;
  133. nloc.time = avg_store.time;
  134. avg_count = 0;
  135. avg_store.lat = 0;
  136. avg_store.lon = 0;
  137. avg_store.time = 0;
  138. last_saved = nloc;
  139. gpx_write(&nloc, file);
  140. return;
  141. }
  142. }
  143. void kalman_init(struct kalman_s *k){
  144. k->initialized = 0;
  145. k->P_last = 0;
  146. //the noise in the system
  147. k->Q = KALMAN_Q; // process variance
  148. k->R = KALMAN_R; // measurement variance
  149. k->K = 0;
  150. }
  151. float kalman_predict(struct kalman_s *k, float data){
  152. if(!k->initialized){
  153. //initial values for the kalman filter
  154. k->x_est_last = data;
  155. k->initialized = 1;
  156. return data;
  157. }
  158. //do a prediction
  159. float x_temp_est = k->x_est_last;
  160. float P_temp = k->P_last + k->Q;
  161. //calculate the Kalman gain
  162. k->K = P_temp * (1.0/(P_temp + k->R));
  163. //correct
  164. float x_est = x_temp_est + k->K * (data - x_temp_est);
  165. k->P_last = (1 - k->K) * P_temp;
  166. k->x_est_last = x_est;
  167. return x_est;
  168. }
  169. float distance(struct location_s *pos1, struct location_s *pos2){
  170. float lon_delta = fabs(pos1->lon - pos2->lon) * 111139.0;
  171. float lat_delta = fabs(pos1->lat - pos2->lat) * 111139.0;
  172. // 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);
  173. return sqrtf(lon_delta * lon_delta + lat_delta * lat_delta);
  174. }