loc_nmea.cpp 90 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404
  1. /* Copyright (c) 2012-2021, The Linux Foundation. All rights reserved.
  2. *
  3. * Redistribution and use in source and binary forms, with or without
  4. * modification, are permitted provided that the following conditions are
  5. * met:
  6. * * Redistributions of source code must retain the above copyright
  7. * notice, this list of conditions and the following disclaimer.
  8. * * Redistributions in binary form must reproduce the above
  9. * copyright notice, this list of conditions and the following
  10. * disclaimer in the documentation and/or other materials provided
  11. * with the distribution.
  12. * * Neither the name of The Linux Foundation nor the names of its
  13. * contributors may be used to endorse or promote products derived
  14. * from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
  17. * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  18. * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
  19. * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
  20. * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  21. * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  22. * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
  23. * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  24. * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  25. * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
  26. * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  27. *
  28. */
  29. #define LOG_NDEBUG 0
  30. #define LOG_TAG "LocSvc_nmea"
  31. #include <loc_nmea.h>
  32. #include <math.h>
  33. #include <log_util.h>
  34. #include <loc_pla.h>
  35. #include <loc_cfg.h>
  36. #define GLONASS_SV_ID_OFFSET 64
  37. #define SBAS_SV_ID_OFFSET (87)
  38. #define QZSS_SV_ID_OFFSET (192)
  39. #define BDS_SV_ID_OFFSET (200)
  40. #define GALILEO_SV_ID_OFFSET (300)
  41. #define NAVIC_SV_ID_OFFSET (400)
  42. #define MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION 64
  43. #define MAX_SATELLITES_IN_USE 12
  44. #define MSEC_IN_ONE_WEEK 604800000ULL
  45. #define UTC_GPS_OFFSET_MSECS 315964800000ULL
  46. #define MAX_TAG_BLOCK_GROUP_CODE (99999)
  47. // GNSS system id according to NMEA spec
  48. #define SYSTEM_ID_GPS 1
  49. #define SYSTEM_ID_GLONASS 2
  50. #define SYSTEM_ID_GALILEO 3
  51. #define SYSTEM_ID_BDS 4
  52. #define SYSTEM_ID_QZSS 5
  53. #define SYSTEM_ID_NAVIC 6
  54. //GNSS signal id according to NMEA spec
  55. #define SIGNAL_ID_ALL_SIGNALS 0
  56. #define SIGNAL_ID_GPS_L1CA 1
  57. #define SIGNAL_ID_GPS_L1P 2
  58. #define SIGNAL_ID_GPS_L1M 3
  59. #define SIGNAL_ID_GPS_L2P 4
  60. #define SIGNAL_ID_GPS_L2CM 5
  61. #define SIGNAL_ID_GPS_L2CL 6
  62. #define SIGNAL_ID_GPS_L5I 7
  63. #define SIGNAL_ID_GPS_L5Q 8
  64. #define SIGNAL_ID_GLO_G1CA 1
  65. #define SIGNAL_ID_GLO_G1P 2
  66. #define SIGNAL_ID_GLO_G2CA 3
  67. #define SIGNAL_ID_GLO_G2P 4
  68. #define SIGNAL_ID_GAL_E5A 1
  69. #define SIGNAL_ID_GAL_E5B 2
  70. #define SIGNAL_ID_GAL_E5AB 3
  71. #define SIGNAL_ID_GAL_E6A 4
  72. #define SIGNAL_ID_GAL_E6BC 5
  73. #define SIGNAL_ID_GAL_L1A 6
  74. #define SIGNAL_ID_GAL_L1BC 7
  75. #define SIGNAL_ID_BDS_B1I 1
  76. #define SIGNAL_ID_BDS_B1Q 2
  77. #define SIGNAL_ID_BDS_B1C 3
  78. #define SIGNAL_ID_BDS_B1A 4
  79. #define SIGNAL_ID_BDS_B2A 5
  80. #define SIGNAL_ID_BDS_B2B 6
  81. #define SIGNAL_ID_BDS_B2AB 7
  82. #define SIGNAL_ID_BDS_B3I 8
  83. #define SIGNAL_ID_BDS_B3Q 9
  84. #define SIGNAL_ID_BDS_B3A 0xA
  85. #define SIGNAL_ID_BDS_B2I 0xB
  86. #define SIGNAL_ID_BDS_B2Q 0xC
  87. #define SIGNAL_ID_QZSS_L1CA 1
  88. #define SIGNAL_ID_QZSS_L1CD 2
  89. #define SIGNAL_ID_QZSS_L1CP 3
  90. #define SIGNAL_ID_QZSS_LIS 4
  91. #define SIGNAL_ID_QZSS_L2CM 5
  92. #define SIGNAL_ID_QZSS_L2CL 6
  93. #define SIGNAL_ID_QZSS_L5I 7
  94. #define SIGNAL_ID_QZSS_L5Q 8
  95. #define SIGNAL_ID_QZSS_L6D 9
  96. #define SIGNAL_ID_QZSS_L6E 0xA
  97. #define SIGNAL_ID_NAVIC_L5SPS 1
  98. #define SIGNAL_ID_NAVIC_SSPS 2
  99. #define SIGNAL_ID_NAVIC_L5RS 3
  100. #define SIGNAL_ID_NAVIC_SRS 4
  101. #define SIGNAL_ID_NAVIC_L1SPS 5
  102. typedef struct loc_nmea_sv_meta_s
  103. {
  104. char talker[3];
  105. uint32_t svTypeMask;
  106. uint64_t mask;
  107. uint32_t svCount;
  108. uint32_t totalSvUsedCount;
  109. uint32_t svIdOffset;
  110. uint32_t signalId;
  111. uint32_t systemId;
  112. } loc_nmea_sv_meta;
  113. typedef struct loc_sv_cache_info_s
  114. {
  115. uint64_t gps_used_mask;
  116. uint64_t glo_used_mask;
  117. uint64_t gal_used_mask;
  118. uint64_t qzss_used_mask;
  119. uint64_t bds_used_mask;
  120. uint64_t navic_used_mask;
  121. uint32_t gps_l1_count;
  122. uint32_t gps_l2_count;
  123. uint32_t gps_l5_count;
  124. uint32_t glo_g1_count;
  125. uint32_t glo_g2_count;
  126. uint32_t gal_e1_count;
  127. uint32_t gal_e5_count;
  128. uint32_t gal_e5b_count;
  129. uint32_t qzss_l1_count;
  130. uint32_t qzss_l2_count;
  131. uint32_t qzss_l5_count;
  132. uint32_t bds_b1i_count;
  133. uint32_t bds_b1c_count;
  134. uint32_t bds_b2_count;
  135. uint32_t navic_l5_count;
  136. float hdop;
  137. float pdop;
  138. float vdop;
  139. } loc_sv_cache_info;
  140. static GnssNmeaTypesMask mEnabledNmeaTypes = NMEA_TYPE_ALL;
  141. /*===========================================================================
  142. FUNCTION convert_Lla_to_Ecef
  143. DESCRIPTION
  144. Convert LLA to ECEF
  145. DEPENDENCIES
  146. NONE
  147. RETURN VALUE
  148. NONE
  149. SIDE EFFECTS
  150. N/A
  151. ===========================================================================*/
  152. static void convert_Lla_to_Ecef(const LocLla& plla, LocEcef& pecef)
  153. {
  154. double r;
  155. r = MAJA / sqrt(1.0 - ESQR * sin(plla.lat) * sin(plla.lat));
  156. pecef.X = (r + plla.alt) * cos(plla.lat) * cos(plla.lon);
  157. pecef.Y = (r + plla.alt) * cos(plla.lat) * sin(plla.lon);
  158. pecef.Z = (r * OMES + plla.alt) * sin(plla.lat);
  159. }
  160. /*===========================================================================
  161. FUNCTION convert_WGS84_to_PZ90
  162. DESCRIPTION
  163. Convert datum from WGS84 to PZ90
  164. DEPENDENCIES
  165. NONE
  166. RETURN VALUE
  167. NONE
  168. SIDE EFFECTS
  169. N/A
  170. ===========================================================================*/
  171. static void convert_WGS84_to_PZ90(const LocEcef& pWGS84, LocEcef& pPZ90)
  172. {
  173. double deltaX = DatumConstFromWGS84[0];
  174. double deltaY = DatumConstFromWGS84[1];
  175. double deltaZ = DatumConstFromWGS84[2];
  176. double deltaScale = DatumConstFromWGS84[3];
  177. double rotX = DatumConstFromWGS84[4];
  178. double rotY = DatumConstFromWGS84[5];
  179. double rotZ = DatumConstFromWGS84[6];
  180. pPZ90.X = deltaX + deltaScale * (pWGS84.X + rotZ * pWGS84.Y - rotY * pWGS84.Z);
  181. pPZ90.Y = deltaY + deltaScale * (pWGS84.Y - rotZ * pWGS84.X + rotX * pWGS84.Z);
  182. pPZ90.Z = deltaZ + deltaScale * (pWGS84.Z + rotY * pWGS84.X - rotX * pWGS84.Y);
  183. }
  184. /*===========================================================================
  185. FUNCTION convert_Ecef_to_Lla
  186. DESCRIPTION
  187. Convert ECEF to LLA
  188. DEPENDENCIES
  189. NONE
  190. RETURN VALUE
  191. NONE
  192. SIDE EFFECTS
  193. N/A
  194. ===========================================================================*/
  195. static void convert_Ecef_to_Lla(const LocEcef& pecef, LocLla& plla)
  196. {
  197. double p, r;
  198. double EcefA = C_PZ90A;
  199. double EcefB = C_PZ90B;
  200. double Ecef1Mf;
  201. double EcefE2;
  202. double Mu;
  203. double Smu;
  204. double Cmu;
  205. double Phi;
  206. double Sphi;
  207. double N;
  208. p = sqrt(pecef.X * pecef.X + pecef.Y * pecef.Y);
  209. r = sqrt(p * p + pecef.Z * pecef.Z);
  210. if (r < 1.0) {
  211. plla.lat = 1.0;
  212. plla.lon = 1.0;
  213. plla.alt = 1.0;
  214. }
  215. Ecef1Mf = 1.0 - (EcefA - EcefB) / EcefA;
  216. EcefE2 = 1.0 - (EcefB * EcefB) / (EcefA * EcefA);
  217. if (p > 1.0) {
  218. Mu = atan2(pecef.Z * (Ecef1Mf + EcefE2 * EcefA / r), p);
  219. } else {
  220. if (pecef.Z > 0.0) {
  221. Mu = M_PI / 2.0;
  222. } else {
  223. Mu = -M_PI / 2.0;
  224. }
  225. }
  226. Smu = sin(Mu);
  227. Cmu = cos(Mu);
  228. Phi = atan2(pecef.Z * Ecef1Mf + EcefE2 * EcefA * Smu * Smu * Smu,
  229. Ecef1Mf * (p - EcefE2 * EcefA * Cmu * Cmu * Cmu));
  230. Sphi = sin(Phi);
  231. N = EcefA / sqrt(1.0 - EcefE2 * Sphi * Sphi);
  232. plla.alt = p * cos(Phi) + pecef.Z * Sphi - EcefA * EcefA/N;
  233. plla.lat = Phi;
  234. if ( p > 1.0) {
  235. plla.lon = atan2(pecef.Y, pecef.X);
  236. } else {
  237. plla.lon = 0.0;
  238. }
  239. }
  240. /*===========================================================================
  241. FUNCTION convert_signalType_to_signalId
  242. DESCRIPTION
  243. convert signalType to signal ID
  244. DEPENDENCIES
  245. NONE
  246. RETURN VALUE
  247. value of signal ID
  248. SIDE EFFECTS
  249. N/A
  250. ===========================================================================*/
  251. static uint32_t convert_signalType_to_signalId(GnssSignalTypeMask signalType)
  252. {
  253. uint32_t signalId = SIGNAL_ID_ALL_SIGNALS;
  254. switch (signalType) {
  255. case GNSS_SIGNAL_GPS_L1CA:
  256. case GNSS_SIGNAL_SBAS_L1:
  257. signalId = SIGNAL_ID_GPS_L1CA;
  258. break;
  259. case GNSS_SIGNAL_GPS_L2:
  260. signalId = SIGNAL_ID_GPS_L2CL;
  261. break;
  262. case GNSS_SIGNAL_GPS_L5:
  263. signalId = SIGNAL_ID_GPS_L5Q;
  264. break;
  265. case GNSS_SIGNAL_GLONASS_G1:
  266. signalId = SIGNAL_ID_GLO_G1CA;
  267. break;
  268. case GNSS_SIGNAL_GLONASS_G2:
  269. signalId = SIGNAL_ID_GLO_G2CA;
  270. break;
  271. case GNSS_SIGNAL_GALILEO_E1:
  272. signalId = SIGNAL_ID_GAL_L1BC;
  273. break;
  274. case GNSS_SIGNAL_GALILEO_E5A:
  275. signalId = SIGNAL_ID_GAL_E5A;
  276. break;
  277. case GNSS_SIGNAL_GALILEO_E5B:
  278. signalId = SIGNAL_ID_GAL_E5B;
  279. break;
  280. case GNSS_SIGNAL_QZSS_L1CA:
  281. signalId = SIGNAL_ID_QZSS_L1CA;
  282. break;
  283. case GNSS_SIGNAL_QZSS_L2:
  284. signalId = SIGNAL_ID_QZSS_L2CL;
  285. break;
  286. case GNSS_SIGNAL_QZSS_L5:
  287. signalId = SIGNAL_ID_QZSS_L5Q;
  288. break;
  289. case GNSS_SIGNAL_BEIDOU_B1I:
  290. signalId = SIGNAL_ID_BDS_B1I;
  291. break;
  292. case GNSS_SIGNAL_BEIDOU_B1C:
  293. signalId = SIGNAL_ID_BDS_B1C;
  294. break;
  295. case GNSS_SIGNAL_BEIDOU_B2I:
  296. signalId = SIGNAL_ID_BDS_B2I;
  297. break;
  298. case GNSS_SIGNAL_BEIDOU_B2AI:
  299. case GNSS_SIGNAL_BEIDOU_B2AQ:
  300. signalId = SIGNAL_ID_BDS_B2A;
  301. break;
  302. case GNSS_SIGNAL_NAVIC_L5:
  303. signalId = SIGNAL_ID_NAVIC_L5SPS;
  304. break;
  305. default:
  306. signalId = SIGNAL_ID_ALL_SIGNALS;
  307. }
  308. return signalId;
  309. }
  310. /*===========================================================================
  311. FUNCTION get_sv_count_from_mask
  312. DESCRIPTION
  313. get the sv count from bit mask
  314. DEPENDENCIES
  315. NONE
  316. RETURN VALUE
  317. value of sv count
  318. SIDE EFFECTS
  319. N/A
  320. ===========================================================================*/
  321. static uint32_t get_sv_count_from_mask(uint64_t svMask, int totalSvCount)
  322. {
  323. int index = 0;
  324. uint32_t svCount = 0;
  325. if(totalSvCount > MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION) {
  326. LOC_LOGE("total SV count in this constellation %d exceeded limit %d",
  327. totalSvCount, MAX_SV_COUNT_SUPPORTED_IN_ONE_CONSTELLATION);
  328. }
  329. for(index = 0; index < totalSvCount; index++) {
  330. if(svMask & 0x1)
  331. svCount += 1;
  332. svMask >>= 1;
  333. }
  334. return svCount;
  335. }
  336. /*===========================================================================
  337. FUNCTION loc_nmea_sv_meta_init
  338. DESCRIPTION
  339. Init loc_nmea_sv_meta passed in
  340. DEPENDENCIES
  341. NONE
  342. RETURN VALUE
  343. Pointer to loc_nmea_sv_meta
  344. SIDE EFFECTS
  345. N/A
  346. ===========================================================================*/
  347. static loc_nmea_sv_meta* loc_nmea_sv_meta_init(loc_nmea_sv_meta& sv_meta,
  348. loc_sv_cache_info& sv_cache_info,
  349. GnssSvType svType,
  350. GnssSignalTypeMask signalType,
  351. bool needCombine)
  352. {
  353. memset(&sv_meta, 0, sizeof(sv_meta));
  354. sv_meta.svTypeMask = (1 << svType);
  355. switch (svType)
  356. {
  357. case GNSS_SV_TYPE_GPS:
  358. sv_meta.talker[0] = 'G';
  359. sv_meta.talker[1] = 'P';
  360. sv_meta.mask = sv_cache_info.gps_used_mask;
  361. sv_meta.systemId = SYSTEM_ID_GPS;
  362. sv_meta.svTypeMask |= (1 << GNSS_SV_TYPE_SBAS);
  363. switch (signalType) {
  364. case GNSS_SIGNAL_GPS_L1CA:
  365. sv_meta.svCount = sv_cache_info.gps_l1_count;
  366. break;
  367. case GNSS_SIGNAL_GPS_L5:
  368. sv_meta.svCount = sv_cache_info.gps_l5_count;
  369. break;
  370. case GNSS_SIGNAL_GPS_L2:
  371. sv_meta.svCount = sv_cache_info.gps_l2_count;
  372. break;
  373. }
  374. break;
  375. case GNSS_SV_TYPE_GLONASS:
  376. sv_meta.talker[0] = 'G';
  377. sv_meta.talker[1] = 'L';
  378. sv_meta.mask = sv_cache_info.glo_used_mask;
  379. // GLONASS SV ids are from 65-96
  380. sv_meta.svIdOffset = GLONASS_SV_ID_OFFSET;
  381. sv_meta.systemId = SYSTEM_ID_GLONASS;
  382. switch (signalType) {
  383. case GNSS_SIGNAL_GLONASS_G1:
  384. sv_meta.svCount = sv_cache_info.glo_g1_count;
  385. break;
  386. case GNSS_SIGNAL_GLONASS_G2:
  387. sv_meta.svCount = sv_cache_info.glo_g2_count;
  388. break;
  389. }
  390. break;
  391. case GNSS_SV_TYPE_GALILEO:
  392. sv_meta.talker[0] = 'G';
  393. sv_meta.talker[1] = 'A';
  394. sv_meta.mask = sv_cache_info.gal_used_mask;
  395. // GALILEO SV ids are from 301-336, So keep svIdOffset 300
  396. sv_meta.svIdOffset = GALILEO_SV_ID_OFFSET;
  397. sv_meta.systemId = SYSTEM_ID_GALILEO;
  398. switch (signalType) {
  399. case GNSS_SIGNAL_GALILEO_E1:
  400. sv_meta.svCount = sv_cache_info.gal_e1_count;
  401. break;
  402. case GNSS_SIGNAL_GALILEO_E5A:
  403. sv_meta.svCount = sv_cache_info.gal_e5_count;
  404. break;
  405. case GNSS_SIGNAL_GALILEO_E5B:
  406. sv_meta.svCount = sv_cache_info.gal_e5b_count;
  407. break;
  408. }
  409. break;
  410. case GNSS_SV_TYPE_QZSS:
  411. sv_meta.talker[0] = 'G';
  412. sv_meta.talker[1] = 'Q';
  413. sv_meta.mask = sv_cache_info.qzss_used_mask;
  414. // QZSS SV ids are from 193-199. So keep svIdOffset 192
  415. sv_meta.svIdOffset = QZSS_SV_ID_OFFSET;
  416. sv_meta.systemId = SYSTEM_ID_QZSS;
  417. switch (signalType) {
  418. case GNSS_SIGNAL_QZSS_L1CA:
  419. sv_meta.svCount = sv_cache_info.qzss_l1_count;
  420. break;
  421. case GNSS_SIGNAL_QZSS_L2:
  422. sv_meta.svCount = sv_cache_info.qzss_l2_count;
  423. break;
  424. case GNSS_SIGNAL_QZSS_L5:
  425. sv_meta.svCount = sv_cache_info.qzss_l5_count;
  426. break;
  427. }
  428. break;
  429. case GNSS_SV_TYPE_BEIDOU:
  430. sv_meta.talker[0] = 'G';
  431. sv_meta.talker[1] = 'B';
  432. sv_meta.mask = sv_cache_info.bds_used_mask;
  433. // BDS SV ids are from 201-237. So keep svIdOffset 200
  434. sv_meta.svIdOffset = BDS_SV_ID_OFFSET;
  435. sv_meta.systemId = SYSTEM_ID_BDS;
  436. switch (signalType) {
  437. case GNSS_SIGNAL_BEIDOU_B1I:
  438. sv_meta.svCount = sv_cache_info.bds_b1i_count;
  439. break;
  440. case GNSS_SIGNAL_BEIDOU_B1C:
  441. sv_meta.svCount = sv_cache_info.bds_b1c_count;
  442. break;
  443. case GNSS_SIGNAL_BEIDOU_B2AI:
  444. sv_meta.svCount = sv_cache_info.bds_b2_count;
  445. break;
  446. }
  447. break;
  448. case GNSS_SV_TYPE_NAVIC:
  449. sv_meta.talker[0] = 'G';
  450. sv_meta.talker[1] = 'I';
  451. sv_meta.mask = sv_cache_info.navic_used_mask;
  452. // NAVIC SV ids are from 401-414. So keep svIdOffset 400
  453. sv_meta.svIdOffset = NAVIC_SV_ID_OFFSET;
  454. sv_meta.systemId = SYSTEM_ID_NAVIC;
  455. switch (signalType) {
  456. case GNSS_SIGNAL_NAVIC_L5:
  457. sv_meta.svCount = sv_cache_info.navic_l5_count;
  458. break;
  459. }
  460. break;
  461. default:
  462. LOC_LOGE("NMEA Error unknow constellation type: %d", svType);
  463. return NULL;
  464. }
  465. sv_meta.signalId = convert_signalType_to_signalId(signalType);
  466. sv_meta.totalSvUsedCount =
  467. get_sv_count_from_mask(sv_cache_info.gps_used_mask,
  468. GPS_SV_PRN_MAX - GPS_SV_PRN_MIN + 1) +
  469. get_sv_count_from_mask(sv_cache_info.glo_used_mask,
  470. GLO_SV_PRN_MAX - GLO_SV_PRN_MIN + 1) +
  471. get_sv_count_from_mask(sv_cache_info.gal_used_mask,
  472. GAL_SV_PRN_MAX - GAL_SV_PRN_MIN + 1) +
  473. get_sv_count_from_mask(sv_cache_info.qzss_used_mask,
  474. QZSS_SV_PRN_MAX - QZSS_SV_PRN_MIN + 1) +
  475. get_sv_count_from_mask(sv_cache_info.bds_used_mask,
  476. BDS_SV_PRN_MAX - BDS_SV_PRN_MIN + 1) +
  477. get_sv_count_from_mask(sv_cache_info.navic_used_mask,
  478. NAVIC_SV_PRN_MAX - NAVIC_SV_PRN_MIN + 1);
  479. if (needCombine &&
  480. (sv_cache_info.gps_used_mask ? 1 : 0) +
  481. (sv_cache_info.glo_used_mask ? 1 : 0) +
  482. (sv_cache_info.gal_used_mask ? 1 : 0) +
  483. (sv_cache_info.qzss_used_mask ? 1 : 0) +
  484. (sv_cache_info.bds_used_mask ? 1 : 0) +
  485. (sv_cache_info.navic_used_mask ? 1 : 0) > 1)
  486. {
  487. // If GPS, GLONASS, Galileo, QZSS, BDS etc. are combined
  488. // to obtain the reported position solution,
  489. // talker shall be set to GN, to indicate that
  490. // the satellites are used in a combined solution
  491. sv_meta.talker[0] = 'G';
  492. sv_meta.talker[1] = 'N';
  493. }
  494. return &sv_meta;
  495. }
  496. /*===========================================================================
  497. FUNCTION loc_nmea_put_checksum
  498. DESCRIPTION
  499. Generate NMEA sentences generated based on position report
  500. DEPENDENCIES
  501. NONE
  502. RETURN VALUE
  503. Total length of the nmea sentence
  504. SIDE EFFECTS
  505. N/A
  506. ===========================================================================*/
  507. static int loc_nmea_put_checksum(char *pNmea, int maxSize, bool isTagBlock)
  508. {
  509. uint8_t checksum = 0;
  510. int length = 0;
  511. int checksumLength = 0;
  512. if(NULL == pNmea)
  513. return 0;
  514. pNmea++; //skip the $ or / for Tag Block
  515. while (*pNmea != '\0')
  516. {
  517. checksum ^= *pNmea++;
  518. length++;
  519. }
  520. if (isTagBlock) {
  521. // length now contains tag block sentence string length not including / sign.
  522. checksumLength = snprintf(pNmea, (maxSize-length-1), "*%02X\\", checksum);
  523. } else {
  524. // length now contains nmea sentence string length not including $ sign.
  525. checksumLength = snprintf(pNmea, (maxSize-length-1), "*%02X\r\n", checksum);
  526. }
  527. // total length of nmea sentence is length of nmea sentence inc $ sign plus
  528. // length of checksum (+1 is to cover the $ character in the length).
  529. return (length + checksumLength + 1);
  530. }
  531. /*===========================================================================
  532. FUNCTION loc_nmea_generate_GSA
  533. DESCRIPTION
  534. Generate NMEA GSA sentences generated based on position report
  535. Currently below sentences are generated:
  536. - $GPGSA : GPS DOP and active SVs
  537. - $GLGSA : GLONASS DOP and active SVs
  538. - $GAGSA : GALILEO DOP and active SVs
  539. - $GNGSA : GNSS DOP and active SVs
  540. DEPENDENCIES
  541. NONE
  542. RETURN VALUE
  543. Number of SVs used
  544. SIDE EFFECTS
  545. N/A
  546. ===========================================================================*/
  547. static uint32_t loc_nmea_generate_GSA(const GpsLocationExtended &locationExtended,
  548. char* sentence,
  549. int bufSize,
  550. loc_nmea_sv_meta* sv_meta_p,
  551. std::vector<std::string> &nmeaArraystr,
  552. bool isTagBlockGroupingEnabled)
  553. {
  554. if (!sentence || bufSize <= 0 || !sv_meta_p)
  555. {
  556. LOC_LOGE("NMEA Error invalid arguments.");
  557. return 0;
  558. }
  559. char* pMarker = sentence;
  560. int lengthRemaining = bufSize;
  561. int length = 0;
  562. int lengthTagBlock = 0;
  563. uint32_t svUsedCount = 0;
  564. uint32_t svUsedList[64] = {0};
  565. uint32_t sentenceCount = 0;
  566. uint32_t sentenceNumber = 1;
  567. size_t svNumber = 1;
  568. static uint32_t code = 1;
  569. char fixType = '\0';
  570. const char* talker = sv_meta_p->talker;
  571. uint32_t svIdOffset = sv_meta_p->svIdOffset;
  572. uint64_t mask = sv_meta_p->mask;
  573. if (!(sv_meta_p->svTypeMask & (1 << GNSS_SV_TYPE_GLONASS))) {
  574. svIdOffset = 0;
  575. }
  576. for (uint8_t i = 1; mask > 0 && svUsedCount < 64; i++)
  577. {
  578. if (mask & 1)
  579. svUsedList[svUsedCount++] = i + svIdOffset;
  580. mask = mask >> 1;
  581. }
  582. if (svUsedCount == 0) {
  583. return 0;
  584. } else {
  585. sentenceNumber = 1;
  586. sentenceCount = svUsedCount / 12 + (svUsedCount % 12 != 0);
  587. svNumber = 1;
  588. }
  589. while (sentenceNumber <= sentenceCount) {
  590. pMarker = sentence;
  591. lengthRemaining = bufSize;
  592. if (svUsedCount > 12 && isTagBlockGroupingEnabled) {
  593. lengthTagBlock = snprintf(pMarker, lengthRemaining, "\\g:%d-%d-%d", sentenceNumber,
  594. sentenceCount, code);
  595. if (MAX_TAG_BLOCK_GROUP_CODE == code) {
  596. code = 1;
  597. }
  598. lengthTagBlock = loc_nmea_put_checksum(sentence, bufSize, true);
  599. pMarker += lengthTagBlock;
  600. lengthRemaining -= lengthTagBlock;
  601. }
  602. if (sv_meta_p->totalSvUsedCount == 0)
  603. fixType = '1'; // no fix
  604. else if (sv_meta_p->totalSvUsedCount <= 3)
  605. fixType = '2'; // 2D fix
  606. else
  607. fixType = '3'; // 3D fix
  608. // Start printing the sentence
  609. // Format: $--GSA,a,x,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,p.p,h.h,v.v,s*cc
  610. // a : Mode : A : Automatic, allowed to automatically switch 2D/3D
  611. // x : Fixtype : 1 (no fix), 2 (2D fix), 3 (3D fix)
  612. // xx : 12 SV ID
  613. // p.p : Position DOP (Dilution of Precision)
  614. // h.h : Horizontal DOP
  615. // v.v : Vertical DOP
  616. // s : GNSS System Id
  617. // cc : Checksum value
  618. length = snprintf(pMarker, lengthRemaining, "$%sGSA,A,%c,", talker, fixType);
  619. if (length < 0 || length >= lengthRemaining) {
  620. LOC_LOGE("NMEA Error in string formatting");
  621. return 0;
  622. }
  623. pMarker += length;
  624. lengthRemaining -= length;
  625. // Add 12 satellite IDs
  626. for (uint8_t i = 0; i < 12; i++, svNumber++)
  627. {
  628. if (svNumber <= svUsedCount)
  629. length = snprintf(pMarker, lengthRemaining, "%02d,", svUsedList[svNumber - 1]);
  630. else
  631. length = snprintf(pMarker, lengthRemaining, ",");
  632. if (length < 0 || length >= lengthRemaining) {
  633. LOC_LOGE("NMEA Error in string formatting");
  634. return 0;
  635. }
  636. pMarker += length;
  637. lengthRemaining -= length;
  638. }
  639. // Add the position/horizontal/vertical DOP values
  640. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
  641. length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,",
  642. locationExtended.pdop,
  643. locationExtended.hdop,
  644. locationExtended.vdop);
  645. } else if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_EXT_DOP) {
  646. length = snprintf(pMarker, lengthRemaining, "%.1f,%.1f,%.1f,",
  647. locationExtended.extDOP.PDOP,
  648. locationExtended.extDOP.HDOP,
  649. locationExtended.extDOP.VDOP);
  650. } else { // no dop
  651. length = snprintf(pMarker, lengthRemaining, ",,,");
  652. }
  653. pMarker += length;
  654. lengthRemaining -= length;
  655. // system id
  656. length = snprintf(pMarker, lengthRemaining, "%d", sv_meta_p->systemId);
  657. pMarker += length;
  658. lengthRemaining -= length;
  659. /* Sentence is ready, add checksum and broadcast */
  660. length = loc_nmea_put_checksum(sentence + lengthTagBlock, bufSize - lengthTagBlock, false);
  661. nmeaArraystr.push_back(sentence);
  662. sentenceNumber++;
  663. if (!isTagBlockGroupingEnabled) {
  664. break;
  665. }
  666. }
  667. if (svUsedCount > 12 && isTagBlockGroupingEnabled) {
  668. code++;
  669. }
  670. return svUsedCount;
  671. }
  672. /*===========================================================================
  673. FUNCTION loc_nmea_generate_GSV
  674. DESCRIPTION
  675. Generate NMEA GSV sentences generated based on sv report
  676. Currently below sentences are generated:
  677. - $GPGSV: GPS Satellites in View
  678. - $GLGSV: GLONASS Satellites in View
  679. - $GAGSV: GALILEO Satellites in View
  680. DEPENDENCIES
  681. NONE
  682. RETURN VALUE
  683. NONE
  684. SIDE EFFECTS
  685. N/A
  686. ===========================================================================*/
  687. static void loc_nmea_generate_GSV(const GnssSvNotification &svNotify,
  688. char* sentence,
  689. int bufSize,
  690. loc_nmea_sv_meta* sv_meta_p,
  691. std::vector<std::string> &nmeaArraystr)
  692. {
  693. if (!sentence || bufSize <= 0)
  694. {
  695. LOC_LOGE("NMEA Error invalid argument.");
  696. return;
  697. }
  698. char* pMarker = sentence;
  699. int lengthRemaining = bufSize;
  700. int length = 0;
  701. int sentenceCount = 0;
  702. int sentenceNumber = 1;
  703. size_t svNumber = 1;
  704. const char* talker = sv_meta_p->talker;
  705. uint32_t svIdOffset = sv_meta_p->svIdOffset;
  706. int svCount = sv_meta_p->svCount;
  707. if (svCount <= 0)
  708. {
  709. LOC_LOGV("No SV in view for talker ID:%s, signal ID:%X", talker, sv_meta_p->signalId);
  710. return;
  711. }
  712. if ((1 << GNSS_SV_TYPE_GLONASS) & sv_meta_p->svTypeMask) {
  713. svIdOffset = 0;
  714. }
  715. svNumber = 1;
  716. sentenceNumber = 1;
  717. sentenceCount = svCount / 4 + (svCount % 4 != 0);
  718. while (sentenceNumber <= sentenceCount)
  719. {
  720. pMarker = sentence;
  721. lengthRemaining = bufSize;
  722. length = snprintf(pMarker, lengthRemaining, "$%sGSV,%d,%d,%02d",
  723. talker, sentenceCount, sentenceNumber, svCount);
  724. if (length < 0 || length >= lengthRemaining)
  725. {
  726. LOC_LOGE("NMEA Error in string formatting");
  727. return;
  728. }
  729. pMarker += length;
  730. lengthRemaining -= length;
  731. for (int i=0; (svNumber <= svNotify.count) && (i < 4); svNumber++)
  732. {
  733. GnssSignalTypeMask signalType = svNotify.gnssSvs[svNumber-1].gnssSignalTypeMask;
  734. if (0 == signalType) {
  735. // If no signal type in report, it means default L1,G1,E1,B1I
  736. switch (svNotify.gnssSvs[svNumber - 1].type)
  737. {
  738. case GNSS_SV_TYPE_GPS:
  739. signalType = GNSS_SIGNAL_GPS_L1CA;
  740. break;
  741. case GNSS_SV_TYPE_GLONASS:
  742. signalType = GNSS_SIGNAL_GLONASS_G1;
  743. break;
  744. case GNSS_SV_TYPE_GALILEO:
  745. signalType = GNSS_SIGNAL_GALILEO_E1;
  746. break;
  747. case GNSS_SV_TYPE_QZSS:
  748. signalType = GNSS_SIGNAL_QZSS_L1CA;
  749. break;
  750. case GNSS_SV_TYPE_BEIDOU:
  751. signalType = GNSS_SIGNAL_BEIDOU_B1I;
  752. break;
  753. case GNSS_SV_TYPE_SBAS:
  754. signalType = GNSS_SIGNAL_SBAS_L1;
  755. break;
  756. case GNSS_SV_TYPE_NAVIC:
  757. signalType = GNSS_SIGNAL_NAVIC_L5;
  758. break;
  759. default:
  760. LOC_LOGE("NMEA Error unknow constellation type: %d",
  761. svNotify.gnssSvs[svNumber - 1].type);
  762. continue;
  763. }
  764. }
  765. if ((sv_meta_p->svTypeMask & (1 << svNotify.gnssSvs[svNumber - 1].type)) &&
  766. sv_meta_p->signalId == convert_signalType_to_signalId(signalType))
  767. {
  768. if (GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svNumber - 1].type) {
  769. svIdOffset = SBAS_SV_ID_OFFSET;
  770. }
  771. if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svNumber - 1].type &&
  772. GLO_SV_PRN_SLOT_UNKNOWN == svNotify.gnssSvs[svNumber - 1].svId) {
  773. length = snprintf(pMarker, lengthRemaining, ",,%02d,%03d,",
  774. (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
  775. (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
  776. } else {
  777. length = snprintf(pMarker, lengthRemaining, ",%02d,%02d,%03d,",
  778. svNotify.gnssSvs[svNumber - 1].svId - svIdOffset,
  779. (int)(0.5 + svNotify.gnssSvs[svNumber - 1].elevation), //float to int
  780. (int)(0.5 + svNotify.gnssSvs[svNumber - 1].azimuth)); //float to int
  781. }
  782. if (length < 0 || length >= lengthRemaining)
  783. {
  784. LOC_LOGE("NMEA Error in string formatting");
  785. return;
  786. }
  787. pMarker += length;
  788. lengthRemaining -= length;
  789. if (svNotify.gnssSvs[svNumber - 1].cN0Dbhz > 0)
  790. {
  791. length = snprintf(pMarker, lengthRemaining, "%02d",
  792. (int)(0.5 + svNotify.gnssSvs[svNumber - 1].cN0Dbhz)); //float to int
  793. if (length < 0 || length >= lengthRemaining)
  794. {
  795. LOC_LOGE("NMEA Error in string formatting");
  796. return;
  797. }
  798. pMarker += length;
  799. lengthRemaining -= length;
  800. }
  801. i++;
  802. }
  803. }
  804. // append signalId
  805. length = snprintf(pMarker, lengthRemaining, ",%X", sv_meta_p->signalId);
  806. pMarker += length;
  807. lengthRemaining -= length;
  808. length = loc_nmea_put_checksum(sentence, bufSize, false);
  809. nmeaArraystr.push_back(sentence);
  810. sentenceNumber++;
  811. } //while
  812. }
  813. /*===========================================================================
  814. FUNCTION loc_nmea_generate_DTM
  815. DESCRIPTION
  816. Generate NMEA DTM sentences generated based on position report
  817. DEPENDENCIES
  818. NONE
  819. RETURN VALUE
  820. NONE
  821. SIDE EFFECTS
  822. N/A
  823. ===========================================================================*/
  824. static void loc_nmea_generate_DTM(const LocLla &ref_lla,
  825. const LocLla &local_lla,
  826. char *talker,
  827. char *sentence,
  828. int bufSize)
  829. {
  830. char* pMarker = sentence;
  831. int lengthRemaining = bufSize;
  832. int length = 0;
  833. int datum_type;
  834. char ref_datum[4] = {'W', '8', '4', '\0'};
  835. char local_datum[4] = {0};
  836. double lla_offset[3] = {0};
  837. char latHem, longHem;
  838. double latMins, longMins;
  839. datum_type = loc_get_datum_type();
  840. switch (datum_type) {
  841. case LOC_GNSS_DATUM_WGS84:
  842. local_datum[0] = 'W';
  843. local_datum[1] = '8';
  844. local_datum[2] = '4';
  845. break;
  846. case LOC_GNSS_DATUM_PZ90:
  847. local_datum[0] = 'P';
  848. local_datum[1] = '9';
  849. local_datum[2] = '0';
  850. break;
  851. default:
  852. break;
  853. }
  854. length = snprintf(pMarker , lengthRemaining , "$%sDTM,%s,," , talker, local_datum);
  855. if (length < 0 || length >= lengthRemaining) {
  856. LOC_LOGE("NMEA Error in string formatting");
  857. return;
  858. }
  859. pMarker += length;
  860. lengthRemaining -= length;
  861. lla_offset[0] = local_lla.lat - ref_lla.lat;
  862. lla_offset[1] = fmod(local_lla.lon - ref_lla.lon, 360.0);
  863. if (lla_offset[1] < -180.0) {
  864. lla_offset[1] += 360.0;
  865. } else if ( lla_offset[1] > 180.0) {
  866. lla_offset[1] -= 360.0;
  867. }
  868. lla_offset[2] = local_lla.alt - ref_lla.alt;
  869. if (lla_offset[0] >= 0.0) {
  870. latHem = 'N';
  871. } else {
  872. latHem = 'S';
  873. lla_offset[0] *= -1.0;
  874. }
  875. latMins = fmod(lla_offset[0] * 60.0, 60.0);
  876. if (lla_offset[1] < 0.0) {
  877. longHem = 'W';
  878. lla_offset[1] *= -1.0;
  879. }else {
  880. longHem = 'E';
  881. }
  882. longMins = fmod(lla_offset[1] * 60.0, 60.0);
  883. length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,%.3lf,",
  884. (uint8_t)floor(lla_offset[0]), latMins, latHem,
  885. (uint8_t)floor(lla_offset[1]), longMins, longHem, lla_offset[2]);
  886. if (length < 0 || length >= lengthRemaining) {
  887. LOC_LOGE("NMEA Error in string formatting");
  888. return;
  889. }
  890. pMarker += length;
  891. lengthRemaining -= length;
  892. length = snprintf(pMarker , lengthRemaining , "%s" , ref_datum);
  893. if (length < 0 || length >= lengthRemaining) {
  894. LOC_LOGE("NMEA Error in string formatting");
  895. return;
  896. }
  897. pMarker += length;
  898. lengthRemaining -= length;
  899. length = loc_nmea_put_checksum(sentence, bufSize, false);
  900. }
  901. /*===========================================================================
  902. FUNCTION get_utctime_with_leapsecond_transition
  903. DESCRIPTION
  904. This function returns true if the position report is generated during
  905. leap second transition period. If not, then the utc timestamp returned
  906. will be set to the timestamp in the position report. If it is,
  907. then the utc timestamp returned will need to take into account
  908. of the leap second transition so that proper calendar year/month/date
  909. can be calculated from the returned utc timestamp.
  910. DEPENDENCIES
  911. NONE
  912. RETURN VALUE
  913. true: position report is generated in leap second transition period.
  914. SIDE EFFECTS
  915. N/A
  916. ===========================================================================*/
  917. static bool get_utctime_with_leapsecond_transition(
  918. const UlpLocation &location,
  919. const GpsLocationExtended &locationExtended,
  920. const LocationSystemInfo &systemInfo,
  921. LocGpsUtcTime &utcPosTimestamp)
  922. {
  923. bool inTransition = false;
  924. // position report is not generated during leap second transition,
  925. // we can use the UTC timestamp from position report as is
  926. utcPosTimestamp = location.gpsLocation.timestamp;
  927. // Check whether we are in leap second transition.
  928. // If so, per NMEA spec, we need to display the extra second in format of 23:59:60
  929. // with year/month/date not getting advanced.
  930. if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_GPS_TIME) &&
  931. ((systemInfo.systemInfoMask & LOCATION_SYS_INFO_LEAP_SECOND) &&
  932. (systemInfo.leapSecondSysInfo.leapSecondInfoMask &
  933. LEAP_SECOND_SYS_INFO_LEAP_SECOND_CHANGE_BIT))) {
  934. const LeapSecondChangeInfo &leapSecondChangeInfo =
  935. systemInfo.leapSecondSysInfo.leapSecondChangeInfo;
  936. const GnssSystemTimeStructType &gpsTimestampLsChange =
  937. leapSecondChangeInfo.gpsTimestampLsChange;
  938. uint64_t gpsTimeLsChange = gpsTimestampLsChange.systemWeek * MSEC_IN_ONE_WEEK +
  939. gpsTimestampLsChange.systemMsec;
  940. uint64_t gpsTimePosReport = locationExtended.gpsTime.gpsWeek * MSEC_IN_ONE_WEEK +
  941. locationExtended.gpsTime.gpsTimeOfWeekMs;
  942. // we are only dealing with positive leap second change, as negative
  943. // leap second change has never occurred and should not occur in future
  944. if (leapSecondChangeInfo.leapSecondsAfterChange >
  945. leapSecondChangeInfo.leapSecondsBeforeChange) {
  946. // leap second adjustment is always 1 second at a time. It can happen
  947. // every quarter end and up to four times per year.
  948. if ((gpsTimePosReport >= gpsTimeLsChange) &&
  949. (gpsTimePosReport < (gpsTimeLsChange + 1000))) {
  950. inTransition = true;
  951. utcPosTimestamp = gpsTimeLsChange + UTC_GPS_OFFSET_MSECS -
  952. leapSecondChangeInfo.leapSecondsBeforeChange * 1000;
  953. // we substract 1000 milli-seconds from UTC timestmap in order to calculate the
  954. // proper year, month and date during leap second transtion.
  955. // Let us give an example, assuming leap second transition is scheduled on 2019,
  956. // Dec 31st mid night. When leap second transition is happening,
  957. // instead of outputting the time as 2020, Jan, 1st, 00 hour, 00 min, and 00 sec.
  958. // The time need to be displayed as 2019, Dec, 31st, 23 hour, 59 min and 60 sec.
  959. utcPosTimestamp -= 1000;
  960. }
  961. }
  962. }
  963. return inTransition;
  964. }
  965. /*===========================================================================
  966. FUNCTION loc_nmea_get_fix_quality
  967. DESCRIPTION
  968. This function obtains the fix quality for GGA sentence, mode indicator
  969. for RMC and VTG sentence based on nav solution mask and tech mask in
  970. the postion report.
  971. DEPENDENCIES
  972. NONE
  973. Output parameter
  974. ggaGpsQuality: gps quality field in GGA sentence
  975. rmcModeIndicator: mode indicator field in RMC sentence
  976. vtgModeIndicator: mode indicator field in VTG sentence
  977. SIDE EFFECTS
  978. N/A
  979. ===========================================================================*/
  980. static void loc_nmea_get_fix_quality(const UlpLocation & location,
  981. const GpsLocationExtended & locationExtended,
  982. bool custom_gga_fix_quality,
  983. char ggaGpsQuality[3],
  984. char & rmcModeIndicator,
  985. char & vtgModeIndicator,
  986. char gnsModeIndicator[7]) {
  987. ggaGpsQuality[0] = '0'; // 0 means no fix
  988. rmcModeIndicator = 'N'; // N means no fix
  989. vtgModeIndicator = 'N'; // N means no fix
  990. memset(gnsModeIndicator, 'N', 6); // N means no fix
  991. gnsModeIndicator[6] = '\0';
  992. do {
  993. if (!(location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)){
  994. break;
  995. }
  996. // NOTE: Order of the check is important
  997. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
  998. if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
  999. ggaGpsQuality[0] = '2'; // 2 means DGPS fix
  1000. rmcModeIndicator = 'P'; // P means precise
  1001. vtgModeIndicator = 'P'; // P means precise
  1002. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1003. gnsModeIndicator[0] = 'P'; // P means precise
  1004. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1005. gnsModeIndicator[1] = 'P'; // P means precise
  1006. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1007. gnsModeIndicator[2] = 'P'; // P means precise
  1008. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1009. gnsModeIndicator[3] = 'P'; // P means precise
  1010. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1011. gnsModeIndicator[4] = 'P'; // P means precise
  1012. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1013. gnsModeIndicator[5] = 'P'; // P means precise
  1014. break;
  1015. } else if (LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask){
  1016. ggaGpsQuality[0] = '4'; // 4 means RTK Fixed fix
  1017. rmcModeIndicator = 'R'; // use R (RTK fixed)
  1018. vtgModeIndicator = 'D'; // use D (differential) as
  1019. // no RTK fixed defined for VTG in NMEA 183 spec
  1020. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1021. gnsModeIndicator[0] = 'R'; // R means RTK fixed
  1022. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1023. gnsModeIndicator[1] = 'R'; // R means RTK fixed
  1024. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1025. gnsModeIndicator[2] = 'R'; // R means RTK fixed
  1026. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1027. gnsModeIndicator[3] = 'R'; // R means RTK fixed
  1028. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1029. gnsModeIndicator[4] = 'R'; // R means RTK fixed
  1030. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1031. gnsModeIndicator[5] = 'R'; // R means RTK fixed
  1032. break;
  1033. } else if (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask){
  1034. ggaGpsQuality[0] = '5'; // 5 means RTK float fix
  1035. rmcModeIndicator = 'F'; // F means RTK float fix
  1036. vtgModeIndicator = 'D'; // use D (differential) as
  1037. // no RTK float defined for VTG in NMEA 183 spec
  1038. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1039. gnsModeIndicator[0] = 'F'; // F means RTK float fix
  1040. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1041. gnsModeIndicator[1] = 'F'; // F means RTK float fix
  1042. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1043. gnsModeIndicator[2] = 'F'; // F means RTK float fix
  1044. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1045. gnsModeIndicator[3] = 'F'; // F means RTK float fix
  1046. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1047. gnsModeIndicator[4] = 'F'; // F means RTK float fix
  1048. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1049. gnsModeIndicator[5] = 'F'; // F means RTK float fix
  1050. break;
  1051. } else if (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask){
  1052. ggaGpsQuality[0] = '2'; // 2 means DGPS fix
  1053. rmcModeIndicator = 'D'; // D means differential
  1054. vtgModeIndicator = 'D'; // D means differential
  1055. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1056. gnsModeIndicator[0] = 'D'; // D means differential
  1057. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1058. gnsModeIndicator[1] = 'D'; // D means differential
  1059. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1060. gnsModeIndicator[2] = 'D'; // D means differential
  1061. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1062. gnsModeIndicator[3] = 'D'; // D means differential
  1063. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1064. gnsModeIndicator[4] = 'D'; // D means differential
  1065. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1066. gnsModeIndicator[5] = 'D'; // D means differential
  1067. break;
  1068. } else if (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask){
  1069. ggaGpsQuality[0] = '2'; // 2 means DGPS fix
  1070. rmcModeIndicator = 'D'; // D means differential
  1071. vtgModeIndicator = 'D'; // D means differential
  1072. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1073. gnsModeIndicator[0] = 'D'; // D means differential
  1074. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1075. gnsModeIndicator[1] = 'D'; // D means differential
  1076. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1077. gnsModeIndicator[2] = 'D'; // D means differential
  1078. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1079. gnsModeIndicator[3] = 'D'; // D means differential
  1080. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1081. gnsModeIndicator[4] = 'D'; // D means differential
  1082. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1083. gnsModeIndicator[5] = 'D'; // D means differential
  1084. break;
  1085. }
  1086. }
  1087. // NOTE: Order of the check is important
  1088. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
  1089. if (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask){
  1090. ggaGpsQuality[0] = '1'; // 1 means GPS
  1091. rmcModeIndicator = 'A'; // A means autonomous
  1092. vtgModeIndicator = 'A'; // A means autonomous
  1093. if (locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask ? 1 : 0)
  1094. gnsModeIndicator[0] = 'A'; // A means autonomous
  1095. if (locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask ? 1 : 0)
  1096. gnsModeIndicator[1] = 'A'; // A means autonomous
  1097. if (locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask ? 1 : 0)
  1098. gnsModeIndicator[2] = 'A'; // A means autonomous
  1099. if (locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask ? 1 : 0)
  1100. gnsModeIndicator[3] = 'A'; // A means autonomous
  1101. if (locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask ? 1 : 0)
  1102. gnsModeIndicator[4] = 'A'; // A means autonomous
  1103. if (locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask ? 1 : 0)
  1104. gnsModeIndicator[5] = 'A'; // A means autonomous
  1105. break;
  1106. } else if ((LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask) ||
  1107. (LOC_POS_TECH_MASK_PROPAGATED & locationExtended.tech_mask)){
  1108. ggaGpsQuality[0] = '6'; // 6 means estimated (dead reckoning)
  1109. rmcModeIndicator = 'E'; // E means estimated (dead reckoning)
  1110. vtgModeIndicator = 'E'; // E means estimated (dead reckoning)
  1111. memset(gnsModeIndicator, 'E', 6); // E means estimated (dead reckoning)
  1112. break;
  1113. }
  1114. }
  1115. } while (0);
  1116. do {
  1117. // check for customized nmea enabled or not
  1118. // with customized GGA quality enabled
  1119. // PPP fix w/o sensor: 59, PPP fix w/ sensor: 69
  1120. // DGNSS/SBAS correction fix w/o sensor: 2, w/ sensor: 62
  1121. // RTK fixed fix w/o sensor: 4, w/ sensor: 64
  1122. // RTK float fix w/o sensor: 5, w/ sensor: 65
  1123. // SPE fix w/o sensor: 1, and w/ sensor: 61
  1124. // Sensor dead reckoning fix: 6
  1125. if (true == custom_gga_fix_quality) {
  1126. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_NAV_SOLUTION_MASK) {
  1127. // PPP fix w/o sensor: fix quality will now be 59
  1128. // PPP fix w sensor: fix quality will now be 69
  1129. if (LOC_NAV_MASK_PPP_CORRECTION & locationExtended.navSolutionMask) {
  1130. if ((locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) &&
  1131. (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask)) {
  1132. ggaGpsQuality[0] = '6';
  1133. ggaGpsQuality[1] = '9';
  1134. } else {
  1135. ggaGpsQuality[0] = '5';
  1136. ggaGpsQuality[1] = '9';
  1137. }
  1138. break;
  1139. }
  1140. }
  1141. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_POS_TECH_MASK) {
  1142. if (LOC_POS_TECH_MASK_SENSORS & locationExtended.tech_mask){
  1143. char ggaQuality_copy = ggaGpsQuality[0];
  1144. ggaGpsQuality[0] = '6'; // 6 sensor assisted
  1145. // RTK fixed fix w/ sensor: fix quality will now be 64
  1146. // RTK float fix w/ sensor: 65
  1147. // DGNSS and/or SBAS correction fix and w/ sensor: 62
  1148. // GPS fix without correction and w/ sensor: 61
  1149. if ((LOC_NAV_MASK_RTK_FIXED_CORRECTION & locationExtended.navSolutionMask)||
  1150. (LOC_NAV_MASK_RTK_CORRECTION & locationExtended.navSolutionMask)||
  1151. (LOC_NAV_MASK_DGNSS_CORRECTION & locationExtended.navSolutionMask)||
  1152. (LOC_NAV_MASK_SBAS_CORRECTION_IONO & locationExtended.navSolutionMask)||
  1153. (LOC_POS_TECH_MASK_SATELLITE & locationExtended.tech_mask)) {
  1154. ggaGpsQuality[1] = ggaQuality_copy;
  1155. break;
  1156. }
  1157. }
  1158. }
  1159. }
  1160. } while (0);
  1161. LOC_LOGv("gps quality: %s, rmc mode indicator: %c, vtg mode indicator: %c",
  1162. ggaGpsQuality, rmcModeIndicator, vtgModeIndicator);
  1163. }
  1164. /*===========================================================================
  1165. FUNCTION loc_nmea_generate_pos
  1166. DESCRIPTION
  1167. Generate NMEA sentences generated based on position report
  1168. Currently below sentences are generated within this function:
  1169. - $GPGSA : GPS DOP and active SVs
  1170. - $GLGSA : GLONASS DOP and active SVs
  1171. - $GAGSA : GALILEO DOP and active SVs
  1172. - $GNGSA : GNSS DOP and active SVs
  1173. - $--VTG : Track made good and ground speed
  1174. - $--RMC : Recommended minimum navigation information
  1175. - $--GGA : Time, position and fix related data
  1176. DEPENDENCIES
  1177. NONE
  1178. RETURN VALUE
  1179. 0
  1180. SIDE EFFECTS
  1181. N/A
  1182. ===========================================================================*/
  1183. void loc_nmea_generate_pos(const UlpLocation &location,
  1184. const GpsLocationExtended &locationExtended,
  1185. const LocationSystemInfo &systemInfo,
  1186. unsigned char generate_nmea,
  1187. bool custom_gga_fix_quality,
  1188. std::vector<std::string> &nmeaArraystr,
  1189. int& indexOfGGA,
  1190. bool isTagBlockGroupingEnabled)
  1191. {
  1192. ENTRY_LOG();
  1193. indexOfGGA = -1;
  1194. LocGpsUtcTime utcPosTimestamp = 0;
  1195. bool inLsTransition = false;
  1196. inLsTransition = get_utctime_with_leapsecond_transition
  1197. (location, locationExtended, systemInfo, utcPosTimestamp);
  1198. time_t utcTime(utcPosTimestamp/1000);
  1199. struct tm result;
  1200. tm * pTm = gmtime_r(&utcTime, &result);
  1201. if (NULL == pTm) {
  1202. LOC_LOGE("gmtime failed");
  1203. return;
  1204. }
  1205. char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1206. char sentence_DTM[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1207. char sentence_RMC[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1208. char sentence_GNS[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1209. char sentence_GGA[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1210. char* pMarker = sentence;
  1211. int lengthRemaining = sizeof(sentence);
  1212. int length = 0;
  1213. int utcYear = pTm->tm_year % 100; // 2 digit year
  1214. int utcMonth = pTm->tm_mon + 1; // tm_mon starts at zero
  1215. int utcDay = pTm->tm_mday;
  1216. int utcHours = pTm->tm_hour;
  1217. int utcMinutes = pTm->tm_min;
  1218. int utcSeconds = pTm->tm_sec;
  1219. int utcMSeconds = (location.gpsLocation.timestamp)%1000;
  1220. int datum_type = loc_get_datum_type();
  1221. LocEcef ecef_w84;
  1222. LocEcef ecef_p90;
  1223. LocLla lla_w84;
  1224. LocLla lla_p90;
  1225. LocLla ref_lla;
  1226. LocLla local_lla;
  1227. memset(&ecef_w84, 0, sizeof(ecef_w84));
  1228. memset(&ecef_p90, 0, sizeof(ecef_p90));
  1229. memset(&lla_w84, 0, sizeof(lla_w84));
  1230. memset(&lla_p90, 0, sizeof(lla_p90));
  1231. memset(&ref_lla, 0, sizeof(ref_lla));
  1232. memset(&local_lla, 0, sizeof(local_lla));
  1233. if (inLsTransition) {
  1234. // During leap second transition, we need to display the extra
  1235. // leap second of hour, minute, second as (23:59:60)
  1236. utcHours = 23;
  1237. utcMinutes = 59;
  1238. utcSeconds = 60;
  1239. // As UTC timestamp is freezing during leap second transition,
  1240. // retrieve milli-seconds portion from GPS timestamp.
  1241. utcMSeconds = locationExtended.gpsTime.gpsTimeOfWeekMs % 1000;
  1242. }
  1243. loc_sv_cache_info sv_cache_info = {};
  1244. if (GPS_LOCATION_EXTENDED_HAS_GNSS_SV_USED_DATA & locationExtended.flags) {
  1245. sv_cache_info.gps_used_mask =
  1246. locationExtended.gnss_sv_used_ids.gps_sv_used_ids_mask;
  1247. sv_cache_info.glo_used_mask =
  1248. locationExtended.gnss_sv_used_ids.glo_sv_used_ids_mask;
  1249. sv_cache_info.gal_used_mask =
  1250. locationExtended.gnss_sv_used_ids.gal_sv_used_ids_mask;
  1251. sv_cache_info.bds_used_mask =
  1252. locationExtended.gnss_sv_used_ids.bds_sv_used_ids_mask;
  1253. sv_cache_info.qzss_used_mask =
  1254. locationExtended.gnss_sv_used_ids.qzss_sv_used_ids_mask;
  1255. sv_cache_info.navic_used_mask =
  1256. locationExtended.gnss_sv_used_ids.navic_sv_used_ids_mask;
  1257. }
  1258. if (generate_nmea) {
  1259. char talker[3] = {'G', 'P', '\0'};
  1260. uint32_t svUsedCount = 0;
  1261. uint32_t count = 0;
  1262. loc_nmea_sv_meta sv_meta;
  1263. if (mEnabledNmeaTypes & NMEA_TYPE_GSA) {
  1264. // -------------------
  1265. // ---$GPGSA/$GNGSA---
  1266. // -------------------
  1267. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1268. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
  1269. GNSS_SIGNAL_GPS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1270. if (count > 0)
  1271. {
  1272. svUsedCount += count;
  1273. talker[0] = sv_meta.talker[0];
  1274. talker[1] = sv_meta.talker[1];
  1275. }
  1276. // -------------------
  1277. // ---$GLGSA/$GNGSA---
  1278. // -------------------
  1279. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1280. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
  1281. GNSS_SIGNAL_GLONASS_G1, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1282. if (count > 0)
  1283. {
  1284. svUsedCount += count;
  1285. talker[0] = sv_meta.talker[0];
  1286. talker[1] = sv_meta.talker[1];
  1287. }
  1288. // -------------------
  1289. // ---$GAGSA/$GNGSA---
  1290. // -------------------
  1291. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1292. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
  1293. GNSS_SIGNAL_GALILEO_E1, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1294. if (count > 0)
  1295. {
  1296. svUsedCount += count;
  1297. talker[0] = sv_meta.talker[0];
  1298. talker[1] = sv_meta.talker[1];
  1299. }
  1300. // ----------------------------
  1301. // ---$GBGSA/$GNGSA (BEIDOU)---
  1302. // ----------------------------
  1303. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1304. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
  1305. GNSS_SIGNAL_BEIDOU_B1I, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1306. if (count > 0)
  1307. {
  1308. svUsedCount += count;
  1309. talker[0] = sv_meta.talker[0];
  1310. talker[1] = sv_meta.talker[1];
  1311. }
  1312. // --------------------------
  1313. // ---$GQGSA/$GNGSA (QZSS)---
  1314. // --------------------------
  1315. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1316. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
  1317. GNSS_SIGNAL_QZSS_L1CA, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1318. if (count > 0)
  1319. {
  1320. svUsedCount += count;
  1321. talker[0] = sv_meta.talker[0];
  1322. talker[1] = sv_meta.talker[1];
  1323. }
  1324. // --------------------------
  1325. // ---$GIGSA/$GNGSA (NavIC)---
  1326. // --------------------------
  1327. count = loc_nmea_generate_GSA(locationExtended, sentence, sizeof(sentence),
  1328. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC,
  1329. GNSS_SIGNAL_NAVIC_L5, true), nmeaArraystr, isTagBlockGroupingEnabled);
  1330. if (count > 0)
  1331. {
  1332. svUsedCount += count;
  1333. talker[0] = sv_meta.talker[0];
  1334. talker[1] = sv_meta.talker[1];
  1335. }
  1336. // if svUsedCount is 0, it means we do not generate any GSA sentence yet.
  1337. // in this case, generate an empty GSA sentence
  1338. if (svUsedCount == 0) {
  1339. strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
  1340. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1341. nmeaArraystr.push_back(sentence);
  1342. }
  1343. }
  1344. char ggaGpsQuality[3] = {'0', '\0', '\0'};
  1345. char rmcModeIndicator = 'N';
  1346. char vtgModeIndicator = 'N';
  1347. char gnsModeIndicator[7] = {'N', 'N', 'N', 'N', 'N', 'N', '\0'};
  1348. loc_nmea_get_fix_quality(location, locationExtended, custom_gga_fix_quality,
  1349. ggaGpsQuality, rmcModeIndicator, vtgModeIndicator, gnsModeIndicator);
  1350. // -------------------
  1351. // ------$--VTG-------
  1352. // -------------------
  1353. if (mEnabledNmeaTypes & NMEA_TYPE_VTG) {
  1354. pMarker = sentence;
  1355. lengthRemaining = sizeof(sentence);
  1356. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
  1357. {
  1358. float magTrack = location.gpsLocation.bearing;
  1359. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
  1360. {
  1361. magTrack = location.gpsLocation.bearing - locationExtended.magneticDeviation;
  1362. if (magTrack < 0.0)
  1363. magTrack += 360.0;
  1364. else if (magTrack > 360.0)
  1365. magTrack -= 360.0;
  1366. }
  1367. length = snprintf(pMarker, lengthRemaining, "$%sVTG,%.1lf,T,%.1lf,M,",
  1368. talker, location.gpsLocation.bearing, magTrack);
  1369. }
  1370. else
  1371. {
  1372. length = snprintf(pMarker, lengthRemaining, "$%sVTG,,T,,M,", talker);
  1373. }
  1374. if (length < 0 || length >= lengthRemaining)
  1375. {
  1376. LOC_LOGE("NMEA Error in string formatting");
  1377. return;
  1378. }
  1379. pMarker += length;
  1380. lengthRemaining -= length;
  1381. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
  1382. {
  1383. float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
  1384. float speedKmPerHour = location.gpsLocation.speed * 3.6;
  1385. length = snprintf(pMarker, lengthRemaining, "%.1lf,N,%.1lf,K,",
  1386. speedKnots, speedKmPerHour);
  1387. }
  1388. else
  1389. {
  1390. length = snprintf(pMarker, lengthRemaining, ",N,,K,");
  1391. }
  1392. if (length < 0 || length >= lengthRemaining)
  1393. {
  1394. LOC_LOGE("NMEA Error in string formatting");
  1395. return;
  1396. }
  1397. pMarker += length;
  1398. lengthRemaining -= length;
  1399. length = snprintf(pMarker, lengthRemaining, "%c", vtgModeIndicator);
  1400. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1401. nmeaArraystr.push_back(sentence);
  1402. lla_w84.lat = location.gpsLocation.latitude / 180.0 * M_PI;
  1403. lla_w84.lon = location.gpsLocation.longitude / 180.0 * M_PI;
  1404. lla_w84.alt = location.gpsLocation.altitude;
  1405. convert_Lla_to_Ecef(lla_w84, ecef_w84);
  1406. convert_WGS84_to_PZ90(ecef_w84, ecef_p90);
  1407. convert_Ecef_to_Lla(ecef_p90, lla_p90);
  1408. ref_lla.lat = location.gpsLocation.latitude;
  1409. ref_lla.lon = location.gpsLocation.longitude;
  1410. ref_lla.alt = location.gpsLocation.altitude;
  1411. switch (datum_type) {
  1412. case LOC_GNSS_DATUM_WGS84:
  1413. local_lla.lat = location.gpsLocation.latitude;
  1414. local_lla.lon = location.gpsLocation.longitude;
  1415. local_lla.alt = location.gpsLocation.altitude;
  1416. break;
  1417. case LOC_GNSS_DATUM_PZ90:
  1418. local_lla.lat = lla_p90.lat / M_PI * 180.0;
  1419. local_lla.lon = lla_p90.lon / M_PI * 180.0;
  1420. local_lla.alt = lla_p90.alt;
  1421. break;
  1422. default:
  1423. break;
  1424. }
  1425. }
  1426. // -------------------
  1427. // ------$--DTM-------
  1428. // -------------------
  1429. if (mEnabledNmeaTypes & NMEA_TYPE_DTM) {
  1430. loc_nmea_generate_DTM(ref_lla, local_lla, talker, sentence_DTM, sizeof(sentence_DTM));
  1431. }
  1432. // -------------------
  1433. // ------$--RMC-------
  1434. // -------------------
  1435. if (mEnabledNmeaTypes & NMEA_TYPE_RMC) {
  1436. pMarker = sentence_RMC;
  1437. lengthRemaining = sizeof(sentence_RMC);
  1438. bool validFix = ((0 != sv_cache_info.gps_used_mask) ||
  1439. (0 != sv_cache_info.glo_used_mask) ||
  1440. (0 != sv_cache_info.gal_used_mask) ||
  1441. (0 != sv_cache_info.qzss_used_mask) ||
  1442. (0 != sv_cache_info.bds_used_mask));
  1443. if (validFix) {
  1444. length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,A,",
  1445. talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
  1446. } else {
  1447. length = snprintf(pMarker, lengthRemaining, "$%sRMC,%02d%02d%02d.%02d,V,",
  1448. talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
  1449. }
  1450. if (length < 0 || length >= lengthRemaining)
  1451. {
  1452. LOC_LOGE("NMEA Error in string formatting");
  1453. return;
  1454. }
  1455. pMarker += length;
  1456. lengthRemaining -= length;
  1457. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
  1458. {
  1459. double latitude = ref_lla.lat;
  1460. double longitude = ref_lla.lon;
  1461. char latHemisphere;
  1462. char lonHemisphere;
  1463. double latMinutes;
  1464. double lonMinutes;
  1465. if (latitude > 0)
  1466. {
  1467. latHemisphere = 'N';
  1468. }
  1469. else
  1470. {
  1471. latHemisphere = 'S';
  1472. latitude *= -1.0;
  1473. }
  1474. if (longitude < 0)
  1475. {
  1476. lonHemisphere = 'W';
  1477. longitude *= -1.0;
  1478. }
  1479. else
  1480. {
  1481. lonHemisphere = 'E';
  1482. }
  1483. latMinutes = fmod(latitude * 60.0, 60.0);
  1484. lonMinutes = fmod(longitude * 60.0, 60.0);
  1485. length = snprintf(pMarker, lengthRemaining,
  1486. "%02d%09.6lf,%c,%03d%09.6lf,%c,",
  1487. (uint8_t)floor(latitude), latMinutes, latHemisphere,
  1488. (uint8_t)floor(longitude), lonMinutes, lonHemisphere);
  1489. }
  1490. else
  1491. {
  1492. length = snprintf(pMarker, lengthRemaining, ",,,,");
  1493. }
  1494. if (length < 0 || length >= lengthRemaining)
  1495. {
  1496. LOC_LOGE("NMEA Error in string formatting");
  1497. return;
  1498. }
  1499. pMarker += length;
  1500. lengthRemaining -= length;
  1501. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_SPEED)
  1502. {
  1503. float speedKnots = location.gpsLocation.speed * (3600.0/1852.0);
  1504. length = snprintf(pMarker, lengthRemaining, "%.1lf,", speedKnots);
  1505. }
  1506. else
  1507. {
  1508. length = snprintf(pMarker, lengthRemaining, ",");
  1509. }
  1510. if (length < 0 || length >= lengthRemaining)
  1511. {
  1512. LOC_LOGE("NMEA Error in string formatting");
  1513. return;
  1514. }
  1515. pMarker += length;
  1516. lengthRemaining -= length;
  1517. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_BEARING)
  1518. {
  1519. length = snprintf(pMarker, lengthRemaining, "%.1lf,",
  1520. location.gpsLocation.bearing);
  1521. }
  1522. else
  1523. {
  1524. length = snprintf(pMarker, lengthRemaining, ",");
  1525. }
  1526. if (length < 0 || length >= lengthRemaining)
  1527. {
  1528. LOC_LOGE("NMEA Error in string formatting");
  1529. return;
  1530. }
  1531. pMarker += length;
  1532. lengthRemaining -= length;
  1533. length = snprintf(pMarker, lengthRemaining, "%2.2d%2.2d%2.2d,",
  1534. utcDay, utcMonth, utcYear);
  1535. if (length < 0 || length >= lengthRemaining)
  1536. {
  1537. LOC_LOGE("NMEA Error in string formatting");
  1538. return;
  1539. }
  1540. pMarker += length;
  1541. lengthRemaining -= length;
  1542. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_MAG_DEV)
  1543. {
  1544. float magneticVariation = locationExtended.magneticDeviation;
  1545. char direction;
  1546. if (magneticVariation < 0.0)
  1547. {
  1548. direction = 'W';
  1549. magneticVariation *= -1.0;
  1550. }
  1551. else
  1552. {
  1553. direction = 'E';
  1554. }
  1555. length = snprintf(pMarker, lengthRemaining, "%.1lf,%c,",
  1556. magneticVariation, direction);
  1557. }
  1558. else
  1559. {
  1560. length = snprintf(pMarker, lengthRemaining, ",,");
  1561. }
  1562. if (length < 0 || length >= lengthRemaining)
  1563. {
  1564. LOC_LOGE("NMEA Error in string formatting");
  1565. return;
  1566. }
  1567. pMarker += length;
  1568. lengthRemaining -= length;
  1569. length = snprintf(pMarker, lengthRemaining, "%c", rmcModeIndicator);
  1570. pMarker += length;
  1571. lengthRemaining -= length;
  1572. // hardcode Navigation Status field to 'V'
  1573. length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
  1574. length = loc_nmea_put_checksum(sentence_RMC, sizeof(sentence_RMC), false);
  1575. }
  1576. // -------------------
  1577. // ------$--GNS-------
  1578. // -------------------
  1579. if (mEnabledNmeaTypes & NMEA_TYPE_GNS) {
  1580. pMarker = sentence_GNS;
  1581. lengthRemaining = sizeof(sentence_GNS);
  1582. length = snprintf(pMarker, lengthRemaining, "$%sGNS,%02d%02d%02d.%02d,",
  1583. talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
  1584. if (length < 0 || length >= lengthRemaining)
  1585. {
  1586. LOC_LOGE("NMEA Error in string formatting");
  1587. return;
  1588. }
  1589. pMarker += length;
  1590. lengthRemaining -= length;
  1591. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
  1592. {
  1593. double latitude = ref_lla.lat;
  1594. double longitude = ref_lla.lon;
  1595. char latHemisphere;
  1596. char lonHemisphere;
  1597. double latMinutes;
  1598. double lonMinutes;
  1599. if (latitude > 0)
  1600. {
  1601. latHemisphere = 'N';
  1602. }
  1603. else
  1604. {
  1605. latHemisphere = 'S';
  1606. latitude *= -1.0;
  1607. }
  1608. if (longitude < 0)
  1609. {
  1610. lonHemisphere = 'W';
  1611. longitude *= -1.0;
  1612. }
  1613. else
  1614. {
  1615. lonHemisphere = 'E';
  1616. }
  1617. latMinutes = fmod(latitude * 60.0, 60.0);
  1618. lonMinutes = fmod(longitude * 60.0, 60.0);
  1619. length = snprintf(pMarker, lengthRemaining,
  1620. "%02d%09.6lf,%c,%03d%09.6lf,%c,",
  1621. (uint8_t)floor(latitude), latMinutes, latHemisphere,
  1622. (uint8_t)floor(longitude), lonMinutes, lonHemisphere);
  1623. }
  1624. else
  1625. {
  1626. length = snprintf(pMarker, lengthRemaining, ",,,,");
  1627. }
  1628. if (length < 0 || length >= lengthRemaining)
  1629. {
  1630. LOC_LOGE("NMEA Error in string formatting");
  1631. return;
  1632. }
  1633. pMarker += length;
  1634. lengthRemaining -= length;
  1635. length = snprintf(pMarker, lengthRemaining, "%s,", gnsModeIndicator);
  1636. pMarker += length;
  1637. lengthRemaining -= length;
  1638. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP) {
  1639. length = snprintf(pMarker, lengthRemaining, "%02d,%.1f,",
  1640. svUsedCount, locationExtended.hdop);
  1641. } else if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_EXT_DOP) {
  1642. length = snprintf(pMarker, lengthRemaining, "%02d,%.1f,",
  1643. svUsedCount, locationExtended.extDOP.HDOP);
  1644. }
  1645. else { // no hdop
  1646. length = snprintf(pMarker, lengthRemaining, "%02d,,",
  1647. svUsedCount);
  1648. }
  1649. if (length < 0 || length >= lengthRemaining)
  1650. {
  1651. LOC_LOGE("NMEA Error in string formatting");
  1652. return;
  1653. }
  1654. pMarker += length;
  1655. lengthRemaining -= length;
  1656. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
  1657. {
  1658. length = snprintf(pMarker, lengthRemaining, "%.1lf,",
  1659. locationExtended.altitudeMeanSeaLevel);
  1660. }
  1661. else
  1662. {
  1663. length = snprintf(pMarker, lengthRemaining, ",");
  1664. }
  1665. if (length < 0 || length >= lengthRemaining)
  1666. {
  1667. LOC_LOGE("NMEA Error in string formatting");
  1668. return;
  1669. }
  1670. pMarker += length;
  1671. lengthRemaining -= length;
  1672. if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
  1673. (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
  1674. {
  1675. length = snprintf(pMarker, lengthRemaining, "%.1lf,",
  1676. ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
  1677. }
  1678. else
  1679. {
  1680. length = snprintf(pMarker, lengthRemaining, ",");
  1681. }
  1682. if (length < 0 || length >= lengthRemaining)
  1683. {
  1684. LOC_LOGE("NMEA Error in string formatting");
  1685. return;
  1686. }
  1687. pMarker += length;
  1688. lengthRemaining -= length;
  1689. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE)
  1690. {
  1691. length = snprintf(pMarker, lengthRemaining, "%.1f,",
  1692. (float)locationExtended.dgnssDataAgeMsec / 1000);
  1693. }
  1694. else
  1695. {
  1696. length = snprintf(pMarker, lengthRemaining, ",");
  1697. }
  1698. if (length < 0 || length >= lengthRemaining)
  1699. {
  1700. LOC_LOGE("NMEA Error in string formatting");
  1701. return;
  1702. }
  1703. pMarker += length;
  1704. lengthRemaining -= length;
  1705. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_REF_STATION_ID)
  1706. {
  1707. length = snprintf(pMarker, lengthRemaining, "%04d",
  1708. locationExtended.dgnssRefStationId);
  1709. if (length < 0 || length >= lengthRemaining)
  1710. {
  1711. LOC_LOGE("NMEA Error in string formatting");
  1712. return;
  1713. }
  1714. pMarker += length;
  1715. lengthRemaining -= length;
  1716. }
  1717. // hardcode Navigation Status field to 'V'
  1718. length = snprintf(pMarker, lengthRemaining, ",%c", 'V');
  1719. pMarker += length;
  1720. lengthRemaining -= length;
  1721. length = loc_nmea_put_checksum(sentence_GNS, sizeof(sentence_GNS), false);
  1722. }
  1723. // -------------------
  1724. // ------$--GGA-------
  1725. // -------------------
  1726. if (mEnabledNmeaTypes & NMEA_TYPE_GGA) {
  1727. pMarker = sentence_GGA;
  1728. lengthRemaining = sizeof(sentence_GGA);
  1729. length = snprintf(pMarker, lengthRemaining, "$%sGGA,%02d%02d%02d.%02d,",
  1730. talker, utcHours, utcMinutes, utcSeconds, utcMSeconds/10);
  1731. if (length < 0 || length >= lengthRemaining)
  1732. {
  1733. LOC_LOGE("NMEA Error in string formatting");
  1734. return;
  1735. }
  1736. pMarker += length;
  1737. lengthRemaining -= length;
  1738. if (location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_LAT_LONG)
  1739. {
  1740. double latitude = ref_lla.lat;
  1741. double longitude = ref_lla.lon;
  1742. char latHemisphere;
  1743. char lonHemisphere;
  1744. double latMinutes;
  1745. double lonMinutes;
  1746. if (latitude > 0)
  1747. {
  1748. latHemisphere = 'N';
  1749. }
  1750. else
  1751. {
  1752. latHemisphere = 'S';
  1753. latitude *= -1.0;
  1754. }
  1755. if (longitude < 0)
  1756. {
  1757. lonHemisphere = 'W';
  1758. longitude *= -1.0;
  1759. }
  1760. else
  1761. {
  1762. lonHemisphere = 'E';
  1763. }
  1764. latMinutes = fmod(latitude * 60.0, 60.0);
  1765. lonMinutes = fmod(longitude * 60.0, 60.0);
  1766. length = snprintf(pMarker, lengthRemaining, "%02d%09.6lf,%c,%03d%09.6lf,%c,",
  1767. (uint8_t)floor(latitude), latMinutes, latHemisphere,
  1768. (uint8_t)floor(longitude), lonMinutes, lonHemisphere);
  1769. }
  1770. else
  1771. {
  1772. length = snprintf(pMarker, lengthRemaining, ",,,,");
  1773. }
  1774. if (length < 0 || length >= lengthRemaining)
  1775. {
  1776. LOC_LOGE("NMEA Error in string formatting");
  1777. return;
  1778. }
  1779. pMarker += length;
  1780. lengthRemaining -= length;
  1781. // Number of satellites in use, 00-12
  1782. if (svUsedCount > MAX_SATELLITES_IN_USE)
  1783. svUsedCount = MAX_SATELLITES_IN_USE;
  1784. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DOP)
  1785. {
  1786. length = snprintf(pMarker, lengthRemaining, "%s,%02d,%.1f,",
  1787. ggaGpsQuality, svUsedCount, locationExtended.hdop);
  1788. } else if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_EXT_DOP) {
  1789. length = snprintf(pMarker, lengthRemaining, "%s,%02d,%.1f,",
  1790. ggaGpsQuality, svUsedCount, locationExtended.extDOP.HDOP);
  1791. }
  1792. else
  1793. { // no hdop
  1794. length = snprintf(pMarker, lengthRemaining, "%s,%02d,,",
  1795. ggaGpsQuality, svUsedCount);
  1796. }
  1797. if (length < 0 || length >= lengthRemaining)
  1798. {
  1799. LOC_LOGE("NMEA Error in string formatting");
  1800. return;
  1801. }
  1802. pMarker += length;
  1803. lengthRemaining -= length;
  1804. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL)
  1805. {
  1806. length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
  1807. locationExtended.altitudeMeanSeaLevel);
  1808. }
  1809. else
  1810. {
  1811. length = snprintf(pMarker, lengthRemaining, ",,");
  1812. }
  1813. if (length < 0 || length >= lengthRemaining)
  1814. {
  1815. LOC_LOGE("NMEA Error in string formatting");
  1816. return;
  1817. }
  1818. pMarker += length;
  1819. lengthRemaining -= length;
  1820. if ((location.gpsLocation.flags & LOC_GPS_LOCATION_HAS_ALTITUDE) &&
  1821. (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_ALTITUDE_MEAN_SEA_LEVEL))
  1822. {
  1823. length = snprintf(pMarker, lengthRemaining, "%.1lf,M,",
  1824. ref_lla.alt - locationExtended.altitudeMeanSeaLevel);
  1825. }
  1826. else
  1827. {
  1828. length = snprintf(pMarker, lengthRemaining, ",,");
  1829. }
  1830. if (length < 0 || length >= lengthRemaining)
  1831. {
  1832. LOC_LOGE("NMEA Error in string formatting");
  1833. return;
  1834. }
  1835. pMarker += length;
  1836. lengthRemaining -= length;
  1837. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_DATA_AGE)
  1838. {
  1839. length = snprintf(pMarker, lengthRemaining, "%.1f,",
  1840. (float)locationExtended.dgnssDataAgeMsec / 1000);
  1841. }
  1842. else
  1843. {
  1844. length = snprintf(pMarker, lengthRemaining, ",");
  1845. }
  1846. if (length < 0 || length >= lengthRemaining)
  1847. {
  1848. LOC_LOGE("NMEA Error in string formatting");
  1849. return;
  1850. }
  1851. pMarker += length;
  1852. lengthRemaining -= length;
  1853. if (locationExtended.flags & GPS_LOCATION_EXTENDED_HAS_DGNSS_REF_STATION_ID)
  1854. {
  1855. length = snprintf(pMarker, lengthRemaining, "%04d",
  1856. locationExtended.dgnssRefStationId);
  1857. if (length < 0 || length >= lengthRemaining)
  1858. {
  1859. LOC_LOGE("NMEA Error in string formatting");
  1860. return;
  1861. }
  1862. pMarker += length;
  1863. lengthRemaining -= length;
  1864. }
  1865. length = loc_nmea_put_checksum(sentence_GGA, sizeof(sentence_GGA), false);
  1866. }
  1867. // ------$--DTM-------
  1868. nmeaArraystr.push_back(sentence_DTM);
  1869. // ------$--RMC-------
  1870. nmeaArraystr.push_back(sentence_RMC);
  1871. if(LOC_GNSS_DATUM_PZ90 == datum_type) {
  1872. // ------$--DTM-------
  1873. nmeaArraystr.push_back(sentence_DTM);
  1874. }
  1875. // ------$--GNS-------
  1876. nmeaArraystr.push_back(sentence_GNS);
  1877. if(LOC_GNSS_DATUM_PZ90 == datum_type) {
  1878. // ------$--DTM-------
  1879. nmeaArraystr.push_back(sentence_DTM);
  1880. }
  1881. // ------$--GGA-------
  1882. nmeaArraystr.push_back(sentence_GGA);
  1883. indexOfGGA = static_cast<int>(nmeaArraystr.size() - 1);
  1884. }
  1885. //Send blank NMEA reports for non-final fixes
  1886. else {
  1887. if (mEnabledNmeaTypes & NMEA_TYPE_GSA) {
  1888. strlcpy(sentence, "$GPGSA,A,1,,,,,,,,,,,,,,,,", sizeof(sentence));
  1889. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1890. nmeaArraystr.push_back(sentence);
  1891. }
  1892. if (mEnabledNmeaTypes & NMEA_TYPE_VTG) {
  1893. strlcpy(sentence, "$GPVTG,,T,,M,,N,,K,N", sizeof(sentence));
  1894. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1895. nmeaArraystr.push_back(sentence);
  1896. }
  1897. if (mEnabledNmeaTypes & NMEA_TYPE_DTM) {
  1898. strlcpy(sentence, "$GPDTM,,,,,,,,", sizeof(sentence));
  1899. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1900. nmeaArraystr.push_back(sentence);
  1901. }
  1902. if (mEnabledNmeaTypes & NMEA_TYPE_RMC) {
  1903. strlcpy(sentence, "$GPRMC,,V,,,,,,,,,,N,V", sizeof(sentence));
  1904. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1905. nmeaArraystr.push_back(sentence);
  1906. }
  1907. if (mEnabledNmeaTypes & NMEA_TYPE_GNS) {
  1908. strlcpy(sentence, "$GPGNS,,,,,,N,,,,,,,V", sizeof(sentence));
  1909. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1910. nmeaArraystr.push_back(sentence);
  1911. }
  1912. if (mEnabledNmeaTypes & NMEA_TYPE_GGA) {
  1913. strlcpy(sentence, "$GPGGA,,,,,,0,,,,,,,,", sizeof(sentence));
  1914. length = loc_nmea_put_checksum(sentence, sizeof(sentence), false);
  1915. nmeaArraystr.push_back(sentence);
  1916. }
  1917. }
  1918. EXIT_LOG(%d, 0);
  1919. }
  1920. /*===========================================================================
  1921. FUNCTION loc_nmea_generate_sv
  1922. DESCRIPTION
  1923. Generate NMEA sentences generated based on sv report
  1924. DEPENDENCIES
  1925. NONE
  1926. RETURN VALUE
  1927. 0
  1928. SIDE EFFECTS
  1929. N/A
  1930. ===========================================================================*/
  1931. void loc_nmea_generate_sv(const GnssSvNotification &svNotify,
  1932. std::vector<std::string> &nmeaArraystr)
  1933. {
  1934. ENTRY_LOG();
  1935. char sentence[NMEA_SENTENCE_MAX_LENGTH] = {0};
  1936. loc_sv_cache_info sv_cache_info = {};
  1937. //Count GPS SVs for saparating GPS from GLONASS and throw others
  1938. for (uint32_t svOffset = 0; svOffset < svNotify.count; svOffset++) {
  1939. if ((GNSS_SV_TYPE_GPS == svNotify.gnssSvs[svOffset].type) ||
  1940. (GNSS_SV_TYPE_SBAS == svNotify.gnssSvs[svOffset].type))
  1941. {
  1942. if (GNSS_SIGNAL_GPS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  1943. sv_cache_info.gps_l5_count++;
  1944. } else if (GNSS_SIGNAL_GPS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  1945. sv_cache_info.gps_l2_count++;
  1946. } else {
  1947. // GNSS_SIGNAL_GPS_L1CA, GNSS_SIGNAL_SBAS_L1 or default
  1948. // If no signal type in report, it means default L1
  1949. sv_cache_info.gps_l1_count++;
  1950. }
  1951. }
  1952. else if (GNSS_SV_TYPE_GLONASS == svNotify.gnssSvs[svOffset].type)
  1953. {
  1954. if (GNSS_SIGNAL_GLONASS_G2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){
  1955. sv_cache_info.glo_g2_count++;
  1956. } else {
  1957. // GNSS_SIGNAL_GLONASS_G1 or default
  1958. // If no signal type in report, it means default G1
  1959. sv_cache_info.glo_g1_count++;
  1960. }
  1961. }
  1962. else if (GNSS_SV_TYPE_GALILEO == svNotify.gnssSvs[svOffset].type)
  1963. {
  1964. if(GNSS_SIGNAL_GALILEO_E5A == svNotify.gnssSvs[svOffset].gnssSignalTypeMask){
  1965. sv_cache_info.gal_e5_count++;
  1966. } else if (GNSS_SIGNAL_GALILEO_E5B == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  1967. sv_cache_info.gal_e5b_count++;
  1968. } else {
  1969. // GNSS_SIGNAL_GALILEO_E1 or default
  1970. // If no signal type in report, it means default E1
  1971. sv_cache_info.gal_e1_count++;
  1972. }
  1973. }
  1974. else if (GNSS_SV_TYPE_QZSS == svNotify.gnssSvs[svOffset].type)
  1975. {
  1976. if (GNSS_SIGNAL_QZSS_L5 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  1977. sv_cache_info.qzss_l5_count++;
  1978. } else if (GNSS_SIGNAL_QZSS_L2 == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  1979. sv_cache_info.qzss_l2_count++;
  1980. } else {
  1981. // GNSS_SIGNAL_QZSS_L1CA or default
  1982. // If no signal type in report, it means default L1
  1983. sv_cache_info.qzss_l1_count++;
  1984. }
  1985. }
  1986. else if (GNSS_SV_TYPE_BEIDOU == svNotify.gnssSvs[svOffset].type)
  1987. {
  1988. // cache the used in fix mask, as it will be needed to send $PQGSA
  1989. // during the position report
  1990. if (GNSS_SV_OPTIONS_USED_IN_FIX_BIT ==
  1991. (svNotify.gnssSvs[svOffset].gnssSvOptionsMask &
  1992. GNSS_SV_OPTIONS_USED_IN_FIX_BIT))
  1993. {
  1994. setSvMask(sv_cache_info.bds_used_mask, svNotify.gnssSvs[svOffset].svId);
  1995. }
  1996. if ((GNSS_SIGNAL_BEIDOU_B2AI == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) ||
  1997. (GNSS_SIGNAL_BEIDOU_B2AQ == svNotify.gnssSvs[svOffset].gnssSignalTypeMask)) {
  1998. sv_cache_info.bds_b2_count++;
  1999. } else if (GNSS_SIGNAL_BEIDOU_B1C == svNotify.gnssSvs[svOffset].gnssSignalTypeMask) {
  2000. sv_cache_info.bds_b1c_count++;
  2001. } else {
  2002. // GNSS_SIGNAL_BEIDOU_B1I or default
  2003. // If no signal type in report, it means default B1I
  2004. sv_cache_info.bds_b1i_count++;
  2005. }
  2006. }
  2007. else if (GNSS_SV_TYPE_NAVIC == svNotify.gnssSvs[svOffset].type)
  2008. {
  2009. // GNSS_SIGNAL_NAVIC_L5 is the only signal type for NAVIC
  2010. sv_cache_info.navic_l5_count++;
  2011. }
  2012. }
  2013. loc_nmea_sv_meta sv_meta;
  2014. if (mEnabledNmeaTypes & NMEA_TYPE_GPGSV) {
  2015. // ---------------------
  2016. // ------$GPGSV:L1CA----
  2017. // ---------------------
  2018. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2019. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
  2020. GNSS_SIGNAL_GPS_L1CA, false), nmeaArraystr);
  2021. // ---------------------
  2022. // ------$GPGSV:L5------
  2023. // ---------------------
  2024. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2025. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
  2026. GNSS_SIGNAL_GPS_L5, false), nmeaArraystr);
  2027. // ---------------------
  2028. // ------$GPGSV:L2------
  2029. // ---------------------
  2030. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2031. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GPS,
  2032. GNSS_SIGNAL_GPS_L2, false), nmeaArraystr);
  2033. }
  2034. if (mEnabledNmeaTypes & NMEA_TYPE_GLGSV) {
  2035. // ---------------------
  2036. // ------$GLGSV:G1------
  2037. // ---------------------
  2038. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2039. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
  2040. GNSS_SIGNAL_GLONASS_G1, false), nmeaArraystr);
  2041. // ---------------------
  2042. // ------$GLGSV:G2------
  2043. // ---------------------
  2044. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2045. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GLONASS,
  2046. GNSS_SIGNAL_GLONASS_G2, false), nmeaArraystr);
  2047. }
  2048. if (mEnabledNmeaTypes & NMEA_TYPE_GAGSV) {
  2049. // ---------------------
  2050. // ------$GAGSV:E1------
  2051. // ---------------------
  2052. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2053. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
  2054. GNSS_SIGNAL_GALILEO_E1, false), nmeaArraystr);
  2055. // -------------------------
  2056. // ------$GAGSV:E5A---------
  2057. // -------------------------
  2058. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2059. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
  2060. GNSS_SIGNAL_GALILEO_E5A, false), nmeaArraystr);
  2061. // -------------------------
  2062. // ------$GAGSV:E5B---------
  2063. // -------------------------
  2064. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2065. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_GALILEO,
  2066. GNSS_SIGNAL_GALILEO_E5B, false), nmeaArraystr);
  2067. }
  2068. if (mEnabledNmeaTypes & NMEA_TYPE_GQGSV) {
  2069. // -----------------------------
  2070. // ------$GQGSV (QZSS):L1CA-----
  2071. // -----------------------------
  2072. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2073. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
  2074. GNSS_SIGNAL_QZSS_L1CA, false), nmeaArraystr);
  2075. // -----------------------------
  2076. // ------$GQGSV (QZSS):L5-------
  2077. // -----------------------------
  2078. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2079. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
  2080. GNSS_SIGNAL_QZSS_L5, false), nmeaArraystr);
  2081. // -----------------------------
  2082. // ------$GQGSV (QZSS):L2-------
  2083. // -----------------------------
  2084. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2085. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_QZSS,
  2086. GNSS_SIGNAL_QZSS_L2, false), nmeaArraystr);
  2087. }
  2088. if (mEnabledNmeaTypes & NMEA_TYPE_GBGSV) {
  2089. // -----------------------------
  2090. // ------$GBGSV (BEIDOU:B1I)----
  2091. // -----------------------------
  2092. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2093. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
  2094. GNSS_SIGNAL_BEIDOU_B1I, false), nmeaArraystr);
  2095. // -----------------------------
  2096. // ------$GBGSV (BEIDOU:B1C)----
  2097. // -----------------------------
  2098. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2099. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
  2100. GNSS_SIGNAL_BEIDOU_B1C, false), nmeaArraystr);
  2101. // -----------------------------
  2102. // ------$GBGSV (BEIDOU:B2AI)---
  2103. // -----------------------------
  2104. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2105. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_BEIDOU,
  2106. GNSS_SIGNAL_BEIDOU_B2AI, false), nmeaArraystr);
  2107. }
  2108. // -----------------------------
  2109. // ------$GIGSV (NAVIC:L5)------
  2110. // -----------------------------
  2111. if (mEnabledNmeaTypes & NMEA_TYPE_GIGSV) {
  2112. loc_nmea_generate_GSV(svNotify, sentence, sizeof(sentence),
  2113. loc_nmea_sv_meta_init(sv_meta, sv_cache_info, GNSS_SV_TYPE_NAVIC,
  2114. GNSS_SIGNAL_NAVIC_L5, false), nmeaArraystr);
  2115. }
  2116. EXIT_LOG(%d, 0);
  2117. }
  2118. /*===========================================================================
  2119. FUNCTION loc_nmea_config_output_types
  2120. DESCRIPTION
  2121. Configure the NMEA sentence types that will be generated.
  2122. DEPENDENCIES
  2123. NONE
  2124. RETURN VALUE
  2125. NONE
  2126. SIDE EFFECTS
  2127. N/A
  2128. ===========================================================================*/
  2129. void loc_nmea_config_output_types(GnssNmeaTypesMask enabledNmeaTypes) {
  2130. mEnabledNmeaTypes = enabledNmeaTypes;
  2131. }