{"action":"created","comment":{"url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/comments/23820033","id":23820033,"diff_hunk":"@@ -0,0 +1,329 @@\n+/*\n+ * Copyright (C) 2014 Swift Navigation Inc.\n+ * Contact: Fergus Noble <fergus@swift-nav.com>\n+ *\n+ * This source is subject to the license found in the file 'LICENSE' which must\n+ * be be distributed together with this source. All other rights reserved.\n+ *\n+ * THIS CODE AND INFORMATION IS PROVIDED \"AS IS\" WITHOUT WARRANTY OF ANY KIND,\n+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED\n+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.\n+ */\n+\n+#include <stdio.h>\n+#include <string.h>\n+#include <stdlib.h>\n+#include <math.h>\n+\n+#include <libswiftnav/sbp_utils.h>\n+#include <libswiftnav/pvt.h>\n+#include <libswiftnav/constants.h>\n+#include <libswiftnav/ephemeris.h>\n+#include <libswiftnav/coord_system.h>\n+#include <libswiftnav/linear_algebra.h>\n+\n+#include \"board/leds.h\"\n+#include \"position.h\"\n+#include \"nmea.h\"\n+#include \"sbp.h\"\n+#include \"solution.h\"\n+#include \"manage.h\"\n+#include \"simulator.h\"\n+#include \"settings.h\"\n+#include \"timing.h\"\n+#include \"base_obs.h\"\n+\n+extern ephemeris_t es[MAX_SATS];\n+\n+/** \\defgroup base_obs Base station observation handling\n+ * \\{ */\n+\n+/** Mutex to control access to the base station observations. */\n+Mutex base_obs_lock;\n+/** Semaphore that is flagged when a new set of observations are received. */\n+BinarySemaphore base_obs_received;\n+/** Most recent observations from the base station. */\n+obss_t base_obss;\n+\n+/** Mutex to control access to the base station position state.\n+ * (#base_pos_ecef and #base_pos_known) */\n+Mutex base_pos_lock;\n+/** Is the base station position known? i.e. is #base_pos_ecef valid? */\n+bool_t base_pos_known = false;\n+/** Base station known position in ECEF as sent to us by the base station in\n+ * the BASE_POS message.  */\n+double base_pos_ecef[3];\n+\n+/** SBP callback for when the base station sends us a message containing its\n+ * known location.\n+ * Stores the base station position in the global #base_pos_ecef variable and\n+ * sets #base_pos_known to `true`.\n+ */\n+static void base_pos_callback(u16 sender_id, u8 len, u8 msg[], void* context)\n+{\n+  (void) context; (void) len; (void) msg; (void) sender_id;\n+\n+  double llh_degrees[3];\n+  double llh[3];\n+  memcpy(llh_degrees, msg, 3*sizeof(double));\n+\n+  llh[0] = llh_degrees[0] * D2R;\n+  llh[1] = llh_degrees[1] * D2R;\n+  llh[2] = llh_degrees[2];\n+\n+  chMtxLock(&base_pos_lock);\n+  wgsllh2ecef(llh, base_pos_ecef);\n+  base_pos_known = true;\n+  chMtxUnlock();\n+}\n+\n+/** SBP callback for the old style observation messages.\n+ * Just prints a deprecation warning. */\n+static void obs_old_callback(u16 sender_id, u8 len, u8 msg[], void* context)\n+{\n+  (void) context; (void) len; (void) msg; (void) sender_id;\n+  printf(\"Receiving an old deprecated observation message.\\n\");\n+  printf(\"Please update your base station firmware.\\n\");\n+}\n+\n+/** Update the #base_obss state given a new set of obss.\n+ * First sorts by PRN and computes the TDCP Doppler for the observation set. If\n+ * #base_pos_known is false then a single point position solution is also\n+ * calculated. Next the `has_pos`, `pos_ecef` and `sat_dists` fields are filled\n+ * in. Finally the #base_obs_received semaphore is flagged to indicate that new\n+ * observations are available.\n+ *\n+ * \\note This function is stateful as it must store the previous observation\n+ *       set for the TDCP Doppler.\n+ */\n+static void update_obss(obss_t *new_obss)\n+{\n+  /* Ensure observations sorted by PRN. */\n+  qsort(new_obss->nm, new_obss->n,\n+        sizeof(navigation_measurement_t), nav_meas_cmp);\n+\n+  /* Lock mutex before modifying base_obss.\n+   * NOTE: We didn't need to lock it before reading in THIS context as this\n+   * is the only thread that writes to base_obss. */\n+  chMtxLock(&base_obs_lock);\n+\n+  /* Create a set of navigation measurements to store the previous\n+   * observations. */\n+  static u8 n_old = 0;\n+  static navigation_measurement_t nm_old[MAX_CHANNELS];\n+\n+  /* Fill in the navigation measurements in base_obss, using TDCP method to\n+   * calculate the Doppler shift. */\n+  base_obss.n = tdcp_doppler(new_obss->n, new_obss->nm,\n+                             n_old, nm_old, base_obss.nm);\n+  /* Copy over the time. */\n+  base_obss.t = new_obss->t;\n+\n+  /* Copy the current observations over to nm_old so we can difference\n+   * against them next time around. */\n+  memcpy(nm_old, new_obss->nm,\n+         new_obss->n * sizeof(navigation_measurement_t));\n+  n_old = new_obss->n;\n+\n+  /* Reset the `has_pos` flag. */\n+  u8 has_pos_old = base_obss.has_pos;\n+  base_obss.has_pos = 0;\n+  /* Check if the base station has sent us its position explicitly and if so us\n+   * that. No need to lock before reading here as base_pos_* is only written\n+   * from this thread (SBP). */\n+  if (base_pos_known) {\n+    /* Copy the known base station position into `base_obss`. */\n+    memcpy(base_obss.pos_ecef, base_pos_ecef, sizeof(base_pos_ecef));\n+    /* Indicate that the position is valid. */\n+    base_obss.has_pos = 1;\n+  /* The base station wasn't sent to us explicitly but if we have >= 4\n+   * satellites we can calculate it ourselves (approximately). */\n+  } else if (base_obss.n >= 4) {\n+    gnss_solution soln;\n+    dops_t dops;\n+\n+    /* Calculate a position solution. */\n+    s32 ret = calc_PVT(base_obss.n, base_obss.nm, &soln, &dops);","path":"src/base_obs.c","position":146,"original_position":146,"commit_id":"72c17bb5e96decfc704d9f5db878ff32fec662a3","original_commit_id":"72c17bb5e96decfc704d9f5db878ff32fec662a3","user":{"login":"imh","id":391217,"avatar_url":"https://avatars.githubusercontent.com/u/391217?v=3","gravatar_id":"","url":"https://api.github.com/users/imh","html_url":"https://github.com/imh","followers_url":"https://api.github.com/users/imh/followers","following_url":"https://api.github.com/users/imh/following{/other_user}","gists_url":"https://api.github.com/users/imh/gists{/gist_id}","starred_url":"https://api.github.com/users/imh/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/imh/subscriptions","organizations_url":"https://api.github.com/users/imh/orgs","repos_url":"https://api.github.com/users/imh/repos","events_url":"https://api.github.com/users/imh/events{/privacy}","received_events_url":"https://api.github.com/users/imh/received_events","type":"User","site_admin":false},"body":"this is the part that was locking us out from the weird dopplers before?","created_at":"2015-01-30T02:10:57Z","updated_at":"2015-01-30T02:10:57Z","html_url":"https://github.com/swift-nav/piksi_firmware/pull/240#discussion_r23820033","pull_request_url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240","_links":{"self":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/comments/23820033"},"html":{"href":"https://github.com/swift-nav/piksi_firmware/pull/240#discussion_r23820033"},"pull_request":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240"}}},"pull_request":{"url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240","id":28328680,"html_url":"https://github.com/swift-nav/piksi_firmware/pull/240","diff_url":"https://github.com/swift-nav/piksi_firmware/pull/240.diff","patch_url":"https://github.com/swift-nav/piksi_firmware/pull/240.patch","issue_url":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/240","number":240,"state":"open","locked":false,"title":"Observation handling fixes","user":{"login":"fnoble","id":163239,"avatar_url":"https://avatars.githubusercontent.com/u/163239?v=3","gravatar_id":"","url":"https://api.github.com/users/fnoble","html_url":"https://github.com/fnoble","followers_url":"https://api.github.com/users/fnoble/followers","following_url":"https://api.github.com/users/fnoble/following{/other_user}","gists_url":"https://api.github.com/users/fnoble/gists{/gist_id}","starred_url":"https://api.github.com/users/fnoble/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fnoble/subscriptions","organizations_url":"https://api.github.com/users/fnoble/orgs","repos_url":"https://api.github.com/users/fnoble/repos","events_url":"https://api.github.com/users/fnoble/events{/privacy}","received_events_url":"https://api.github.com/users/fnoble/received_events","type":"User","site_admin":false},"body":"Fixes #211 \r\n\r\nThe second commit in the PR fixes the issue of not going into float mode immediately upon getting >= 5 sats. The problem was the observations did not contain a correct Doppler which is used to calculate the velocity and hence triggered the position solver to filter out due to the crazy velocity. This patch uses the `tdcp_doppler` function to fill in the Doppler based on the time difference of carrier phase observables.\r\n\r\nThe rest is mainly refactoring. I pulled the base station observation handling code into its own file, cleaned it up and added much more thorough documentation.","created_at":"2015-01-30T00:32:28Z","updated_at":"2015-01-30T02:10:57Z","closed_at":null,"merged_at":null,"merge_commit_sha":"9727d0583219625c45319ff2a0aa96244e2fd6c7","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240/commits","review_comments_url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240/comments","review_comment_url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/comments/{number}","comments_url":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/240/comments","statuses_url":"https://api.github.com/repos/swift-nav/piksi_firmware/statuses/72c17bb5e96decfc704d9f5db878ff32fec662a3","head":{"label":"fnoble:obs_handling_fixes","ref":"obs_handling_fixes","sha":"72c17bb5e96decfc704d9f5db878ff32fec662a3","user":{"login":"fnoble","id":163239,"avatar_url":"https://avatars.githubusercontent.com/u/163239?v=3","gravatar_id":"","url":"https://api.github.com/users/fnoble","html_url":"https://github.com/fnoble","followers_url":"https://api.github.com/users/fnoble/followers","following_url":"https://api.github.com/users/fnoble/following{/other_user}","gists_url":"https://api.github.com/users/fnoble/gists{/gist_id}","starred_url":"https://api.github.com/users/fnoble/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fnoble/subscriptions","organizations_url":"https://api.github.com/users/fnoble/orgs","repos_url":"https://api.github.com/users/fnoble/repos","events_url":"https://api.github.com/users/fnoble/events{/privacy}","received_events_url":"https://api.github.com/users/fnoble/received_events","type":"User","site_admin":false},"repo":{"id":2464166,"name":"piksi_firmware","full_name":"fnoble/piksi_firmware","owner":{"login":"fnoble","id":163239,"avatar_url":"https://avatars.githubusercontent.com/u/163239?v=3","gravatar_id":"","url":"https://api.github.com/users/fnoble","html_url":"https://github.com/fnoble","followers_url":"https://api.github.com/users/fnoble/followers","following_url":"https://api.github.com/users/fnoble/following{/other_user}","gists_url":"https://api.github.com/users/fnoble/gists{/gist_id}","starred_url":"https://api.github.com/users/fnoble/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/fnoble/subscriptions","organizations_url":"https://api.github.com/users/fnoble/orgs","repos_url":"https://api.github.com/users/fnoble/repos","events_url":"https://api.github.com/users/fnoble/events{/privacy}","received_events_url":"https://api.github.com/users/fnoble/received_events","type":"User","site_admin":false},"private":false,"html_url":"https://github.com/fnoble/piksi_firmware","description":"Firmware for the Piksi GNSS receiver.","fork":true,"url":"https://api.github.com/repos/fnoble/piksi_firmware","forks_url":"https://api.github.com/repos/fnoble/piksi_firmware/forks","keys_url":"https://api.github.com/repos/fnoble/piksi_firmware/keys{/key_id}","collaborators_url":"https://api.github.com/repos/fnoble/piksi_firmware/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/fnoble/piksi_firmware/teams","hooks_url":"https://api.github.com/repos/fnoble/piksi_firmware/hooks","issue_events_url":"https://api.github.com/repos/fnoble/piksi_firmware/issues/events{/number}","events_url":"https://api.github.com/repos/fnoble/piksi_firmware/events","assignees_url":"https://api.github.com/repos/fnoble/piksi_firmware/assignees{/user}","branches_url":"https://api.github.com/repos/fnoble/piksi_firmware/branches{/branch}","tags_url":"https://api.github.com/repos/fnoble/piksi_firmware/tags","blobs_url":"https://api.github.com/repos/fnoble/piksi_firmware/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/fnoble/piksi_firmware/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/fnoble/piksi_firmware/git/refs{/sha}","trees_url":"https://api.github.com/repos/fnoble/piksi_firmware/git/trees{/sha}","statuses_url":"https://api.github.com/repos/fnoble/piksi_firmware/statuses/{sha}","languages_url":"https://api.github.com/repos/fnoble/piksi_firmware/languages","stargazers_url":"https://api.github.com/repos/fnoble/piksi_firmware/stargazers","contributors_url":"https://api.github.com/repos/fnoble/piksi_firmware/contributors","subscribers_url":"https://api.github.com/repos/fnoble/piksi_firmware/subscribers","subscription_url":"https://api.github.com/repos/fnoble/piksi_firmware/subscription","commits_url":"https://api.github.com/repos/fnoble/piksi_firmware/commits{/sha}","git_commits_url":"https://api.github.com/repos/fnoble/piksi_firmware/git/commits{/sha}","comments_url":"https://api.github.com/repos/fnoble/piksi_firmware/comments{/number}","issue_comment_url":"https://api.github.com/repos/fnoble/piksi_firmware/issues/comments/{number}","contents_url":"https://api.github.com/repos/fnoble/piksi_firmware/contents/{+path}","compare_url":"https://api.github.com/repos/fnoble/piksi_firmware/compare/{base}...{head}","merges_url":"https://api.github.com/repos/fnoble/piksi_firmware/merges","archive_url":"https://api.github.com/repos/fnoble/piksi_firmware/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/fnoble/piksi_firmware/downloads","issues_url":"https://api.github.com/repos/fnoble/piksi_firmware/issues{/number}","pulls_url":"https://api.github.com/repos/fnoble/piksi_firmware/pulls{/number}","milestones_url":"https://api.github.com/repos/fnoble/piksi_firmware/milestones{/number}","notifications_url":"https://api.github.com/repos/fnoble/piksi_firmware/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/fnoble/piksi_firmware/labels{/name}","releases_url":"https://api.github.com/repos/fnoble/piksi_firmware/releases{/id}","created_at":"2011-09-26T23:37:06Z","updated_at":"2015-01-30T01:33:16Z","pushed_at":"2015-01-30T01:33:15Z","git_url":"git://github.com/fnoble/piksi_firmware.git","ssh_url":"git@github.com:fnoble/piksi_firmware.git","clone_url":"https://github.com/fnoble/piksi_firmware.git","svn_url":"https://github.com/fnoble/piksi_firmware","homepage":"swift-nav.com","size":3659,"stargazers_count":7,"watchers_count":7,"language":"C","has_issues":false,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":4,"mirror_url":null,"open_issues_count":0,"forks":4,"open_issues":0,"watchers":7,"default_branch":"master"}},"base":{"label":"swift-nav:master","ref":"master","sha":"4b27f8aab02d70aa44d3336fccc46d1b25f58af3","user":{"login":"swift-nav","id":1069835,"avatar_url":"https://avatars.githubusercontent.com/u/1069835?v=3","gravatar_id":"","url":"https://api.github.com/users/swift-nav","html_url":"https://github.com/swift-nav","followers_url":"https://api.github.com/users/swift-nav/followers","following_url":"https://api.github.com/users/swift-nav/following{/other_user}","gists_url":"https://api.github.com/users/swift-nav/gists{/gist_id}","starred_url":"https://api.github.com/users/swift-nav/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/swift-nav/subscriptions","organizations_url":"https://api.github.com/users/swift-nav/orgs","repos_url":"https://api.github.com/users/swift-nav/repos","events_url":"https://api.github.com/users/swift-nav/events{/privacy}","received_events_url":"https://api.github.com/users/swift-nav/received_events","type":"Organization","site_admin":false},"repo":{"id":2464156,"name":"piksi_firmware","full_name":"swift-nav/piksi_firmware","owner":{"login":"swift-nav","id":1069835,"avatar_url":"https://avatars.githubusercontent.com/u/1069835?v=3","gravatar_id":"","url":"https://api.github.com/users/swift-nav","html_url":"https://github.com/swift-nav","followers_url":"https://api.github.com/users/swift-nav/followers","following_url":"https://api.github.com/users/swift-nav/following{/other_user}","gists_url":"https://api.github.com/users/swift-nav/gists{/gist_id}","starred_url":"https://api.github.com/users/swift-nav/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/swift-nav/subscriptions","organizations_url":"https://api.github.com/users/swift-nav/orgs","repos_url":"https://api.github.com/users/swift-nav/repos","events_url":"https://api.github.com/users/swift-nav/events{/privacy}","received_events_url":"https://api.github.com/users/swift-nav/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/swift-nav/piksi_firmware","description":"Firmware for the Piksi GNSS receiver.","fork":false,"url":"https://api.github.com/repos/swift-nav/piksi_firmware","forks_url":"https://api.github.com/repos/swift-nav/piksi_firmware/forks","keys_url":"https://api.github.com/repos/swift-nav/piksi_firmware/keys{/key_id}","collaborators_url":"https://api.github.com/repos/swift-nav/piksi_firmware/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/swift-nav/piksi_firmware/teams","hooks_url":"https://api.github.com/repos/swift-nav/piksi_firmware/hooks","issue_events_url":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/events{/number}","events_url":"https://api.github.com/repos/swift-nav/piksi_firmware/events","assignees_url":"https://api.github.com/repos/swift-nav/piksi_firmware/assignees{/user}","branches_url":"https://api.github.com/repos/swift-nav/piksi_firmware/branches{/branch}","tags_url":"https://api.github.com/repos/swift-nav/piksi_firmware/tags","blobs_url":"https://api.github.com/repos/swift-nav/piksi_firmware/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/swift-nav/piksi_firmware/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/swift-nav/piksi_firmware/git/refs{/sha}","trees_url":"https://api.github.com/repos/swift-nav/piksi_firmware/git/trees{/sha}","statuses_url":"https://api.github.com/repos/swift-nav/piksi_firmware/statuses/{sha}","languages_url":"https://api.github.com/repos/swift-nav/piksi_firmware/languages","stargazers_url":"https://api.github.com/repos/swift-nav/piksi_firmware/stargazers","contributors_url":"https://api.github.com/repos/swift-nav/piksi_firmware/contributors","subscribers_url":"https://api.github.com/repos/swift-nav/piksi_firmware/subscribers","subscription_url":"https://api.github.com/repos/swift-nav/piksi_firmware/subscription","commits_url":"https://api.github.com/repos/swift-nav/piksi_firmware/commits{/sha}","git_commits_url":"https://api.github.com/repos/swift-nav/piksi_firmware/git/commits{/sha}","comments_url":"https://api.github.com/repos/swift-nav/piksi_firmware/comments{/number}","issue_comment_url":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/comments/{number}","contents_url":"https://api.github.com/repos/swift-nav/piksi_firmware/contents/{+path}","compare_url":"https://api.github.com/repos/swift-nav/piksi_firmware/compare/{base}...{head}","merges_url":"https://api.github.com/repos/swift-nav/piksi_firmware/merges","archive_url":"https://api.github.com/repos/swift-nav/piksi_firmware/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/swift-nav/piksi_firmware/downloads","issues_url":"https://api.github.com/repos/swift-nav/piksi_firmware/issues{/number}","pulls_url":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls{/number}","milestones_url":"https://api.github.com/repos/swift-nav/piksi_firmware/milestones{/number}","notifications_url":"https://api.github.com/repos/swift-nav/piksi_firmware/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/swift-nav/piksi_firmware/labels{/name}","releases_url":"https://api.github.com/repos/swift-nav/piksi_firmware/releases{/id}","created_at":"2011-09-26T23:35:50Z","updated_at":"2015-01-30T01:30:02Z","pushed_at":"2015-01-30T01:30:02Z","git_url":"git://github.com/swift-nav/piksi_firmware.git","ssh_url":"git@github.com:swift-nav/piksi_firmware.git","clone_url":"https://github.com/swift-nav/piksi_firmware.git","svn_url":"https://github.com/swift-nav/piksi_firmware","homepage":"swift-nav.com","size":4032,"stargazers_count":52,"watchers_count":52,"language":"C","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":38,"mirror_url":null,"open_issues_count":65,"forks":38,"open_issues":65,"watchers":52,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240"},"html":{"href":"https://github.com/swift-nav/piksi_firmware/pull/240"},"issue":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/240"},"comments":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/issues/240/comments"},"review_comments":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240/comments"},"review_comment":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/comments/{number}"},"commits":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/pulls/240/commits"},"statuses":{"href":"https://api.github.com/repos/swift-nav/piksi_firmware/statuses/72c17bb5e96decfc704d9f5db878ff32fec662a3"}}}}