GnssDebug.cpp 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299
  1. /*
  2. * Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
  3. * Not a Contribution
  4. */
  5. /*
  6. * Copyright (C) 2016 The Android Open Source Project
  7. *
  8. * Licensed under the Apache License, Version 2.0 (the "License");
  9. * you may not use this file except in compliance with the License.
  10. * You may obtain a copy of the License at
  11. *
  12. * http://www.apache.org/licenses/LICENSE-2.0
  13. *
  14. * Unless required by applicable law or agreed to in writing, software
  15. * distributed under the License is distributed on an "AS IS" BASIS,
  16. * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  17. * See the License for the specific language governing permissions and
  18. * limitations under the License.
  19. */
  20. #define LOG_TAG "LocSvc_GnssDebugInterface"
  21. #include <log/log.h>
  22. #include <log_util.h>
  23. #include "Gnss.h"
  24. #include "GnssDebug.h"
  25. #include "LocationUtil.h"
  26. namespace android {
  27. namespace hardware {
  28. namespace gnss {
  29. namespace V2_0 {
  30. namespace implementation {
  31. using ::android::hardware::hidl_vec;
  32. using ::android::hardware::gnss::V2_0::IGnssDebug;
  33. #define GNSS_DEBUG_UNKNOWN_HORIZONTAL_ACCURACY_METERS (20000000)
  34. #define GNSS_DEBUG_UNKNOWN_VERTICAL_ACCURACY_METERS (20000)
  35. #define GNSS_DEBUG_UNKNOWN_SPEED_ACCURACY_PER_SEC (500)
  36. #define GNSS_DEBUG_UNKNOWN_BEARING_ACCURACY_DEG (180)
  37. #define GNSS_DEBUG_UNKNOWN_UTC_TIME (1483228800000LL) // 1/1/2017 00:00 GMT
  38. #define GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MIN (999) // 999 ns
  39. #define GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MAX (1.57783680E17) // 5 years in ns
  40. #define GNSS_DEBUG_UNKNOWN_FREQ_UNC_NS_PER_SEC (2.0e5) // ppm
  41. GnssDebug::GnssDebug(Gnss* gnss) : mGnss(gnss)
  42. {
  43. }
  44. /*
  45. * This methods requests position, time and satellite ephemeris debug information
  46. * from the HAL.
  47. *
  48. * @return void
  49. */
  50. Return<void> GnssDebug::getDebugData(getDebugData_cb _hidl_cb)
  51. {
  52. LOC_LOGD("%s]: ", __func__);
  53. V1_0::IGnssDebug::DebugData data = { };
  54. if((nullptr == mGnss) || (nullptr == mGnss->getGnssInterface())){
  55. LOC_LOGE("GnssDebug - Null GNSS interface");
  56. _hidl_cb(data);
  57. return Void();
  58. }
  59. // get debug report snapshot via hal interface
  60. GnssDebugReport reports = { };
  61. mGnss->getGnssInterface()->getDebugReport(reports);
  62. // location block
  63. if (reports.mLocation.mValid) {
  64. data.position.valid = true;
  65. data.position.latitudeDegrees = reports.mLocation.mLocation.latitude;
  66. data.position.longitudeDegrees = reports.mLocation.mLocation.longitude;
  67. data.position.altitudeMeters = reports.mLocation.mLocation.altitude;
  68. data.position.speedMetersPerSec =
  69. (double)(reports.mLocation.mLocation.speed);
  70. data.position.bearingDegrees =
  71. (double)(reports.mLocation.mLocation.bearing);
  72. data.position.horizontalAccuracyMeters =
  73. (double)(reports.mLocation.mLocation.accuracy);
  74. data.position.verticalAccuracyMeters =
  75. reports.mLocation.verticalAccuracyMeters;
  76. data.position.speedAccuracyMetersPerSecond =
  77. reports.mLocation.speedAccuracyMetersPerSecond;
  78. data.position.bearingAccuracyDegrees =
  79. reports.mLocation.bearingAccuracyDegrees;
  80. timeval tv_now, tv_report;
  81. tv_report.tv_sec = reports.mLocation.mUtcReported.tv_sec;
  82. tv_report.tv_usec = reports.mLocation.mUtcReported.tv_nsec / 1000ULL;
  83. gettimeofday(&tv_now, NULL);
  84. data.position.ageSeconds =
  85. (tv_now.tv_sec - tv_report.tv_sec) +
  86. (float)((tv_now.tv_usec - tv_report.tv_usec)) / 1000000;
  87. }
  88. else {
  89. data.position.valid = false;
  90. }
  91. if (data.position.horizontalAccuracyMeters <= 0 ||
  92. data.position.horizontalAccuracyMeters > GNSS_DEBUG_UNKNOWN_HORIZONTAL_ACCURACY_METERS) {
  93. data.position.horizontalAccuracyMeters = GNSS_DEBUG_UNKNOWN_HORIZONTAL_ACCURACY_METERS;
  94. }
  95. if (data.position.verticalAccuracyMeters <= 0 ||
  96. data.position.verticalAccuracyMeters > GNSS_DEBUG_UNKNOWN_VERTICAL_ACCURACY_METERS) {
  97. data.position.verticalAccuracyMeters = GNSS_DEBUG_UNKNOWN_VERTICAL_ACCURACY_METERS;
  98. }
  99. if (data.position.speedAccuracyMetersPerSecond <= 0 ||
  100. data.position.speedAccuracyMetersPerSecond > GNSS_DEBUG_UNKNOWN_SPEED_ACCURACY_PER_SEC) {
  101. data.position.speedAccuracyMetersPerSecond = GNSS_DEBUG_UNKNOWN_SPEED_ACCURACY_PER_SEC;
  102. }
  103. if (data.position.bearingAccuracyDegrees <= 0 ||
  104. data.position.bearingAccuracyDegrees > GNSS_DEBUG_UNKNOWN_BEARING_ACCURACY_DEG) {
  105. data.position.bearingAccuracyDegrees = GNSS_DEBUG_UNKNOWN_BEARING_ACCURACY_DEG;
  106. }
  107. // time block
  108. if (reports.mTime.mValid) {
  109. data.time.timeEstimate = reports.mTime.timeEstimate;
  110. data.time.timeUncertaintyNs = reports.mTime.timeUncertaintyNs;
  111. data.time.frequencyUncertaintyNsPerSec =
  112. reports.mTime.frequencyUncertaintyNsPerSec;
  113. }
  114. if (data.time.timeEstimate < GNSS_DEBUG_UNKNOWN_UTC_TIME) {
  115. data.time.timeEstimate = GNSS_DEBUG_UNKNOWN_UTC_TIME;
  116. }
  117. if (data.time.timeUncertaintyNs <= 0) {
  118. data.time.timeUncertaintyNs = (float)GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MIN;
  119. } else if (data.time.timeUncertaintyNs > GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MAX) {
  120. data.time.timeUncertaintyNs = (float)GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MAX;
  121. }
  122. if (data.time.frequencyUncertaintyNsPerSec <= 0 ||
  123. data.time.frequencyUncertaintyNsPerSec > (float)GNSS_DEBUG_UNKNOWN_FREQ_UNC_NS_PER_SEC) {
  124. data.time.frequencyUncertaintyNsPerSec = (float)GNSS_DEBUG_UNKNOWN_FREQ_UNC_NS_PER_SEC;
  125. }
  126. // satellite data block
  127. V1_0::IGnssDebug::SatelliteData s = { };
  128. std::vector<V1_0::IGnssDebug::SatelliteData> s_array;
  129. for (uint32_t i=0; i<reports.mSatelliteInfo.size(); i++) {
  130. memset(&s, 0, sizeof(s));
  131. s.svid = reports.mSatelliteInfo[i].svid;
  132. convertGnssConstellationType(
  133. reports.mSatelliteInfo[i].constellation, s.constellation);
  134. convertGnssEphemerisType(
  135. reports.mSatelliteInfo[i].mEphemerisType, s.ephemerisType);
  136. convertGnssEphemerisSource(
  137. reports.mSatelliteInfo[i].mEphemerisSource, s.ephemerisSource);
  138. convertGnssEphemerisHealth(
  139. reports.mSatelliteInfo[i].mEphemerisHealth, s.ephemerisHealth);
  140. s.ephemerisAgeSeconds =
  141. reports.mSatelliteInfo[i].ephemerisAgeSeconds;
  142. s.serverPredictionIsAvailable =
  143. reports.mSatelliteInfo[i].serverPredictionIsAvailable;
  144. s.serverPredictionAgeSeconds =
  145. reports.mSatelliteInfo[i].serverPredictionAgeSeconds;
  146. s_array.push_back(s);
  147. }
  148. data.satelliteDataArray = s_array;
  149. // callback HIDL with collected debug data
  150. _hidl_cb(data);
  151. return Void();
  152. }
  153. Return<void> GnssDebug::getDebugData_2_0(getDebugData_2_0_cb _hidl_cb)
  154. {
  155. LOC_LOGD("%s]: ", __func__);
  156. V2_0::IGnssDebug::DebugData data = { };
  157. if((nullptr == mGnss) || (nullptr == mGnss->getGnssInterface())){
  158. LOC_LOGE("GnssDebug - Null GNSS interface");
  159. _hidl_cb(data);
  160. return Void();
  161. }
  162. // get debug report snapshot via hal interface
  163. GnssDebugReport reports = { };
  164. mGnss->getGnssInterface()->getDebugReport(reports);
  165. // location block
  166. if (reports.mLocation.mValid) {
  167. data.position.valid = true;
  168. data.position.latitudeDegrees = reports.mLocation.mLocation.latitude;
  169. data.position.longitudeDegrees = reports.mLocation.mLocation.longitude;
  170. data.position.altitudeMeters = reports.mLocation.mLocation.altitude;
  171. data.position.speedMetersPerSec =
  172. (double)(reports.mLocation.mLocation.speed);
  173. data.position.bearingDegrees =
  174. (double)(reports.mLocation.mLocation.bearing);
  175. data.position.horizontalAccuracyMeters =
  176. (double)(reports.mLocation.mLocation.accuracy);
  177. data.position.verticalAccuracyMeters =
  178. reports.mLocation.verticalAccuracyMeters;
  179. data.position.speedAccuracyMetersPerSecond =
  180. reports.mLocation.speedAccuracyMetersPerSecond;
  181. data.position.bearingAccuracyDegrees =
  182. reports.mLocation.bearingAccuracyDegrees;
  183. timeval tv_now, tv_report;
  184. tv_report.tv_sec = reports.mLocation.mUtcReported.tv_sec;
  185. tv_report.tv_usec = reports.mLocation.mUtcReported.tv_nsec / 1000ULL;
  186. gettimeofday(&tv_now, NULL);
  187. data.position.ageSeconds =
  188. (tv_now.tv_sec - tv_report.tv_sec) +
  189. (float)((tv_now.tv_usec - tv_report.tv_usec)) / 1000000;
  190. }
  191. else {
  192. data.position.valid = false;
  193. }
  194. if (data.position.horizontalAccuracyMeters <= 0 ||
  195. data.position.horizontalAccuracyMeters > GNSS_DEBUG_UNKNOWN_HORIZONTAL_ACCURACY_METERS) {
  196. data.position.horizontalAccuracyMeters = GNSS_DEBUG_UNKNOWN_HORIZONTAL_ACCURACY_METERS;
  197. }
  198. if (data.position.verticalAccuracyMeters <= 0 ||
  199. data.position.verticalAccuracyMeters > GNSS_DEBUG_UNKNOWN_VERTICAL_ACCURACY_METERS) {
  200. data.position.verticalAccuracyMeters = GNSS_DEBUG_UNKNOWN_VERTICAL_ACCURACY_METERS;
  201. }
  202. if (data.position.speedAccuracyMetersPerSecond <= 0 ||
  203. data.position.speedAccuracyMetersPerSecond > GNSS_DEBUG_UNKNOWN_SPEED_ACCURACY_PER_SEC) {
  204. data.position.speedAccuracyMetersPerSecond = GNSS_DEBUG_UNKNOWN_SPEED_ACCURACY_PER_SEC;
  205. }
  206. if (data.position.bearingAccuracyDegrees <= 0 ||
  207. data.position.bearingAccuracyDegrees > GNSS_DEBUG_UNKNOWN_BEARING_ACCURACY_DEG) {
  208. data.position.bearingAccuracyDegrees = GNSS_DEBUG_UNKNOWN_BEARING_ACCURACY_DEG;
  209. }
  210. // time block
  211. if (reports.mTime.mValid) {
  212. data.time.timeEstimate = reports.mTime.timeEstimate;
  213. data.time.timeUncertaintyNs = reports.mTime.timeUncertaintyNs;
  214. data.time.frequencyUncertaintyNsPerSec =
  215. reports.mTime.frequencyUncertaintyNsPerSec;
  216. }
  217. if (data.time.timeEstimate < GNSS_DEBUG_UNKNOWN_UTC_TIME) {
  218. data.time.timeEstimate = GNSS_DEBUG_UNKNOWN_UTC_TIME;
  219. }
  220. if (data.time.timeUncertaintyNs <= 0) {
  221. data.time.timeUncertaintyNs = (float)GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MIN;
  222. }
  223. else if (data.time.timeUncertaintyNs > GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MAX) {
  224. data.time.timeUncertaintyNs = (float)GNSS_DEBUG_UNKNOWN_UTC_TIME_UNC_MAX;
  225. }
  226. if (data.time.frequencyUncertaintyNsPerSec <= 0 ||
  227. data.time.frequencyUncertaintyNsPerSec > (float)GNSS_DEBUG_UNKNOWN_FREQ_UNC_NS_PER_SEC) {
  228. data.time.frequencyUncertaintyNsPerSec = (float)GNSS_DEBUG_UNKNOWN_FREQ_UNC_NS_PER_SEC;
  229. }
  230. // satellite data block
  231. V2_0::IGnssDebug::SatelliteData s = { };
  232. std::vector<V2_0::IGnssDebug::SatelliteData> s_array;
  233. for (uint32_t i=0; i<reports.mSatelliteInfo.size(); i++) {
  234. memset(&s, 0, sizeof(s));
  235. s.v1_0.svid = reports.mSatelliteInfo[i].svid;
  236. convertGnssConstellationType(
  237. reports.mSatelliteInfo[i].constellation, s.constellation);
  238. convertGnssEphemerisType(
  239. reports.mSatelliteInfo[i].mEphemerisType, s.v1_0.ephemerisType);
  240. convertGnssEphemerisSource(
  241. reports.mSatelliteInfo[i].mEphemerisSource, s.v1_0.ephemerisSource);
  242. convertGnssEphemerisHealth(
  243. reports.mSatelliteInfo[i].mEphemerisHealth, s.v1_0.ephemerisHealth);
  244. s.v1_0.ephemerisAgeSeconds =
  245. reports.mSatelliteInfo[i].ephemerisAgeSeconds;
  246. s.v1_0.serverPredictionIsAvailable =
  247. reports.mSatelliteInfo[i].serverPredictionIsAvailable;
  248. s.v1_0.serverPredictionAgeSeconds =
  249. reports.mSatelliteInfo[i].serverPredictionAgeSeconds;
  250. s_array.push_back(s);
  251. }
  252. data.satelliteDataArray = s_array;
  253. // callback HIDL with collected debug data
  254. _hidl_cb(data);
  255. return Void();
  256. }
  257. } // namespace implementation
  258. } // namespace V2_0
  259. } // namespace gnss
  260. } // namespace hardware
  261. } // namespace android