You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

sbn.cc 8.4KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333
  1. /*
  2. Locosys NaviGPS GT-31/BGT-31 SiRF binary logging format (SBN)
  3. Copyright (C) 2008 Rodney Lorrimar <rodney@rodney.id.au>
  4. Copyright (C) 2005 Robert Lipe, robertlipe+source@gpsbabel.org
  5. This program 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 2 of the License, or
  8. (at your option) any later version.
  9. This program is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with this program; if not, write to the Free Software
  15. Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA
  16. */
  17. #include "defs.h"
  18. #include "navilink.h"
  19. #define MYNAME "sbn"
  20. static gbfile* file_handle = NULL;
  21. static
  22. arglist_t sbn_args[] = {
  23. ARG_TERMINATOR
  24. };
  25. /**********************************************************************/
  26. /* Packets are encoded according to the SiRF Binary Protocol.
  27. *
  28. * http://www.navmanwirelessoem.com/oem/customer-support/oem-ne
  29. * ws/product-briefs-and-data-sheets/jupiter-32-xlp-new2/sirf-b
  30. * inary-protocol-reference-manual
  31. *
  32. * The important packet is "Geodetic Navigation Data" (Message ID 41).
  33. */
  34. #define PID_SBN 41
  35. #define SBN_RECORD_LEN 97 /* 91 plus three two shorts added by Locosys */
  36. /* V1.3 of the s/w added SDOP and and VSDOP bytes */
  37. #define PID_VISIBLE_LIST 13
  38. #define PID_QRY_INFORMATION 253
  39. #define QRY_INFORMATION_LEN 41
  40. #define INFO_USERNAME_LEN 13
  41. #define INFO_SERIAL_NUM_LEN 9
  42. #define INFO_LOG_RATE_LEN 3
  43. #define INFO_VERSION_LEN 12
  44. #define INFO_SEP ','
  45. /*
  46. * Very similar to read_packet in navilink.c, except reads from file
  47. * instead of serial, and integers are read in big endian order.
  48. */
  49. static size_t
  50. read_packet(int* type, void* payload, size_t max_len)
  51. {
  52. size_t size, data_size;
  53. unsigned char start[4];
  54. unsigned int checksum_exp, checksum_act;
  55. unsigned char* data;
  56. if (gbfread(start, sizeof(start), 1, file_handle) != 1) {
  57. if (gbfeof(file_handle)) {
  58. return 0;
  59. }
  60. fatal(MYNAME ": Format error: No packet start.\n");
  61. }
  62. if (start[0] != 0xa0 || start[1] != 0xa2) {
  63. fatal(MYNAME ": Format error: Bad packet start.\n");
  64. }
  65. size = be_readu16(start + 2);
  66. if (size < 1 || max_len < size) {
  67. fatal(MYNAME ": Format error: unexpected size: %d.\n", (int) size);
  68. }
  69. /* allocate space for checksum and trailing 0xb0b3 */
  70. data_size = size + 4;
  71. /* data_size can be up to about 64k */
  72. data = (unsigned char*) xmalloc(data_size);
  73. if (gbfread(data, data_size, 1, file_handle) != 1) {
  74. fatal(MYNAME ": Format error: could not read %d bytes.\n",
  75. (int) data_size);
  76. }
  77. *type = data[0];
  78. checksum_exp = be_readu16(data + size);
  79. checksum_act = navilink_checksum_packet(data, size);
  80. if (checksum_exp != checksum_act) {
  81. fatal(MYNAME ": Checksum error - expected %x got %x\n",
  82. checksum_exp, checksum_act);
  83. }
  84. if (data[size + 2] != 0xb0 || data[size + 3] != 0xb3) {
  85. fatal(MYNAME ": Format error: Bad packet trailer\n");
  86. }
  87. --size;
  88. memcpy(payload, data + 1, size);
  89. xfree(data);
  90. return size;
  91. }
  92. #ifdef LOCOSYS_PARSE_FILE_ID
  93. static size_t
  94. hdrcpy(char* dest, const char* src, size_t max_len)
  95. {
  96. size_t i;
  97. for (i = 0; i < max_len && *src != INFO_SEP; i++) {
  98. *dest++ = *src++;
  99. }
  100. *dest++ = 0;
  101. return ++i;
  102. }
  103. #endif /* LOCOSYS_PARSE_FILE_ID */
  104. int
  105. locosys_decode_file_id(char* header, size_t len)
  106. {
  107. #ifdef LOCOSYS_PARSE_FILE_ID
  108. /*
  109. * MID_FILE_ID(0xfd) contains the following payload :
  110. * User Name, Serial Number, Log Rate, Firmware Version
  111. * >field separator:","
  112. * >User Name : MAX CHAR(13)
  113. * >Serial Number : MAX CHAR(8)
  114. * >Log Rate : MAX CHAR 3, 0..255 in seconds
  115. * >Firmware Version : MAX CHAR (13)
  116. */
  117. char username[INFO_USERNAME_LEN + 1];
  118. char serial_num[INFO_SERIAL_NUM_LEN + 1];
  119. char log_rate[INFO_LOG_RATE_LEN + 1];
  120. char version[INFO_VERSION_LEN + 1];
  121. char* p = header;
  122. p += hdrcpy(username, p, INFO_USERNAME_LEN);
  123. p += hdrcpy(serial_num, p, INFO_SERIAL_NUM_LEN);
  124. p += hdrcpy(log_rate, p, INFO_LOG_RATE_LEN);
  125. p += hdrcpy(version, p, INFO_VERSION_LEN);
  126. printf(MYNAME ": Username: %s\n", username);
  127. printf(MYNAME ": Serial Number: %s\n", serial_num);
  128. printf(MYNAME ": Log rate (seconds): %s\n", log_rate);
  129. printf(MYNAME ": Firmware version: %s\n", version);
  130. #endif /* LOCOSYS_PARSE_FILE_ID */
  131. return TRUE;
  132. }
  133. static void
  134. read_sbn_header(route_head* track)
  135. {
  136. char header[QRY_INFORMATION_LEN];
  137. size_t len;
  138. int type = 0;
  139. len = read_packet(&type, header, sizeof(header));
  140. if (len == 0 || type != PID_QRY_INFORMATION ||
  141. !locosys_decode_file_id(header, len)) {
  142. fatal(MYNAME ": Format error: Couldn't read SBN header."
  143. "This probably isn't a SBN file.\n");
  144. }
  145. }
  146. static int
  147. is_sbn_valid(const unsigned char* buffer)
  148. {
  149. /* valid navigation (any bit set implies navigation solution is not
  150. * optimal) */
  151. return !buffer[0] && !buffer[1];
  152. }
  153. static fix_type
  154. decode_sbn_mode(const unsigned char* mode)
  155. {
  156. static const fix_type fixes[8] = {
  157. fix_none, /* 000 No Nav */
  158. fix_none, /* 001 1 SV solution */
  159. fix_none, /* 010 2 SV solution */
  160. fix_2d, /* 011 3 SV solution (2D) */
  161. fix_3d, /* 100 4 or more SV solution (3D) */
  162. fix_2d, /* 101 Least Square 2D solution */
  163. fix_3d, /* 110 Least Square 3D solution */
  164. fix_none /* 111 DR solution (no SV) */
  165. };
  166. int dgps_correction = *mode & 0x80;
  167. fix_type fix = fixes[*mode & 7];
  168. return dgps_correction && fix == fix_3d ? fix_dgps : fix;
  169. }
  170. static void
  171. decode_sbn_datetime(const unsigned char* buffer, Waypoint* waypt)
  172. {
  173. struct tm tm;
  174. int ms = be_readu16(buffer + 6);
  175. tm.tm_sec = ms / 1000;
  176. tm.tm_min = buffer[5];
  177. tm.tm_hour = buffer[4];
  178. tm.tm_mday = buffer[3];
  179. tm.tm_mon = buffer[2] - 1;
  180. tm.tm_year = be_readu16(buffer) - 1900;
  181. waypt->SetCreationTime(mkgmtime(&tm), (ms % 1000));
  182. }
  183. static void
  184. decode_sbn_position(const unsigned char* buffer, Waypoint* waypt)
  185. {
  186. waypt->latitude = be_read32(buffer + 0) * 0.0000001;
  187. waypt->longitude = be_read32(buffer + 4) * 0.0000001;
  188. waypt->altitude = be_read32(buffer + 12) * 0.01;
  189. }
  190. static Waypoint*
  191. decode_sbn_record(unsigned char* buffer)
  192. {
  193. Waypoint* waypt = NULL;
  194. waypt = new Waypoint;
  195. if (is_sbn_valid(buffer)) {
  196. waypt->fix = decode_sbn_mode(buffer + 3);
  197. } else {
  198. waypt->fix = fix_none;
  199. }
  200. decode_sbn_datetime(buffer + 10, waypt);
  201. decode_sbn_position(buffer + 22, waypt);
  202. WAYPT_SET(waypt, speed, be_read16(buffer + 39) * 0.01f);
  203. WAYPT_SET(waypt, course, be_read16(buffer + 41) * 0.01f);
  204. waypt->sat = buffer[87];
  205. waypt->hdop = buffer[88] * 0.2f;
  206. return waypt;
  207. }
  208. static void
  209. add_logpoints(route_head* track)
  210. {
  211. unsigned char buffer[SBN_RECORD_LEN];
  212. int type = 0;
  213. while (read_packet(&type, buffer, sizeof(buffer))) {
  214. if (type == PID_SBN) {
  215. track_add_wpt(track, decode_sbn_record(buffer));
  216. } else if (type == PID_VISIBLE_LIST) {
  217. /* A list of visible SVs, their IDs, azimuths, elevations.
  218. * Not storing this info for now. */
  219. } else {
  220. warning(MYNAME ": Format error: Unknown packet type 0x%02x.\n", type);
  221. }
  222. }
  223. }
  224. /**********************************************************************/
  225. static void
  226. sbn_rd_init(const QString& fname)
  227. {
  228. file_handle = gbfopen(fname, "r", MYNAME);
  229. }
  230. static void
  231. sbn_rd_deinit(void)
  232. {
  233. gbfclose(file_handle);
  234. }
  235. static void
  236. sbn_read(void)
  237. {
  238. if (global_opts.masked_objective & TRKDATAMASK) {
  239. route_head* track;
  240. track = route_head_alloc();
  241. track_add_head(track);
  242. read_sbn_header(track);
  243. add_logpoints(track);
  244. }
  245. }
  246. static void
  247. sbn_exit(void)
  248. {
  249. }
  250. /**********************************************************************/
  251. ff_vecs_t sbn_vecs = {
  252. ff_type_file,
  253. {
  254. ff_cap_none /* waypoints */,
  255. ff_cap_read /* tracks */,
  256. ff_cap_none /* routes */
  257. },
  258. sbn_rd_init,
  259. NULL,
  260. sbn_rd_deinit,
  261. NULL,
  262. sbn_read,
  263. NULL,
  264. sbn_exit,
  265. sbn_args,
  266. /* Characters are always encoded in ASCII. Even if the unit is set
  267. * to Chinese language, only ASCII characters can be entered. */
  268. CET_CHARSET_ASCII, 0
  269. };
  270. /**********************************************************************/