diff --git a/Cargo.lock b/Cargo.lock index c708d3a..ae596aa 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2,6 +2,15 @@ # It is not intended for manual editing. version = 4 +[[package]] +name = "aho-corasick" +version = "1.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e60d3430d3a69478ad0993f19238d2df97c507009a52b3c10addcd7f6bcb916" +dependencies = [ + "memchr", +] + [[package]] name = "atomic-polyfill" version = "1.0.3" @@ -32,6 +41,26 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" +[[package]] +name = "bindgen" +version = "0.71.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f58bf3d7db68cfbac37cfc485a8d711e87e064c3d0fe0435b92f7a407f9d6b3" +dependencies = [ + "bitflags 2.9.0", + "cexpr", + "clang-sys", + "itertools", + "log", + "prettyplease", + "proc-macro2", + "quote", + "regex", + "rustc-hash", + "shlex", + "syn 2.0.100", +] + [[package]] name = "bitfield" version = "0.13.2" @@ -46,9 +75,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.6.0" +version = "2.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de" +checksum = "5c8214115b7bf84099f1309324e63141d4c5d7cc26862f97a0a857dbefe165bd" dependencies = [ "serde", ] @@ -65,6 +94,24 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "37b2a672a2cb129a2e41c10b1224bb368f9f37a2b16b612598138befd7b37eb5" +[[package]] +name = "cc" +version = "1.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e3a13707ac958681c13b39b458c073d0d9bc8a22cb1b2f4c8e55eb72c13f362" +dependencies = [ + "shlex", +] + +[[package]] +name = "cexpr" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" +dependencies = [ + "nom", +] + [[package]] name = "cfg-if" version = "1.0.0" @@ -80,6 +127,26 @@ dependencies = [ "serde", ] +[[package]] +name = "clang-sys" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b023947811758c97c59bf9d1c188fd619ad4718dcaa767947df1cadb14f39f4" +dependencies = [ + "glob", + "libc", + "libloading", +] + +[[package]] +name = "cmake" +version = "0.1.54" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7caa3f9de89ddbe2c607f4101924c5abec803763ae9534e4f4d7d8f84aa81f0" +dependencies = [ + "cc", +] + [[package]] name = "cobs" version = "0.2.3" @@ -91,7 +158,7 @@ name = "common-arm" version = "0.1.0" dependencies = [ "cortex-m", - "defmt", + "defmt 0.3.100", "defmt-rtt", "defmt-test", "derive_more", @@ -126,22 +193,22 @@ dependencies = [ [[package]] name = "cortex-m-rt" -version = "0.7.3" +version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" +checksum = "801d4dec46b34c299ccf6b036717ae0fce602faa4f4fe816d9013b9a7c9f5ba6" dependencies = [ "cortex-m-rt-macros", ] [[package]] name = "cortex-m-rt-macros" -version = "0.7.0" +version = "0.7.5" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" +checksum = "e37549a379a9e0e6e576fd208ee60394ccb8be963889eebba3ffe0980364f472" dependencies = [ "proc-macro2", "quote", - "syn 1.0.109", + "syn 2.0.100", ] [[package]] @@ -167,9 +234,18 @@ checksum = "790eea4361631c5e7d22598ecd5723ff611904e3344ce8720784c93e3d83d40b" [[package]] name = "defmt" -version = "0.3.8" +version = "0.3.100" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0963443817029b2024136fc4dd07a5107eb8f977eaf18fcd1fdeb11306b64ad" +dependencies = [ + "defmt 1.0.1", +] + +[[package]] +name = "defmt" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a99dd22262668b887121d4672af5a64b238f026099f1a2a1b322066c9ecfe9e0" +checksum = "548d977b6da32fa1d1fda2876453da1e7df63ad0304c8b3dae4dbe7b96f39b78" dependencies = [ "bitflags 1.3.2", "defmt-macros", @@ -177,72 +253,78 @@ dependencies = [ [[package]] name = "defmt-macros" -version = "0.3.9" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e3a9f309eff1f79b3ebdf252954d90ae440599c26c2c553fe87a2d17195f2dcb" +checksum = "3d4fc12a85bcf441cfe44344c4b72d58493178ce635338a3f3b78943aceb258e" dependencies = [ "defmt-parser", - "proc-macro-error", + "proc-macro-error2", "proc-macro2", "quote", - "syn 2.0.85", + "syn 2.0.100", ] [[package]] name = "defmt-parser" -version = "0.3.4" +version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff4a5fefe330e8d7f31b16a318f9ce81000d8e35e69b93eae154d16d2278f70f" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ - "thiserror", + "thiserror 2.0.12", ] [[package]] name = "defmt-rtt" -version = "0.4.1" +version = "0.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bab697b3dbbc1750b7c8b821aa6f6e7f2480b47a99bc057a2ed7b170ebef0c51" +checksum = "c6eca0aae8aa2cf8333200ecbd236274697bc0a394765c858b3d9372eb1abcfa" dependencies = [ "critical-section", - "defmt", + "defmt 0.3.100", ] [[package]] name = "defmt-test" -version = "0.3.2" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "290966e8c38f94b11884877242de876280d0eab934900e9642d58868e77c5df1" +checksum = "1c1e67ff0e1c6b1a9540a1a3e04454658faacdd188c91987c444d56e469d7dea" dependencies = [ "cortex-m-rt", "cortex-m-semihosting", - "defmt", + "defmt 0.3.100", "defmt-test-macros", ] [[package]] name = "defmt-test-macros" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "984bc6eca246389726ac2826acc2488ca0fe5fcd6b8d9b48797021951d76a125" +checksum = "fe5520fd36862f281c026abeaab153ebbc001717c29a9b8e5ba9704d8f3a879d" dependencies = [ "proc-macro2", "quote", - "syn 2.0.85", + "syn 2.0.100", ] [[package]] name = "derive_more" -version = "0.99.18" +version = "0.99.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f33878137e4dafd7fa914ad4e259e18a4e8e532b9617a2d0150262bf53abfce" +checksum = "6edb4b64a43d977b8e99788fe3a04d483834fba1215a7e02caa415b626497f7f" dependencies = [ "convert_case", "proc-macro2", "quote", "rustc_version 0.4.1", - "syn 2.0.85", + "syn 2.0.100", ] +[[package]] +name = "either" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "48c757948c5ede0e46177b7add2e67155f70e33c07fea8284df6576da70b3719" + [[package]] name = "embedded-alloc" version = "0.5.1" @@ -289,13 +371,14 @@ dependencies = [ [[package]] name = "embedded-hal-bus" -version = "0.1.0" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "57b4e6ede84339ebdb418cd986e6320a34b017cdf99b5cc3efceec6450b06886" +checksum = "0d3980bf28e8577db59fe2bdb3df868a419469d2cecb363644eea2b6f7797669" dependencies = [ "critical-section", "embedded-hal 1.0.0", "embedded-hal-async", + "portable-atomic", ] [[package]] @@ -318,9 +401,9 @@ checksum = "a21dea9854beb860f3062d10228ce9b976da520a73474aed3171ec276bc0c032" [[package]] name = "equivalent" -version = "1.0.1" +version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" [[package]] name = "fdcan" @@ -342,6 +425,7 @@ version = "0.3.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17186ad64927d5ac8f02c1e77ccefa08ccd9eaa314d5a4772278aa204a22f7e7" dependencies = [ + "defmt 0.3.100", "gcd", ] @@ -384,6 +468,12 @@ dependencies = [ "typenum", ] +[[package]] +name = "glob" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a8d1add55171497b4705a648c6b583acafb01d58050a51727785f0b2c8e0a2b2" + [[package]] name = "hash32" version = "0.2.1" @@ -404,9 +494,9 @@ dependencies = [ [[package]] name = "hashbrown" -version = "0.15.0" +version = "0.15.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e087f84d4f86bf4b218b927129862374b72199ae7d8657835f1e89000eea4fb" +checksum = "bf151400ff0baff5465007dd2f3e717f3fe502074ca563069ce3a6629d07b289" [[package]] name = "heapless" @@ -428,26 +518,52 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" dependencies = [ + "defmt 0.3.100", "hash32 0.3.1", "stable_deref_trait", ] [[package]] name = "indexmap" -version = "2.6.0" +version = "2.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "707907fe3c25f5424cce2cb7e1cbcafee6bdbe735ca90ef77c29e84591e5b9da" +checksum = "cea70ddb795996207ad57735b50c5982d8844f38ba9ee5f1aedcfb708a2aa11e" dependencies = [ "equivalent", "hashbrown", ] +[[package]] +name = "itertools" +version = "0.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "413ee7dfc52ee1a4949ceeb7dbc8a33f2d6c088194d9f922fb8318faf1f01186" +dependencies = [ + "either", +] + [[package]] name = "lazy_static" version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" +[[package]] +name = "libc" +version = "0.2.172" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d750af042f7ef4f724306de029d18836c26c1765a54a6a3f094cbd23a7267ffa" + +[[package]] +name = "libloading" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc2f4eb4bc735547cfed7c0a4922cbd04a4655978c09b54f1f7b228750664c34" +dependencies = [ + "cfg-if", + "windows-targets", +] + [[package]] name = "linked_list_allocator" version = "0.10.5" @@ -466,9 +582,9 @@ dependencies = [ [[package]] name = "log" -version = "0.4.22" +version = "0.4.27" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7a70ba024b9dc04c27ea2f0c0548feb474ec5c54bba33a7f72f873a39d07b24" +checksum = "13dc2df351e3202783a1fe0d44375f7295ffb4049267b0f3018346dc122a1d94" [[package]] name = "m" @@ -493,6 +609,12 @@ dependencies = [ "madgwick", ] +[[package]] +name = "managed" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ca88d725a0a943b096803bd34e73a4437208b6077654cc4ecb2947a5f91618d" + [[package]] name = "mat" version = "0.2.0" @@ -524,7 +646,7 @@ dependencies = [ "proc-macro2", "quick-xml", "quote", - "thiserror", + "thiserror 1.0.69", ] [[package]] @@ -547,28 +669,36 @@ checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" [[package]] name = "messages" version = "0.1.0" -source = "git+https://github.com/uorocketry/messages#5b9b56735beba4449d72e5ec074389d3363775a7" +source = "git+https://github.com/uorocketry/messages#ed32222acc2072669eadc32eb6feb69a426439e3" dependencies = [ - "bitflags 2.6.0", + "bitflags 2.9.0", "chrono", - "defmt", + "defmt 0.3.100", "derive_more", "fugit", "heapless 0.7.17", "mavlink", "messages-proc-macros-lib", "serde", + "stm32h7xx-hal", + "ublox", ] [[package]] name = "messages-proc-macros-lib" version = "0.1.0" -source = "git+https://github.com/uorocketry/messages#5b9b56735beba4449d72e5ec074389d3363775a7" +source = "git+https://github.com/uorocketry/messages#ed32222acc2072669eadc32eb6feb69a426439e3" dependencies = [ "quote", "serde", ] +[[package]] +name = "minimal-lexical" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" + [[package]] name = "nb" version = "0.1.3" @@ -584,6 +714,16 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[package]] +name = "nom" +version = "7.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" +dependencies = [ + "memchr", + "minimal-lexical", +] + [[package]] name = "num-derive" version = "0.3.3" @@ -611,7 +751,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4047d9235d1423d66cc97da7d07eddb54d4f154d6c13805c6d0793956f4f25b0" dependencies = [ "cortex-m", - "defmt", + "defmt 0.3.100", ] [[package]] @@ -628,7 +768,7 @@ dependencies = [ "common-arm", "cortex-m", "cortex-m-rt", - "defmt", + "defmt 0.3.100", "defmt-rtt", "defmt-test", "embedded-alloc", @@ -641,14 +781,15 @@ dependencies = [ "rtic", "rtic-monotonics", "rtic-sync", + "sbg-rs", "stm32h7xx-hal", ] [[package]] name = "pin-project-lite" -version = "0.2.15" +version = "0.2.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "915a1e146535de9163f3987b8944ed8cf49a18bb0056bcebcdcece385cece4ff" +checksum = "3b3cff922bd51709b605d9ead9aa71031d81447142d828eb4a6eba76fe619f9b" [[package]] name = "pin-utils" @@ -658,51 +799,59 @@ checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" [[package]] name = "portable-atomic" -version = "1.9.0" +version = "1.11.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc9c68a3f6da06753e9335d63e27f6b9754dd1920d941135b7ea8224f141adb2" +checksum = "350e9b48cbc6b0e028b0473b114454c6316e57336ee184ceab6e53f72c178b3e" [[package]] name = "postcard" -version = "1.0.10" +version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f7f0a8d620d71c457dd1d47df76bb18960378da56af4527aaa10f515eee732e" +checksum = "170a2601f67cc9dba8edd8c4870b15f71a6a2dc196daec8c83f72b59dff628a8" dependencies = [ "cobs", - "defmt", + "defmt 0.3.100", "heapless 0.7.17", "serde", ] [[package]] -name = "proc-macro-error" -version = "1.0.4" +name = "prettyplease" +version = "0.2.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" +checksum = "664ec5419c51e34154eec046ebcba56312d5a2fc3b09a06da188e1ad21afadf6" +dependencies = [ + "proc-macro2", + "syn 2.0.100", +] + +[[package]] +name = "proc-macro-error-attr2" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96de42df36bb9bba5542fe9f1a054b8cc87e172759a1868aa05c1f3acc89dfc5" dependencies = [ - "proc-macro-error-attr", "proc-macro2", "quote", - "syn 1.0.109", - "version_check", ] [[package]] -name = "proc-macro-error-attr" -version = "1.0.4" +name = "proc-macro-error2" +version = "2.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" +checksum = "11ec05c52be0a07b08061f7dd003e7d7092e0472bc731b4af7bb1ef876109802" dependencies = [ + "proc-macro-error-attr2", "proc-macro2", "quote", - "version_check", + "syn 2.0.100", ] [[package]] name = "proc-macro2" -version = "1.0.89" +version = "1.0.95" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f139b0662de085916d1fb67d2b4169d1addddda1919e696f3252b740b629986e" +checksum = "02b3e5e68a3a1a02aad3ec490a98007cbc13c37cbe84a3cd7b8e406d76e7f778" dependencies = [ "unicode-ident", ] @@ -718,23 +867,52 @@ dependencies = [ [[package]] name = "quote" -version = "1.0.37" +version = "1.0.40" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af" +checksum = "1885c039570dc00dcb4ff087a89e185fd56bae234ddc7f056a945bf36467248d" dependencies = [ "proc-macro2", ] +[[package]] +name = "regex" +version = "1.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b544ef1b4eac5dc2db33ea63606ae9ffcfac26c1416a2806ae0bf5f56b201191" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "809e8dc61f6de73b46c85f4c96486310fe304c434cfa43669d7b40f711150908" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b15c43186be67a4fd63bee50d0303afffcef381492ebe2c5d87f324e1b8815c" + [[package]] name = "rtic" -version = "2.1.1" +version = "2.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c443db16326376bdd64377da268f6616d5f804aba8ce799bac7d1f7f244e9d51" +checksum = "401961431a1e491124cdd216a313fada2d395aa2b5bee2867c872fc8af7c1bc1" dependencies = [ - "atomic-polyfill", "bare-metal 1.0.0", "cortex-m", "critical-section", + "portable-atomic", "rtic-core", "rtic-macros", ] @@ -756,15 +934,15 @@ checksum = "d9369355b04d06a3780ec0f51ea2d225624db777acbc60abd8ca4832da5c1a42" [[package]] name = "rtic-macros" -version = "2.1.0" +version = "2.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "54053598ea24b1b74937724e366558412a1777eb2680b91ef646db540982789a" +checksum = "ac22ab522d80079b48f46ac66ded4d349e1adf81b52430d6a74faa3a7790ed80" dependencies = [ "indexmap", - "proc-macro-error", + "proc-macro-error2", "proc-macro2", "quote", - "syn 2.0.85", + "syn 2.0.100", ] [[package]] @@ -791,7 +969,7 @@ dependencies = [ "common-arm", "cortex-m", "cortex-m-rt", - "defmt", + "defmt 0.3.100", "defmt-rtt", "embedded-alloc", "fdcan", @@ -807,9 +985,9 @@ dependencies = [ [[package]] name = "rtic-sync" -version = "1.3.0" +version = "1.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49b1200137ccb2bf272a1801fa6e27264535facd356cb2c1d5bc8e12aa211bad" +checksum = "d0c3648d8ab27216a940ccfc1939479bfd14010fbae9f5b7706a5b3d30ef561f" dependencies = [ "critical-section", "embedded-hal 1.0.0", @@ -834,6 +1012,12 @@ dependencies = [ "rtic-common", ] +[[package]] +name = "rustc-hash" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "357703d41365b4b27c590e3ed91eabb1b663f07c4c084095e60cbed4362dff0d" + [[package]] name = "rustc_version" version = "0.2.3" @@ -849,7 +1033,20 @@ version = "0.4.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" dependencies = [ - "semver 1.0.23", + "semver 1.0.26", +] + +[[package]] +name = "sbg-rs" +version = "0.1.0" +dependencies = [ + "bindgen", + "bitflags 2.9.0", + "cmake", + "defmt 0.3.100", + "heapless 0.7.17", + "messages", + "serde", ] [[package]] @@ -869,9 +1066,9 @@ dependencies = [ [[package]] name = "semver" -version = "1.0.23" +version = "1.0.26" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b" +checksum = "56e6fa9c48d24d85fb3de5ad847117517440f6beceb7798af16b4a87d616b8d0" [[package]] name = "semver-parser" @@ -881,24 +1078,30 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.213" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3ea7893ff5e2466df8d720bb615088341b295f849602c6956047f8f80f0e9bc1" +checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.213" +version = "1.0.219" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7e85ad2009c50b58e87caa8cd6dac16bdf511bbfb7af6c33df902396aa480fa5" +checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" dependencies = [ "proc-macro2", "quote", - "syn 2.0.85", + "syn 2.0.100", ] +[[package]] +name = "shlex" +version = "1.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fda2ff0d084019ba4d7c6f371c95d8fd75ce3524c3cb8fb653a3023f6323e64" + [[package]] name = "simple-playground" version = "0.1.0" @@ -907,7 +1110,7 @@ dependencies = [ "common-arm", "cortex-m", "cortex-m-rt", - "defmt", + "defmt 0.3.100", "defmt-rtt", "embedded-alloc", "fdcan", @@ -921,6 +1124,20 @@ dependencies = [ "stm32h7xx-hal", ] +[[package]] +name = "smoltcp" +version = "0.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dad095989c1533c1c266d9b1e8d70a1329dd3723c3edac6d03bbd67e7bf6f4bb" +dependencies = [ + "bitflags 1.3.2", + "byteorder", + "cfg-if", + "defmt 0.3.100", + "heapless 0.8.0", + "managed", +] + [[package]] name = "spin" version = "0.9.8" @@ -966,20 +1183,23 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" version = "0.16.0" -source = "git+https://github.com/uorocketry/stm32h7xx-hal#412160269f1729d55bc52de17463695db2c6bc6c" +source = "git+https://github.com/uorocketry/stm32h7xx-hal#4911af6221101b88aff47915ea5b76c6e60b93e8" dependencies = [ "bare-metal 1.0.0", "cast", "chrono", "cortex-m", - "defmt", + "defmt 0.3.100", "embedded-dma", "embedded-hal 0.2.7", + "embedded-hal 1.0.0", "embedded-storage", "fdcan", "fugit", "nb 1.1.0", "paste", + "serde", + "smoltcp", "stm32h7", "void", ] @@ -997,9 +1217,9 @@ dependencies = [ [[package]] name = "syn" -version = "2.0.85" +version = "2.0.100" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5023162dfcd14ef8f32034d8bcd4cc5ddc61ef7a247c024a33e24e1f24d21b56" +checksum = "b09a44accad81e1ba1cd74a32461ba89dee89095ba17b32f5d03683b1b1fc2a0" dependencies = [ "proc-macro2", "quote", @@ -1008,35 +1228,78 @@ dependencies = [ [[package]] name = "thiserror" -version = "1.0.65" +version = "1.0.69" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5d11abd9594d9b38965ef50805c5e469ca9cc6f197f883f717e0269a3057b3d5" +checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52" dependencies = [ - "thiserror-impl", + "thiserror-impl 1.0.69", +] + +[[package]] +name = "thiserror" +version = "2.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "567b8a2dae586314f7be2a752ec7474332959c6460e02bde30d702a66d488708" +dependencies = [ + "thiserror-impl 2.0.12", ] [[package]] name = "thiserror-impl" -version = "1.0.65" +version = "1.0.69" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ae71770322cbd277e69d762a16c444af02aa0575ac0d174f0b9562d3b37f8602" +checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" dependencies = [ "proc-macro2", "quote", - "syn 2.0.85", + "syn 2.0.100", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f7cf42b4507d8ea322120659672cf1b9dbb93f8f2d4ecfd6e51350ff5b17a1d" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.100", ] [[package]] name = "typenum" -version = "1.17.0" +version = "1.18.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" +checksum = "1dccffe3ce07af9386bfd29e80c0ab1a8205a2fc34e4bcd40364df902cfa8f3f" + +[[package]] +name = "ublox" +version = "0.4.5" +source = "git+https://github.com/uorocketry/ublox#286d43a3ae9a3ba30a04be04c65bf89aca6b1bfe" +dependencies = [ + "bitflags 2.9.0", + "chrono", + "defmt 0.3.100", + "num-traits", + "serde", + "ublox_derive", +] + +[[package]] +name = "ublox_derive" +version = "0.1.0" +source = "git+https://github.com/uorocketry/ublox#286d43a3ae9a3ba30a04be04c65bf89aca6b1bfe" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] [[package]] name = "unicode-ident" -version = "1.0.13" +version = "1.0.18" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e91b56cd4cadaeb79bbf1a5645f6b4f8dc5bde8834ad5894a8db35fda9efa1fe" +checksum = "5a5f39404a5da50712a4c1eecf25e90dd62b613502b7e925fd4e4d19b5c96512" [[package]] name = "vcell" @@ -1044,12 +1307,6 @@ version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" -[[package]] -name = "version_check" -version = "0.9.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" - [[package]] name = "void" version = "1.0.2" @@ -1064,3 +1321,67 @@ checksum = "de437e2a6208b014ab52972a27e59b33fa2920d3e00fe05026167a1c509d19cc" dependencies = [ "vcell", ] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" diff --git a/Cargo.toml b/Cargo.toml index d55289a..7e879bc 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -7,6 +7,12 @@ members = ["phoenix", "examples/*", "crates/*"] # compile errors if built directly. default-members = ["phoenix", "examples/*"] +[profile.dev.package.sbg-rs] +opt-level = 0 + +[profile.release.package.sbg-rs] +opt-level = 0 + [workspace.dependencies.stm32h7xx-hal] git = "https://github.com/uorocketry/stm32h7xx-hal" # We use 35 even though we have the 33. diff --git a/crates/sbg-rs/.gitignore b/crates/sbg-rs/.gitignore new file mode 100644 index 0000000..62a677e --- /dev/null +++ b/crates/sbg-rs/.gitignore @@ -0,0 +1,2 @@ +# Ignore auto-generated bindings +src/bindings.rs diff --git a/crates/sbg-rs/Cargo.toml b/crates/sbg-rs/Cargo.toml new file mode 100644 index 0000000..2a000a8 --- /dev/null +++ b/crates/sbg-rs/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "sbg-rs" +version = "0.1.0" +edition = "2021" + +[dependencies] +bitflags = { version = "2.9", features = ["serde"] } +defmt = { workspace = true} +heapless = { workspace = true } +messages = { workspace = true } +serde = { workspace = true } + +[build-dependencies] +bindgen = "0.71" +cmake = "0.1" diff --git a/crates/sbg-rs/README.md b/crates/sbg-rs/README.md new file mode 100644 index 0000000..07ba616 --- /dev/null +++ b/crates/sbg-rs/README.md @@ -0,0 +1,13 @@ +# SBG-RS + +This library provides Rust bindings for the sbgECom C library. + +## Build + +The `build.rs` file deals with compiling and linking the sbgECom C library and generating a `src/bindings.rs` file. These bindings are generated for everything included in the `wrapper.h` file. + +To build this library, you must have [CMake](https://cmake.org/download/) installed on your system. Additionally, you must have the [GCC ARM Cross-Compiler](https://developer.arm.com/downloads/-/arm-gnu-toolchain-downloads) `(arm-none-eabi)` installed and added to path. The `toolchain.cmake` file specifies that the `arm-none-eabi` binaries should be used to build the library. + +For the binding generation stage, rust-bindgen requires that Clang be installed. See the [requirements file](https://github.com/rust-lang/rust-bindgen/blob/main/book/src/requirements.md) for bindgen. + +Once everything is setup, `cargo build` will automatically compile the sbgECom library and generate the bindings as necessary. It will also ensure the sbgECom library is linked after compiling the Rust source code. diff --git a/crates/sbg-rs/build.rs b/crates/sbg-rs/build.rs new file mode 100644 index 0000000..b8c63cb --- /dev/null +++ b/crates/sbg-rs/build.rs @@ -0,0 +1,102 @@ +use std::path::{Path, PathBuf}; + +/// The path to the sbgECom C library relative to this script. +const SGB_C_LIB_PATH: &str = "sbgECom"; +/// The name of the built sbgECom library to link against. +const SBG_C_LIB_NAME: &str = "sbgECom"; +/// The header file for bindgen to create Rust bindings for. +const BINDGEN_HEADER: &str = "wrapper.h"; + +/// Speficy the CMake generator based on the target platform. +#[cfg(windows)] +const GENERATOR: &str = "MinGW Makefiles"; +#[cfg(not(windows))] +const GENERATOR: &str = "Unix Makefiles"; + +fn main() { + build_c_library(); + create_rust_bindings(); +} + +/// Builds the sbgECom C library using CMake. +fn build_c_library() { + // Get the toolchain.cmake file for cross-compilation + let toolchain_path = normalize_path(Path::new("toolchain.cmake").to_path_buf()); + + // Build the SBG C library with CMake + let dst = cmake::Config::new(SGB_C_LIB_PATH) + .generator(GENERATOR) + .define("CMAKE_TOOLCHAIN_FILE", toolchain_path) + .build(); + + // Special instructions for cargo: + // - cargo:rustc-link-search={path} tells cargo where to find the library + // - cargo:rustc-link-lib=static={name} tells cargo to link the library as static + // - cargo:rerun-if-changed={path} tells cargo to rerun the build script if the library changes + println!("cargo:rustc-link-search={}", dst.join("lib").display()); + println!("cargo:rustc-link-lib=static={}", SBG_C_LIB_NAME); + println!("cargo:rerun-if-changed={}", SBG_C_LIB_NAME); +} + +/// Generates Rust bindings for the SBG C library using bindgen. +fn create_rust_bindings() { + // The sysroot path specifies where to find the cross-compilation headers + let sysroot_path = { + // Run `arm-none-eabi-gcc -print-sysroot` to get the sysroot path. + // This is the binary used to cross-compile the C library thus + // the headers provided should suffice to create the bindings. + let output = std::process::Command::new("arm-none-eabi-gcc") + .arg("-print-sysroot") + .output() + .expect("Failed to execute arm-none-eabi-gcc"); + if output.status.success() { + String::from_utf8(output.stdout) + .expect("Invalid UTF-8 in sysroot path") + .trim() + .to_string() + } else { + panic!("Failed to detect sysroot: {}", String::from_utf8_lossy(&output.stderr)); + } + }; + let sysroot_path = normalize_path(Path::new(&sysroot_path).to_path_buf()); + + // Include the relevant sbgECom paths so bindgen can find the headers + let common_include_path = normalize_path(Path::new("sbgECom").join("common").to_path_buf()); + let src_include_path = normalize_path(Path::new("sbgECom").join("src").to_path_buf()); + + bindgen::Builder::default() + // Specify the header file to generate bindings for + .header(BINDGEN_HEADER) + .clang_args(&[ + &format!("--sysroot={}", sysroot_path), + &format!("-I{}", common_include_path), + &format!("-I{}", src_include_path), + ]) + // Use core rather than std as we are compiling for a no_std environment + .use_core() + // Register cargo callbacks to rebuild bindings when the header changes + .parse_callbacks(Box::new(bindgen::CargoCallbacks::new())) + // Generate the bindings + .generate() + .expect("Unable to generate bindings") + // Write the bindings to src/bindings.rs + .write_to_file(Path::new("src").join("bindings.rs")) + .expect("Couldn't write bindings!"); +} + +/// Normalizes a path and returns it as a String. +/// Equivalent to [`PathBuf::canonicalize`] in most instances. +/// On Windows, it will additionally remove any \\?\ prefixes. +fn normalize_path(p: PathBuf) -> String { + const VERBATIM_PREFIX: &str = r#"\\?\"#; + let p = p.canonicalize() + .unwrap() + .display() + .to_string(); + + if cfg!(windows) && p.starts_with(VERBATIM_PREFIX) { + p[VERBATIM_PREFIX.len()..].to_string() + } else { + p + } +} diff --git a/crates/sbg-rs/sbgECom/CHANGELOG.md b/crates/sbg-rs/sbgECom/CHANGELOG.md new file mode 100644 index 0000000..cb3a339 --- /dev/null +++ b/crates/sbg-rs/sbgECom/CHANGELOG.md @@ -0,0 +1,303 @@ +# Change Log +This change log lists all modifications for each sbgECom library release. + +sbgECom C library change log issued on: 2022-11-15 +Copyright (C) 2022, SBG Systems SAS. All rights reserved. + +## Release Summary + +The sbgECom 3.2.4011-stable release adds support to latest High Performance INS firmware 4.2 and ELLIPSE firmware 2.5 +New configurations and output logs have been added such as `SBG_ECOM_LOG_GPS#_SAT` binary log. + +It also adds two new commands sbgEComCmdApiGet and sbgEComCmdApiPost to support the new [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) over serial interfaces. + +A new extended sbgECom frame format has been introduced to support large payloads. +This new format is fully backward compatible and your code, as well as older firmware, should still work with this new sbgECom implementation. + +New tools and examples have been added to ease product evaluation and integration. + +This release also improves the overall code quality, documentation and examples. + +## Import Highlights for this release + +Please find below, the main improvements and modifications: + - Support for latest firmware + - Support for new sbgInsRestApi GET/POST commands + - New sbgEComBasicLogger tool to convert logs to CSV files + - New sbgEComApi tool to use the sbgInsRestApi from command line + - Improved sbgECom frame for large payloads + - Improved ELLIPSE on-board magnetic calibration example + - Improve code quality + - Improved Doxygen documentation + - Improved CMake & GitHub support + +## Release - 3.2.4011-stable + +### New Features + - [SBGECOM-306] - Added built in CHANGELOG file in the repository + - [SBGECOM-307] - Add support for event markers in sbgBasicLogger + - [SBGECOM-309] - Add SBG_ECOM_LOG_GPS#_SAT message containing satellite information + - [SBGECOM-310] - Add NMEA message GPROT + - [SBGECOM-312] - sbgBasicLogger: handle sbgEComLogSat messages + - [SBGECOM-313] - Added a static motion profile for 27/7 operations on ELLIPSE + - [SBGECOM-314] - Added pedestrian motion profile that uses foot odometry + - [SBGECOM-317] - Added search capabilities for Doxygen documentation + - [SBGECOM-319] - Add support for the Teledyne Wayfinder DVL protocol + - [SBGECOM-323] - Add a new PSBGA NMEA message that output the INS atitude + - [SBGECOM-326] - Add NMEA GSV output message with satellite in view information + - [SBGECOM-327] - Added L-Band support for GNSS receiver channel reporting + +### Improvements +- [SBGECOM-311] - Improve public CMakeLists +- [SBGECOM-315] - Enable sbgEComApi users to provide command execution parameters +- [SBGECOM-322] - Added Truck and Railway motion profiles +- [SBGECOM-325] - sbgBasicLogger: Improve project code and build script for code source public release +- [SBGECOM-328] - Improved doxygen documentation + +### Bug Fixes +- [SBGECOM-316] - Fix ELLIPSE settings import that was not working +- [SBGECOM-324] - Fix invalid time-out errors when calling sbgEComReceiveCmd2 + +## Release - 3.1.2358-stable + +### New Features + - [SBGECOM-225] - Added KMB binary sensor output log for High Performance INS + - [SBGECOM-226] - Add a new log SBG_ECOM_LOG_RTCM_STREAM + - [SBGECOM-228] - Add new commands to implement GET/POST over sbgECom protocol + - [SBGECOM-229] - Now ELLIPSE-N and D can accept RTCM corrections over Port A + - [SBGECOM-231] - Add support for NMEA PPS frames + - [SBGECOM-253] - Add specific NMEA like output log WASSP + - [SBGECOM-279] - Add in SBG_ECOM_CMD_ADVANCED_CONF a GNSS option bitmask + - [SBGECOM-281] - Add a new sbgBasicLogger command line tool + - [SBGECOM-282] - Add a sbgEComApi CLI tool to access sbgInsRestApi over sbgECom + - [SBGECOM-283] - Add a minimal sbgECom example for PULSE IMU + - [SBGECOM-285] - Add in SBG_ECOM_CMD_ADVANCED_CONF an option to always output time in NMEA messages + - [SBGECOM-286] - Add in SBG_ECOM_CMD_ADVANCED_CONF an option select standard or extended NMEA mode + +### Improvements + - [SBGECOM-181] - Reduce stack usage and use new sbgEComReceiveCmd2 + - [SBGECOM-220] - Updated license to MIT + - [SBGECOM-232] - Updated protocol to support large transfer using multiple pages + - [SBGECOM-287] - Updated sbgEComSetReceiveLogCallback to remove the unused SbgErrorCode return + - [SBGECOM-301] - Improved ELLIPSE onboard magnetic calibration code example + - [SBGECOM-302] - Added new sbgEComPurgeIncoming method to discard rx data + - [SBGECOM-303] - Improved examples, they now use simple CLI arguments + +### Bug Fixes + - [SBGECOM-265] - Add missing 25ms period to output log message definitions + - [SBGECOM-267] - Add missing 40ms period to output log message definitions + - [SBGECOM-284] - Receive a command must not be blocking when timeout = 0 + - [SBGECOM-289] - Fix SBG_ECAN_MSG_GPS#_ALT NUM_SV & DIFF_CORR DBC/DBF definitions + +## Release - 2.0.4536-stable + +### New Features + - [SBGECOM-180] - Added NMEA GGK message output + - [SBGECOM-191] - Added in SBG_ECOM_LOG_GPS#_POS status report for all GNSS constellations + - [SBGECOM-194] - Add specific SBG_ECOM_THIRD_PARTY_ADA_01 output log + - [SBGECOM-208] - Added a README.md file with migration guidelines + - [SBGECOM-216] - Add Cobham SBG_ECOM_THIRD_PARTY_AT_ITINS output log support + - [SBGECOM-219] - Added CAN dbc and BusMaster definitions in sbgECom project +### Improvements + - [SBGECOM-188] - Updated SBG_ECAN_MSG_ODO_VELOCITY from float field to integers + - [SBGECOM-196] - Added status field for SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV (0x220) message + - [SBGECOM-199] - Simplified SbgEComGnssModelsStdIds enum for ELLIPSE-N and ELLIPSE-D + - [SBGECOM-204] - Renamed SBG_ECOM_GNSS_MODEL_UBLOX_GPS_GLONASS to SBG_ECOM_GNSS_MODEL_INTERNAL + - [SBGECOM-207] - Reworked motion profile / aiding equipments errors models set/get API (removed SbgEComModelInfo) + - [SBGECOM-209] - Reworked and simplified GNSS model ids to comply with sbgECom 2.x + - [SBGECOM-212] - Added in SBG_ECOM_CMD_FEATURES gnss firmware version field +### Removed Features + - [SBGECOM-200] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_DYNAMICS, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - [SBGECOM-201] - Removed SBG_ECOM_GNSS_MODEL_ELLIPSE_D_INTERNAL, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - [SBGECOM-202] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - [SBGECOM-203] - Removed SBG_ECOM_GNSS_MODEL_UBLOX_LOW_SPEED, use SBG_ECOM_GNSS_MODEL_INTERNAL instead + - [SBGECOM-206] - Removed deprecated methods sbgEComCmdGnss1GetLeverArmAlignment & sbgEComCmdGnss1SetLeverArmAlignment + - [SBGECOM-211] - Removed legacy IG-500 protocol support + +## Release - 1.11.920-stable + +### New Features + - [SBGECOM-123] - Implement DVL aiding configuration SBG_ECOM_CMD_DVL_#### + - [SBGECOM-126] - Implement AirData aiding configuration SBG_ECOM_CMD_AIRDATA_#### + - [SBGECOM-135] - Add configuration commands for CAN odometer support + - [SBGECOM-136] - Add sbgECom Log Event Output useful for virtual odometer + - [SBGECOM-137] - Add SBG_ECOM_LOG_DEPTH / SBG_ECAN_MSG_DEPTH_INFO/ALTITUDE output logs + - [SBGECOM-140] - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for AirData support + - [SBGECOM-141] - Add airDataInput demo project to show external sbgECom AirData aiding + - [SBGECOM-142] - Updated SBG_ECOM_LOG_PRESSURE to SBG_ECOM_LOG_AIR_DATA with airspeed + - [SBGECOM-143] - Updated SBG_ECAN_MSG_PRESSURE to SBG_ECAN_MSG_AIR_DATA with airspeed + - [SBGECOM-156] - Add baseline length field in SBG_ECOM_LOG_GPS#_HDT log + - [SBGECOM-162] - Add SBG_ECOM_LOG_DIAG message to send text + - [SBGECOM-163] - Add CAN output message with vehicle body velocity + - [SBGECOM-164] - Add CAN output message with track, slip and curvature indications + - [SBGECOM-168] - Add INDYN NMEA like message output for marine applications + - [SBGECOM-170] - Add SBG_ECOM_CMD_GNSS_1_INSTALLATION command to set/get GNSS lever arm + - [SBGECOM-173] - Add compatibility support Crossbow AHRS500 series + - [SBGECOM-174] - Basic Logger: Add support for error log messages + - [SBGECOM-175] - Basic Logger: Add support for UDP interfaces + +### Improvements + - [SBGECOM-132] - Updated SBG_ECOM_CMD_AIDING_ASSIGNMENT command for DVL support + - [SBGECOM-133] - Renamed DVL standard deviation as quality indicator in DVL log structure + - [SBGECOM-144] - Converted SBG_ECOM_AIDING_EM_LOG_RECV to SBG_ECOM_AIDING_DEPTH_RECV + - [SBGECOM-145] - Renamed SBG_ECOM_AIDING_PRESSURE_RECV to SBG_ECOM_AIDING_AIR_DATA_RECV + - [SBGECOM-146] - Renamed SBG_ECOM_SOL_PRESSURE_USED to SBG_ECOM_SOL_AIR_DATA_USED + - [SBGECOM-147] - Added SBG_ECOM_SOL_DEPTH_USED in EKF solution status + - [SBGECOM-165] - Updated CAN log default IDs for Ship Motion HP from 0x210 to 0x14A + - [SBGECOM-169] - Updated SBG_ECOM_THIRD_PARTY_IXBLUE_XXX logs to comply with naming conventions + - [SBGECOM-176] - Minor code cleanup and removed useless defines + - [SBGECOM-177] - Updated all C types to use standard ones uint32 -> uint32_t + - [SBGECOM-178] - Updated common lib code with improved organization + +### Removed Features + - [SBGECOM-100] - Removed deprecated sbgEComSetReceiveCallback method + - [SBGECOM-101] - Removed deprecated sbgEComCmdGnss1SetModel method + - [SBGECOM-102] - Removed deprecated sbgEComCmdSensorSetMotionProfile method + - [SBGECOM-103] - Removed deprecated sbgEComCmdMagSetModel method + - [SBGECOM-171] - SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT deprecated and replaced by SBG_ECOM_CMD_GNSS_1_INSTALLATION + +## Release - 1.10.3692-stable + +### New Features + - [SBGECOM-115] - Add Septentrio Internal GNSS model for new Ellipse D + - [SBGECOM-117] - Added getters for delta a angle / velocity / temperature for IMU Short log + - [SBGECOM-119] - Added new Swell Mode Ship Motion status flags + +### Improvements + - [SBGECOM-118] - Improved sbgEComStartFrameGeneration & sbgEComFinalizeFrameGeneration methods + - [SBGECOM-122] - Increased time out for sbgEComCmdLicenseApply to support new ELLIPSE-D internal GNSS + +## Release - 1.9.706-stable + +### New Features + - [SBGECOM-110] - Added DOLOG HRP proprietary message + - [SBGECOM-111] - Added a new short IMU log message also used for post processing + - [SBGECOM-112] - Add a heavy machinery motion profile definition for Ellipse series + +### Bug Fixes + - [SBGECOM-113] - Fixed invalid SBG_ECOM_CAN_RX/TX_OK comments + +## Release - 1.8.2916-stable + +### New Features + - [SBGECOM-95] - Added GPS number of SV used and diff corrections details in sbgECan protocol + - [SBGECOM-105] - Add compatibility with SBG_ECOM_CMD_VALIDITY_THRESHOLDS command + - [SBGECOM-108] - Added support for aiding assignment on Port E for ELLIPSE-E and N + +### Improvements + - [SBGECOM-91] - Added sbgEComSetCmdTrialsAndTimeOut to setup the number of trials and default time out for commands + +## Release - 1.7.235-stable + +### New Features + - [SBGECOM-89] - Implement Kongsberg Binary 26 message output + - [SBGECOM-87] - Add an uptime indication in SBG_ECOM_LOG_STATUS + - [SBGECOM-86] - Added the command SBG_ECOM_CMD_ETHERNET_INFO to current device IP address + - [SBGECOM-85] - Added command SBG_ECOM_CMD_ETHERNET_CONF to define / retrieve the Ethernet configuration + - [SBGECOM-77] - Add new output log class for NMEA proprietary messages + - [SBGECOM-75] - Added UAV motion profile definition (for low dynamic rotary wing UAV applications) + +### Improvements + - [SBGECOM-83] - Better use of size_t type instead of uint32 to comply with C standard and 64 bit platforms + - [SBGECOM-84] - Updated sbgCommonLib to latest revision + +### Removed Features + - [SBGECOM-79] - Removed deprecated "course" from the GNSS configurable aiding sources + +## Release - 1.5.209-stable + +### New Features + - [SBGECOM-72] - Added proprietary NMEA message PASHR for roll, pitch, heading, heave + - [SBGECOM-70] - Added SBG Proprietary NMEA message with acceleration and angular rate + - [SBGECOM-68] - Added SBG_ECOM_OUTPUT_MODE_DIV_5 flag for 40 Hz output + - [SBGECOM-66] - Added SBG_ECOM_GENERAL_CPU_OK status flag + - [SBGECOM-65] - Added sbgEComHandleOneLog method to return even if more logs are available + - [SBGECOM-64] - Added sbgEComSendAck method + - [SBGECOM-62] - Added sbgEComStartFrameGeneration and sbgEComFinalizeFrameGeneration methods + - [SBGECOM-59] - Added sbgECom log generation code + - [SBGECOM-57] - Added SBG_ECOM_LOG_FAST_IMU_DATA message definition + - [SBGECOM-40] - Added KVH third party output format id (SBG_ECOM_THIRD_PARTY_KVH) + +### Improvements + - [SBGECOM-74] - Switched unix projects to CMake + - [SBGECOM-73] - Added examples into the sbgECom + - [SBGECOM-63] - Updated sbgCommonLib to latest revision + - [SBGECOM-61] - Use stream buffer instead of basic buffer for sbgECom log parsing + - [SBGECOM-51] - Added SBF (Septentrio) protocol support on Ellipse-E + - [SBGECOM-50] - Added direct PPS from internal GNSS Sync Output (SBG_ECOM_CMD_SYNC_OUT_CONF) + - [SBGECOM-47] - Made the sbgECom 64 bit compatible + - [SBGECOM-46] - Switched project files to Visual Studio 2013 + - [SBGECOM-45] - Added a new callback method (sbgEComSetReceiveLogCallback) and deprecated the old one (sbgEComSetReceiveCallback) + - [SBGECOM-42] - Improved handling of MSG and CLASS fields in low level protocol functions + +### Bug Fixes + - [SBGECOM-71] - Fixed sbgEComCmdGetInfo incorrect error code return when an invalid payload is received + - [SBGECOM-44] - Fixed Various incompatibilities in Big Endian platforms + - [SBGECOM-43] - Added output of NACK reasons in sbgECom configuration commands using the return code + +### Removed Features + - [SBGECOM-60] - Removed Ship Motion 1,2,3 and Ship Motion HP 1,2,3 due to new deported heave concepts + +## Release - 1.4.3239-stable + +### New Features + - [SBGECOM-28] - Added differential correction age, diff base id and num sv to the SBG_ECOM_LOG_GPS#_POS + - [SBGECOM-29] - Added GNSS raw data log for the second GNSS receiver + - [SBGECOM-30] - Added official support for Ellipse additional output interfaces PORT C and PORT E + - [SBGECOM-33] - Added big/little endian support for stream buffer + - [SBGECOM-34] - Added sbgPlatform.h file to setup platform specific configuration such as endianness + +### Improvements + - [SBGECOM-7] - Added support for both little and big endian platforms + - [SBGECOM-32] - Improved stream buffer error handling + - [SBGECOM-36] - Improved File naming and overall library organization + - [SBGECOM-37] - Modified firmware and software version numbering scheme + - [SBGECOM-38] - Increased raw GPS data buffer size from 2048 to 4096 bytes + +### Bug Fixes + - [SBGECOM-21] - Fixed SBG_ECOM_ETH#_RX_OK and SBG_ECOM_ETH#_TX_OK status definitions + - [SBGECOM-27] - Changed sbgEComHandle behavior so the error returned by receive call back is taken into account + - [SBGECOM-35] - Fixed improper comments in some configuration structures + +## Release - 1.3 + +### New Features + - [SBGECOM-10] - Added sbgInterfaceChangeBaudrate for both windows and unix platforms + - [SBGECOM-19] - Added SBG_ECOM_LOG_PRESSURE log for depth sensors and altimeters + - [SBGECOM-25] - Added support for Ellipse series + - [SBGECOM-26] - Added SBG_ECOM_LOG_USBL log for USBL aiding equipments (beta) + +### Improvements + - [SBGECOM-18] - Fixed Typos in GPS pos, Vel and Hdt Fix Status + - [SBGECOM-20] - Better error checking for sbgStreamBuffer with new method sbgStreamBufferGetLastError + - [SBGECOM-22] - Added UTC & Clock status to the binary log SbgLogUtcData + - [SBGECOM-23] - Added Solution status to the binary log SbgLogEkfEuler, SbgLogEkfQuat, SbgLogEkfNav + - [SBGECOM-24] - Added time stamp to the log SBG_ECOM_LOG_MAG_CALIB + +## Release - 1.2 + +### New Features + - [SBGECOM-14] - Added SBG_ECOM_LOG_SHIP_MOTION_HP logs for delayed heave output + - [SBGECOM-15] - Added sbgInterfaceSerialChangeBaudrate method to change the serial interface baud rate + - [SBGECOM-17] - Added SBG_ECOM_POS_FIXED / SBG_ECAN_POS_FIXED position type for GPS + +### Improvements + - [SBGECOM-13] - Updated SBG_ECOM_LOG_SHIP_MOTION_XXX logs to add velocity and status data + - [SBGECOM-16] - Changed GPS OmniStar solution type to PPP ones for better compatibility with third party GPS + +### Removed Features + - [SBGECOM-11] - Removed heave status field from SBG_ECOM_LOG_STATUS log + +## Release - 1.1 + +### New Features + - [SBGECOM-1] - Added output log for DVL support + - [SBGECOM-3] - Added output for GPS 1 raw data in order to support post processing + - [SBGECOM-4] - Added event markers logs support + - [SBGECOM-6] - Added Unix support and build script + - [SBGECOM-8] - Added sbgEComReceiveAnyCmd method that return any received command that is not an output log + - [SBGECOM-9] - Added settings import and export command + +### Improvements + - [SBGECOM-2] - Added pitch information in the SbgLogGpsHdt GPS true heading log + - [SBGECOM-5] - Now sbgEComProtocolReceive method returns the received command even if the CRC is not valid diff --git a/crates/sbg-rs/sbgECom/CMakeLists.txt b/crates/sbg-rs/sbgECom/CMakeLists.txt new file mode 100644 index 0000000..eaae029 --- /dev/null +++ b/crates/sbg-rs/sbgECom/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.18 FATAL_ERROR) + +set(CMAKE_BUILD_TYPE Release CACHE STRING "build type") +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +project(sbgECom) + +# +# Project configuration +# +set(CMAKE_C_STANDARD 99) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +option(BUILD_EXAMPLES "Build examples" OFF) +option(BUILD_TOOLS "Build tools" OFF) + +# +# sbgECom +# +add_library(${PROJECT_NAME} STATIC) + +file(GLOB_RECURSE COMMON_SRC ${PROJECT_SOURCE_DIR}/common/*.c) +file(GLOB_RECURSE ECOM_SRC ${PROJECT_SOURCE_DIR}/src/*.c) + +# Exclude platform specific files +if (NOT MSVC) + list(REMOVE_ITEM COMMON_SRC ${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialWin.c) +else () + list(REMOVE_ITEM COMMON_SRC ${PROJECT_SOURCE_DIR}/common/interfaces/sbgInterfaceSerialUnix.c) +endif() + +target_sources(${PROJECT_NAME} PRIVATE ${COMMON_SRC} ${ECOM_SRC}) + +target_include_directories(${PROJECT_NAME} + PRIVATE + ${PROJECT_SOURCE_DIR}/src + ${PROJECT_SOURCE_DIR}/common + INTERFACE + $ + $ + $) + +target_compile_definitions(${PROJECT_NAME} PUBLIC SBG_COMMON_STATIC_USE) + +if (MSVC) + target_compile_definitions(${PROJECT_NAME} PUBLIC _CRT_SECURE_NO_WARNINGS) + target_link_libraries(${PROJECT_NAME} PUBLIC Ws2_32) +endif() + +# +# Tools +# +if (BUILD_TOOLS) + include(FetchContent) + + FetchContent_Declare(argtable3 + GIT_REPOSITORY https://github.com/argtable/argtable3.git + GIT_TAG v3.1.5.1c1bb23 + ) + + FetchContent_GetProperties(argtable3) + + if (NOT argtable3_POPULATED) + FetchContent_Populate(argtable3) + add_subdirectory(${argtable3_SOURCE_DIR} ${argtable3_BINARY_DIR} EXCLUDE_FROM_ALL) + endif() + + add_executable(sbgBasicLogger + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/main.c + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c + ${PROJECT_SOURCE_DIR}/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c + ) + target_include_directories(sbgBasicLogger PRIVATE ${argtable3_SOURCE_DIR}/src) + target_link_libraries(sbgBasicLogger ${PROJECT_NAME} argtable3_static) + install(TARGETS sbgBasicLogger DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) + install(FILES tools/sbgBasicLogger/README.md DESTINATION bin/tools/sbgBasicLogger COMPONENT executables) + + add_executable(sbgEComApi ${PROJECT_SOURCE_DIR}/tools/sbgEComApi/src/main.c) + target_include_directories(sbgEComApi PRIVATE ${argtable3_SOURCE_DIR}/src) + target_link_libraries(sbgEComApi ${PROJECT_NAME} argtable3_static) + install(TARGETS sbgEComApi DESTINATION bin/tools/sbgEComApi COMPONENT executables) + install(FILES tools/sbgEComApi/README.md DESTINATION bin/tools/sbgEComApi COMPONENT executables) +endif(BUILD_TOOLS) + +# +# Install +# +install(TARGETS ${PROJECT_NAME} EXPORT sbgEComTargets) +install(EXPORT sbgEComTargets + FILE sbgEComTargets.cmake + NAMESPACE sbg:: + DESTINATION lib/cmake/sbg) +install(DIRECTORY common/ TYPE INCLUDE FILES_MATCHING REGEX ".*\\.h") +install(DIRECTORY src/ TYPE INCLUDE FILES_MATCHING REGEX ".*\\.h") diff --git a/crates/sbg-rs/sbgECom/LICENSE.md b/crates/sbg-rs/sbgECom/LICENSE.md new file mode 100644 index 0000000..0117237 --- /dev/null +++ b/crates/sbg-rs/sbgECom/LICENSE.md @@ -0,0 +1,22 @@ +# License +The MIT License (MIT) + +Copyright (C) 2022, SBG Systems SAS. All rights reserved. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. \ No newline at end of file diff --git a/crates/sbg-rs/sbgECom/README.md b/crates/sbg-rs/sbgECom/README.md new file mode 100644 index 0000000..4d871a8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/README.md @@ -0,0 +1,123 @@ +# sbgECom Library +sbgECom is a C library provided under the MIT License and used to interface easily [SBG Systems](https://www.sbg-systems.com/) IMU, AHRS and INS to your application. + +This library has been designed to be platform and OS independent and can safely be used on small micro-controller or larger multi-core CPUs. +This package contains the following items: + - sbgECom library full C source code + - Doxygen library source code documentation + - Examples C source code and binaries to quickly start using the sbgECom library + - Several useful tools provided as binary package + +The sbgECom library and examples can be compiled on any platform using CMake. +The pre-compiled sbgECom examples and tools are 64 bits binaries available on Windows, Linux and Mac OS X platforms. + +The library is written and maintained by SBG Systems SAS. You can contact the support@sbg-systems.com for if you need assistance. + +# Documentation + +You can access the full online sbgECom Doxygen documentation [here](https://developer.sbg-systems.com/sbgECom/). +You should also read the SBG Systems [Support Center](https://support.sbg-systems.com) to quickly start using and integrating your products. +Please also have a look at the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/) documentation that is used to configure High Performance INS products. + +# Platform support +The library has been designed to be easily ported to any platform by just providing/implementing a few low level methods and some configuration: + - The platform configuration is set in `common/sbgConfig.h` + - In the file `common/platform/sbgPlatform.c` please provide _sbgGetTime_, _sbgSleep_ and _sbgPlatformDebugLogMsg_ + - In the directory `common/interfaces/` you can provide custom open/read/write implementations for serial communications + +# Building sbgECom Library +The sbgECom library and code examples are very easy to compile on any platform using CMake. +The library has no third party library dependencies making it very easy to build. + +SBG Systems doesn't provide the sbgECom as a pre-compiled library for obvious and good reasons. + +## Dependencies +SBG Systems has validated the following toolchain: +- \>= CMake 3.0 +- \>= GNU GCC 8 (any platform) +- \>= AppleClang 13 (Mac OS X) +- \>= Visual Studio 2015 or MSBuild equivalent (Windows) + +## Building sbgECom +To build the sbgECom static library, the C example and the command line tools go to the sbgECom library folder and type the following commands: + +```sh +cmake -Bbuild -DBUILD_EXAMPLES=ON -DBUILD_TOOLS=ON +cmake --build build +``` + +You should find the sbgECom static library, examples and tools binaries in the `build/Debug` folder. + +# Code Examples +SBG Systems provides several and simple C code examples to quickly use the sbgECom library. +You can find both the source code as well as a binary for each example. + +All examples source code are located in the `examples` directory. You can find pre-compiled 64 bits binaries in the `bin/examples` folder. + +## Ellipse Minimal +Simple C example to illustrate how to connect and read data from an ELLIPSE using the sbgECom library. + +You can test this example using the command below: + +```sh +ellipseMinimal COM4 115200 +``` + +## High Performance INS Minimal +Simple C example to illustrate how to read data from an High Performance INS over an UDP connection and using the sbgECom library. + +You can test this example using the command below. The INS ip address is *192.168.1.1* and send logs on the UDP port *1234*: + +```sh +hpInsMinimal COM4 192.168.1.1 5678 1234 +``` + +## Pulse Minimal +Simple C example to illustrate how to connect and read data from a PULSE IMU using the sbgECom library. + +You can test this example using the command below: + +```sh +pulseMinimal COM4 921600 +``` + +## Ellipse On Board Magnetic Calibration +Simple C example to illustrate how to use the ELLIPSE on board magnetic calibration algorithms. + +You can test this example using the command below: + +```sh +ellipseOnBoardMagCalib COM4 115200 +``` + +## Air Date Input +Simple C example to illustrate how to send air date aiding measurements to an ELLIPSE using the sbgECom library. + +You can test this example using the command below: + +```sh +airDataInput COM4 115200 +``` + +# Command Line Tools +SBG Systems offers two very useful tools to ease evaluation and integration. You can find the C source code for each tool in the `tools` directory. +The `bin/tools` directory contains pre-compiled 64 bits binaries for Windows, Linux and Mac OS X platforms. + +> Please read the dedicated README.md files provided with each tool. + +## sbgBasicLogger +Simply parse sbgECom logs from a serial or ethernet interface and write log content to CSV like files. +This tool can also read sbgECom logs from a binary file making it very interesting to convert ELLIPSE binary streams to easy to use text files. + +## sbgEComApi +Easily access sbgInsRest API configuration over a serial or UDP interface. You can execute GET and POST queries using simple to use command lines arguments. +This tool is perfect if you would like to setup a High Performance INS product over a serial or ethernet interface and using only bash scripts for example. + +# CAN messages +SBG Systems provides a DBC and BusMaster CAN messages database definition to quickly interface your product with a CAN logger and application. +You can find these CAN database in the `can` directory + + + +Read Next: [Migrations](doc/migrations.md) + diff --git a/crates/sbg-rs/sbgECom/common/crc/sbgCrc.c b/crates/sbg-rs/sbgECom/common/crc/sbgCrc.c new file mode 100644 index 0000000..3b5c5f6 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/crc/sbgCrc.c @@ -0,0 +1,271 @@ +#include "sbgCrc.h" + +//----------------------------------------------------------------------// +//- Static global CRC tables -// +//----------------------------------------------------------------------// + +/*!< CRC table used to compute a 16 bit CRC with the polynom 0x8408. */ +static const uint16_t crc16LookupTable[256] = { + 0x0000,0x1189,0x2312,0x329B,0x4624,0x57AD,0x6536,0x74BF,0x8C48,0x9DC1,0xAF5A,0xBED3,0xCA6C,0xDBE5,0xE97E,0xF8F7, + 0x1081,0x0108,0x3393,0x221A,0x56A5,0x472C,0x75B7,0x643E,0x9CC9,0x8D40,0xBFDB,0xAE52,0xDAED,0xCB64,0xF9FF,0xE876, + 0x2102,0x308B,0x0210,0x1399,0x6726,0x76AF,0x4434,0x55BD,0xAD4A,0xBCC3,0x8E58,0x9FD1,0xEB6E,0xFAE7,0xC87C,0xD9F5, + 0x3183,0x200A,0x1291,0x0318,0x77A7,0x662E,0x54B5,0x453C,0xBDCB,0xAC42,0x9ED9,0x8F50,0xFBEF,0xEA66,0xD8FD,0xC974, + 0x4204,0x538D,0x6116,0x709F,0x0420,0x15A9,0x2732,0x36BB,0xCE4C,0xDFC5,0xED5E,0xFCD7,0x8868,0x99E1,0xAB7A,0xBAF3, + 0x5285,0x430C,0x7197,0x601E,0x14A1,0x0528,0x37B3,0x263A,0xDECD,0xCF44,0xFDDF,0xEC56,0x98E9,0x8960,0xBBFB,0xAA72, + 0x6306,0x728F,0x4014,0x519D,0x2522,0x34AB,0x0630,0x17B9,0xEF4E,0xFEC7,0xCC5C,0xDDD5,0xA96A,0xB8E3,0x8A78,0x9BF1, + 0x7387,0x620E,0x5095,0x411C,0x35A3,0x242A,0x16B1,0x0738,0xFFCF,0xEE46,0xDCDD,0xCD54,0xB9EB,0xA862,0x9AF9,0x8B70, + 0x8408,0x9581,0xA71A,0xB693,0xC22C,0xD3A5,0xE13E,0xF0B7,0x0840,0x19C9,0x2B52,0x3ADB,0x4E64,0x5FED,0x6D76,0x7CFF, + 0x9489,0x8500,0xB79B,0xA612,0xD2AD,0xC324,0xF1BF,0xE036,0x18C1,0x0948,0x3BD3,0x2A5A,0x5EE5,0x4F6C,0x7DF7,0x6C7E, + 0xA50A,0xB483,0x8618,0x9791,0xE32E,0xF2A7,0xC03C,0xD1B5,0x2942,0x38CB,0x0A50,0x1BD9,0x6F66,0x7EEF,0x4C74,0x5DFD, + 0xB58B,0xA402,0x9699,0x8710,0xF3AF,0xE226,0xD0BD,0xC134,0x39C3,0x284A,0x1AD1,0x0B58,0x7FE7,0x6E6E,0x5CF5,0x4D7C, + 0xC60C,0xD785,0xE51E,0xF497,0x8028,0x91A1,0xA33A,0xB2B3,0x4A44,0x5BCD,0x6956,0x78DF,0x0C60,0x1DE9,0x2F72,0x3EFB, + 0xD68D,0xC704,0xF59F,0xE416,0x90A9,0x8120,0xB3BB,0xA232,0x5AC5,0x4B4C,0x79D7,0x685E,0x1CE1,0x0D68,0x3FF3,0x2E7A, + 0xE70E,0xF687,0xC41C,0xD595,0xA12A,0xB0A3,0x8238,0x93B1,0x6B46,0x7ACF,0x4854,0x59DD,0x2D62,0x3CEB,0x0E70,0x1FF9, + 0xF78F,0xE606,0xD49D,0xC514,0xB1AB,0xA022,0x92B9,0x8330,0x7BC7,0x6A4E,0x58D5,0x495C,0x3DE3,0x2C6A,0x1EF1,0x0F78}; + +/*!< CRC table used to compute an Ethernet 32 bit CRC using the normal polynom 0x04C11DB7. */ +static const uint32_t crc32EthernetTable[256] = +{ + 0x00000000, + 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, + 0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, + 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, + 0x5bd4b01b, 0x569796c2, 0x52568b75, 0x6a1936c8, 0x6ed82b7f, + 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, + 0x745e66cd, 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, + 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, 0xbe2b5b58, + 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, + 0xa4ad16ea, 0xa06c0b5d, 0xd4326d90, 0xd0f37027, 0xddb056fe, + 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, + 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, 0x34867077, 0x30476dc0, + 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, + 0x2ac12072, 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, + 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, 0x7897ab07, + 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, + 0x6211e6b5, 0x66d0fb02, 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, + 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, + 0xbb60adfc, 0xb6238b25, 0xb2e29692, 0x8aad2b2f, 0x8e6c3698, + 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, + 0x94ea7b2a, 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, + 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, 0xc6bcf05f, + 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, + 0xdc3abded, 0xd8fba05a, 0x690ce0ee, 0x6dcdfd59, 0x608edb80, + 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, + 0x58c1663d, 0x558240e4, 0x51435d53, 0x251d3b9e, 0x21dc2629, + 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, + 0x3b5a6b9b, 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, + 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, 0xf12f560e, + 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, + 0xeba91bbc, 0xef68060b, 0xd727bbb6, 0xd3e6a601, 0xdea580d8, + 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, + 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, 0x9b3660c6, 0x9ff77d71, + 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, + 0x857130c3, 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, + 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, 0x7b827d21, + 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, + 0x61043093, 0x65c52d24, 0x119b4be9, 0x155a565e, 0x18197087, + 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, + 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, 0xc5a92679, 0xc1683bce, + 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, + 0xdbee767c, 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, + 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, 0x89b8fd09, + 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, + 0x933eb0bb, 0x97ffad0c, 0xafb010b1, 0xab710d06, 0xa6322bdf, + 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4 +}; + +//----------------------------------------------------------------------// +//- 32 bits Ethernet CRC -// +//----------------------------------------------------------------------// + +/*! + * Initialize the 32 bit CRC computation system. + * \param[in] pInstance Pointer on an allocated but non initialized Crc32 object. + */ +SBG_COMMON_LIB_API void sbgCrc32Initialize(SbgCrc32 *pInstance) +{ + // + // Test input argument + // + assert(pInstance); + + *pInstance = 0xFFFFFFFF; +} + +/*! + * Compute a 32 bit CRC using an Ethernet polynome. + * Warning: the buffer size should be at least 4 bytes long. + * \param[in] pInstance Read only pointer on a valid Crc32 object. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. + */ +SBG_COMMON_LIB_API void sbgCrc32Update(SbgCrc32 *pInstance, const void *pData, size_t dataSize) +{ + const uint8_t *pBuffer = (const uint8_t*)pData; + uint32_t byte; + size_t i; + size_t dataSizeCorrected; + size_t numBytesLeft; + size_t index; + + // + // Test input arguments + // + assert(pInstance); + assert(pData); + + // + // Compute the data size that corresponds to complete uinht32 and how many bytes remains + // + dataSizeCorrected = dataSize & (~0x00000003); + numBytesLeft = dataSize & 0x03; + + // For each byte, update the CRC + // + for (i = 0; i < dataSizeCorrected; i++) + { + // + // We have to get index in reversed order per 4 bytes + // + index = i ^ 0x03; + + // + // Get the current byte value + // + byte = pBuffer[index]; + + // + // Update the CRC value + // + *pInstance = (*pInstance << 8) ^ crc32EthernetTable[((*pInstance >> 24) ^ byte) & 0xFF]; + } + + // + // Test how many bytes remains + // + for (i = 0; i < numBytesLeft; i++) + { + // + // We have to get index in reversed order per 4 bytes + // + index = (dataSizeCorrected-1) + (numBytesLeft - i); + + // + // Get the current byte value + // + byte = pBuffer[index]; + + // + // Update the CRC value + // + *pInstance = (*pInstance << 8) ^ crc32EthernetTable[((*pInstance >> 24) ^ byte) & 0xFF]; + } +} + +/*! + * Compute a 32 Bit CRC using an Ethernet polynome. + * Warning: the buffer size should be at least 4 bytes long. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. + * \return The computed CRC. + */ +SBG_COMMON_LIB_API uint32_t sbgCrc32Compute(const void *pData, size_t dataSize) +{ + SbgCrc32 crcInst; + + // + // Initialize the CRC system + // + sbgCrc32Initialize(&crcInst); + + // + // Compute the CRC + // + sbgCrc32Update(&crcInst, pData, dataSize); + + // + // Return it + // + return sbgCrc32Get(&crcInst); +} + +//----------------------------------------------------------------------// +//- CRC-16 operations -// +//----------------------------------------------------------------------// + +/*! + * Initialize the 16 bit CRC computation system. + * \param[in] pInstance Pointer on an allocated but non initialized Crc16 object. + */ +SBG_COMMON_LIB_API void sbgCrc16Initialize(SbgCrc16 *pInstance) +{ + // + // Test input argument + // + assert(pInstance); + + *pInstance = 0; +} + +/*! + * Compute a 16 bit CRC using an the polynome 0x8408. + * \param[in] pInstance Read only pointer on a valid Crc16 object. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer. + */ +SBG_COMMON_LIB_API void sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize) +{ + const uint8_t *pBuffer = (const uint8_t*)pData; + uint8_t index; + size_t i; + + // + // Test input arguments + // + assert(pInstance); + assert(pData); + + // + // For each byte in our buffer + // + for (i = 0; i < dataSize; i++) + { + // + // Update the current CRC + // + index = (pBuffer[i] ^ *pInstance) & 0xFF; + *pInstance = crc16LookupTable[index] ^ (*pInstance >> 8); + } +} + +/*! + * Compute a 32 Bit CRC using an the polynome 0x8408. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer. + * \return The computed CRC. + */ +SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize) +{ + SbgCrc16 crcInst; + + // + // Initialize the CRC system + // + sbgCrc16Initialize(&crcInst); + + // + // Compute the CRC + // + sbgCrc16Update(&crcInst, pData, dataSize); + + // + // Return it + // + return sbgCrc16Get(&crcInst); +} diff --git a/crates/sbg-rs/sbgECom/common/crc/sbgCrc.h b/crates/sbg-rs/sbgECom/common/crc/sbgCrc.h new file mode 100644 index 0000000..9d945de --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/crc/sbgCrc.h @@ -0,0 +1,135 @@ +/*! + * \file sbgCrc.h + * \ingroup common + * \author SBG Systems + * \date 15 January 2013 + * + * \brief This file provides CRC-32 and CRC-16 methods. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ +#ifndef SBG_CRC_H +#define SBG_CRC_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +#include + +//----------------------------------------------------------------------// +//- Types definitions -// +//----------------------------------------------------------------------// + +/*!< Type used to compute a 32 bit Ethernet CRC. */ +typedef uint32_t SbgCrc32; + +/*!< Type used to compute a 16 bit CRC. */ +typedef uint16_t SbgCrc16; + +//----------------------------------------------------------------------// +//- 32 bits Ethernet CRC -// +//----------------------------------------------------------------------// + +/*! + * Initialize the 32 bit CRC computation system. + * \param[in] pInstance Pointer on an allocated but non initialized Crc32 object. + */ +SBG_COMMON_LIB_API void sbgCrc32Initialize(SbgCrc32 *pInstance); + +/*! + * Compute a 32 bit CRC using an Ethernet polynome. + * Warning: the buffer size should be at least 4 bytes long. + * \param[in] pInstance Read only pointer on a valid Crc32 object. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. + */ +SBG_COMMON_LIB_API void sbgCrc32Update(SbgCrc32 *pInstance, const void *pData, size_t dataSize); + +/*! + * Returns the computed 32 bit CRC value. + * \param[in] pInstance Read only pointer on a valid Crc32 object. + * \return The computed CRC. + */ +SBG_INLINE uint32_t sbgCrc32Get(const SbgCrc32 *pInstance) +{ + return *pInstance; +} + +/*! + * Compute a 32 Bit CRC using an Ethernet polynome. + * Warning: the buffer size should be at least 4 bytes long. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer, has to be greater or equals to 4. + * \return The computed CRC. + */ +SBG_COMMON_LIB_API uint32_t sbgCrc32Compute(const void *pData, size_t dataSize); + +//----------------------------------------------------------------------// +//- CRC-16 operations -// +//----------------------------------------------------------------------// + +/*! + * Initialize the 16 bit CRC computation system. + * \param[in] pInstance Pointer on an allocated but non initialized Crc16 object. + */ +SBG_COMMON_LIB_API void sbgCrc16Initialize(SbgCrc16 *pInstance); + +/*! + * Compute a 16 bit CRC using an the polynome 0x8408. + * \param[in] pInstance Read only pointer on a valid Crc16 object. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer. + */ +SBG_COMMON_LIB_API void sbgCrc16Update(SbgCrc16 *pInstance, const void *pData, size_t dataSize); + +/*! + * Returns the computed 32 bit CRC value. + * \param[in] pInstance Read only pointer on a valid Crc16 object. + * \return The computed CRC. + */ +SBG_INLINE uint16_t sbgCrc16Get(const SbgCrc16 *pInstance) +{ + return *pInstance; +} + +/*! + * Compute a 32 Bit CRC using an the polynome 0x8408. + * \param[in] pData Read only pointer on the data buffer to compute CRC on. + * \param[in] dataSize Data size in bytes of the buffer. + * \return The computed CRC. + */ +SBG_COMMON_LIB_API uint16_t sbgCrc16Compute(const void *pData, size_t dataSize); + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_CRC_H */ diff --git a/crates/sbg-rs/sbgECom/common/debug/sbgDebug.c b/crates/sbg-rs/sbgECom/common/debug/sbgDebug.c new file mode 100644 index 0000000..fdf22cb --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/debug/sbgDebug.c @@ -0,0 +1,153 @@ +/*! + * \file sbgDebug.c + * \author SBG Systems + * + * \brief Error logging for the SBG Systems common C library. + * + * \section CodeCopyright Copyright Notice + * Copyright (C) 2019, SBG Systems SAS. All rights reserved. + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + */ + +// Standard headers +#include + +// sbgCommonLib headers +#include + +// Local headers +#include "sbgDebug.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Number of bytes translated per line. + */ +#define SBG_DEBUG_NR_BYTES_PER_LINE (16) + +/*! + * Size of the buffer used to generate lines, in bytes. + */ +#define SBG_DEBUG_LINE_BUFFER_SIZE (256) + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Dump the given buffer on a single text line. + * + * \param[out] pLine Output line buffer. + * \param[in] lineSize Size of the ouput line buffer. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size. + */ +static void sbgDebugHexDumpGenerateLine(char *pLine, size_t lineSize, const uint8_t *pBuffer, size_t size) +{ + size_t length; + + assert(pLine); + assert(lineSize != 0); + assert(pBuffer); + assert(size <= SBG_DEBUG_NR_BYTES_PER_LINE); + + for (size_t i = 0; i < size; i++) + { + length = snprintf(pLine, lineSize, "%02x ", pBuffer[i]); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } + + for (size_t i = size; i < SBG_DEBUG_NR_BYTES_PER_LINE; i++) + { + length = snprintf(pLine, lineSize, " "); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } + + length = snprintf(pLine, lineSize, " | "); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + + for (size_t i = 0; i < size; i++) + { + char c; + + if (isprint(pBuffer[i])) + { + c = pBuffer[i]; + } + else + { + c = '.'; + } + + length = snprintf(pLine, lineSize, "%c", c); + assert(length < lineSize); + + pLine = &pLine[length]; + lineSize -= length; + } +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +void sbgDebugHexDump(const char *pPrefix, const void *pBuffer, size_t size) +{ + char line[SBG_DEBUG_LINE_BUFFER_SIZE]; + size_t index = 0; + bool multiLine = false; + + assert(pPrefix); + assert(pBuffer || (size == 0)); + + if (size > SBG_DEBUG_NR_BYTES_PER_LINE) + { + SBG_LOG_DEBUG("%s: multi-line dump start (%zu bytes)", pPrefix, size); + multiLine = true; + } + + while (size != 0) + { + const uint8_t *pByteBuffer = pBuffer; + size_t rangeSize; + + if (size < SBG_DEBUG_NR_BYTES_PER_LINE) + { + rangeSize = size; + } + else + { + rangeSize = SBG_DEBUG_NR_BYTES_PER_LINE; + } + + sbgDebugHexDumpGenerateLine(line, sizeof(line), &pByteBuffer[index], rangeSize); + SBG_LOG_DEBUG("%s: %s", pPrefix, line); + + size -= rangeSize; + index += rangeSize; + } + + if (multiLine) + { + SBG_LOG_DEBUG("%s: multi-line dump end", pPrefix); + } +} diff --git a/crates/sbg-rs/sbgECom/common/debug/sbgDebug.h b/crates/sbg-rs/sbgECom/common/debug/sbgDebug.h new file mode 100644 index 0000000..a4879cd --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/debug/sbgDebug.h @@ -0,0 +1,163 @@ +/*! + * \file sbgDebug.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 + * + * \brief Define and handle error logging for the SBG Systems common C library. + * + * The methods defined here should be implemented in sbgPlatform.h/sbgPlatform.c + * according to your platform and needs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ +#ifndef SBG_DEBUG_H +#define SBG_DEBUG_H + +// sbgCommonLib headers +#include + +#ifndef SBG_DEBUG_LOG_CATEGORY + #define SBG_DEBUG_LOG_CATEGORY ("None") +#endif + +//----------------------------------------------------------------------// +//- Errors and warning definitions -// +//----------------------------------------------------------------------// + +/*! + * Enum that identify the type of error / warning that has been thrown. + */ +typedef enum _SbgDebugLogType +{ + SBG_DEBUG_LOG_TYPE_ERROR, /*!< The message to log is an error. */ + SBG_DEBUG_LOG_TYPE_WARNING, /*!< The message to log is a warning. */ + SBG_DEBUG_LOG_TYPE_INFO, /*!< The message to log is an information. */ + SBG_DEBUG_LOG_TYPE_DEBUG /*!< The message to log is a debug information. */ +} SbgDebugLogType; + +//----------------------------------------------------------------------// +//- Errors and warning macros -// +//----------------------------------------------------------------------// + +/*! + * Log an error with its associated message. + * \param[in] errorCode The error code that has thrown this error. + * \param[in] format String litteral for the associated error message (you can use printf like string formating). + */ +#define SBG_LOG_ERROR_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_ERROR, errorCode, format, ##__VA_ARGS__) + +#if SBG_CONFIG_ENABLE_LOG_ERROR == 1 + #define SBG_LOG_ERROR SBG_LOG_ERROR_CALL +#else + #define SBG_LOG_ERROR(format, ...) ((void)sizeof(SBG_LOG_ERROR_CALL(format, ## __VA_ARGS__), 0)) +#endif + +/*! + * Log a warning with its associated message. + * \param[in] errorCode The error code that has thrown this warning. + * \param[in] format String litteral for the associated warning message (you can use printf like string formating). + */ +#define SBG_LOG_WARNING_CALL(errorCode, format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_WARNING, errorCode, format, ##__VA_ARGS__) + +#if SBG_CONFIG_ENABLE_LOG_WARNING == 1 + #define SBG_LOG_WARNING SBG_LOG_WARNING_CALL +#else + #define SBG_LOG_WARNING(format, ...) ((void)sizeof(SBG_LOG_WARNING_CALL(format, ## __VA_ARGS__), 0)) +#endif + +/*! + * Log an information message. + * \param[in] format String litteral for the information message (you can use printf like string formating). + */ +#define SBG_LOG_INFO_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_INFO, SBG_NO_ERROR, format, ##__VA_ARGS__) + +#if SBG_CONFIG_ENABLE_LOG_INFO == 1 + #define SBG_LOG_INFO SBG_LOG_INFO_CALL +#else + #define SBG_LOG_INFO(format, ...) ((void)sizeof(SBG_LOG_INFO_CALL(format, ## __VA_ARGS__), 0)) +#endif + +/*! + * Log an information message only in debug mode + * \param[in] format String litteral for the information message (you can use printf like string formating). + */ +#define SBG_LOG_DEBUG_CALL(format, ...) sbgPlatformDebugLogMsg((const char*)__BASE_FILE__, (const char*)__FUNCTION__, __LINE__, SBG_DEBUG_LOG_CATEGORY, SBG_DEBUG_LOG_TYPE_DEBUG, SBG_NO_ERROR, format, ##__VA_ARGS__) + +#if SBG_CONFIG_ENABLE_LOG_DEBUG == 1 + #define SBG_LOG_DEBUG SBG_LOG_DEBUG_CALL +#else + #define SBG_LOG_DEBUG(format, ...) ((void)sizeof(SBG_LOG_DEBUG_CALL(format, ## __VA_ARGS__), 0)) +#endif + +//----------------------------------------------------------------------// +//- Inline functions -// +//----------------------------------------------------------------------// + +/*! + * Convert a log type into a string representation. + * + * \param[in] logType Log type. + * \return String representation for the given log type. + */ +SBG_INLINE const char *sbgDebugLogTypeToStr(SbgDebugLogType logType) +{ + const char *pString; + + switch (logType) + { + case SBG_DEBUG_LOG_TYPE_ERROR: + pString = "error"; + break; + case SBG_DEBUG_LOG_TYPE_WARNING: + pString = "warning"; + break; + case SBG_DEBUG_LOG_TYPE_INFO: + pString = "info"; + break; + case SBG_DEBUG_LOG_TYPE_DEBUG: + pString = "debug"; + break; + default: + pString = "unknown"; + break; + } + + return pString; +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Produce a text dump of a buffer. + * + * \param[in] pPrefix Prefix string before each line. + * \param[in] pBuffer Data buffer, may be NULL. + * \param[in] size Data size, in bytes. + */ +void sbgDebugHexDump(const char *pPrefix, const void *pBuffer, size_t size); + +#endif /* SBG_DEBUG_H */ diff --git a/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.c b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.c new file mode 100644 index 0000000..0b728ff --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.c @@ -0,0 +1,83 @@ +/* sbgCommonLib headers */ +#include + +/* Local headers */ +#include "sbgInterface.h" + +//----------------------------------------------------------------------// +//- Private definitions -// +//----------------------------------------------------------------------// + +static const char *gInterfaceType[] = +{ + [SBG_IF_TYPE_UNKNOW] = "unknown", + [SBG_IF_TYPE_SERIAL] = "serial", + [SBG_IF_TYPE_ETH_UDP] = "eth UDP", + [SBG_IF_TYPE_ETH_TCP_IP] = "eth TCP", + [SBG_IF_TYPE_FILE] = "file" +}; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void sbgInterfaceZeroInit(SbgInterface *pInterface) +{ + assert(pInterface); + + // + // Make sure the whole struct is zero init + // + memset(pInterface, 0x00, sizeof(*pInterface)); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceDestroy(SbgInterface *pInterface) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pInterface); + + if (pInterface->pDestroyFunc) + { + errorCode = pInterface->pDestroyFunc(pInterface); + } + + return errorCode; +} + +const char *sbgInterfaceTypeGetAsString(const SbgInterface *pInterface) +{ + assert(pInterface); + + if (pInterface->type < SBG_ARRAY_SIZE(gInterfaceType)) + { + return gInterfaceType[pInterface->type]; + } + else + { + SBG_LOG_ERROR(SBG_INVALID_PARAMETER, "Unknown interface type: %" PRIu32, pInterface->type); + return gInterfaceType[SBG_IF_TYPE_UNKNOW]; + } +} + +void sbgInterfaceNameSet(SbgInterface *pInterface, const char *pName) +{ + size_t nameLen; + + assert(pInterface); + assert(pName); + + // + // Only keep the end of the name that can fit in the interface name buffer + // + nameLen = strlen(pName); + + if (nameLen < SBG_ARRAY_SIZE(pInterface->name)) + { + strcpy(pInterface->name, pName); + } + else + { + strcpy(pInterface->name, pName+(nameLen-(SBG_ARRAY_SIZE(pInterface->name)-1))); + } +} diff --git a/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.h b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.h new file mode 100644 index 0000000..6ebf057 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterface.h @@ -0,0 +1,455 @@ +/*! + * \file sbgInterface.h + * \ingroup common + * \author SBG Systems + * \date 10 December 2012 + * + * \brief This file implements the base interface for all Serial and Ethernet ports. + * + * An interface is used to provide a common API for both serial and Ethernet ports. + * An interface can be opened/closed and some data can be written or read from it. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_INTERFACE_H +#define SBG_INTERFACE_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +/* sbgCommonLib headers */ +#include + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +#define SBG_IF_NAME_MAX_SIZE (48) /*!< Maximum size in bytes for the interface name string */ + +/*! + * Type values reserved for standard interface types. + */ +#define SBG_IF_TYPE_UNKNOW (0) /*!< The interface type is not defined. */ +#define SBG_IF_TYPE_SERIAL (1) /*!< The interface is a serial com port. */ +#define SBG_IF_TYPE_ETH_UDP (2) /*!< The interface is an UDP one. */ +#define SBG_IF_TYPE_ETH_TCP_IP (3) /*!< The interface is an TCP/IP one. */ +#define SBG_IF_TYPE_FILE (4) /*!< The interface is a file. */ +#define SBG_IF_TYPE_LAST_RESERVED (999) /*!< Last reserved value for standard types. */ + +// +// Flags for the flush operation. +// +#define SBG_IF_FLUSH_INPUT ((uint32_t)1 << 0) /*!< Flush input data flag. */ +#define SBG_IF_FLUSH_OUTPUT ((uint32_t)1 << 1) /*!< Flush output data flag. */ +#define SBG_IF_FLUSH_ALL (SBG_IF_FLUSH_INPUT | SBG_IF_FLUSH_OUTPUT) /*!< Flag combination to flush both input and output data. */ + +//----------------------------------------------------------------------// +//- Predefinitions -// +//----------------------------------------------------------------------// + +/*! + * Interface structure pre-definition. + */ +typedef struct _SbgInterface SbgInterface; + +/*! + * Handle that stores the internal interface handle (ie Serial or Ethernet) + */ +typedef void* SbgInterfaceHandle; + +//----------------------------------------------------------------------// +//- Callbacks definitions -// +//----------------------------------------------------------------------// + +/*! + * Method to implement that close and destroy an interface. + * + * \param[in] pInterface Interface instance. + * \return SBG_NO_ERROR if the interface has been closed successfully. + */ +typedef SbgErrorCode (*SbgInterfaceDestroyFunc)(SbgInterface *pInterface); + +/*! + * Method to implement to write a buffer to an interface. + * + * This method should return an error only if all bytes were not written successfully. + * If you try to write zero byte, the method shouldn't return any error. + * + * \param[in] pInterface Interface instance. + * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write + * \param[in] bytesToWrite Number of bytes we would like to write (can be zero). + * \return SBG_NO_ERROR if exactly bytesToWrite have been written successfully. + */ +typedef SbgErrorCode (*SbgInterfaceWriteFunc)(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite); + +/*! + * Method to implement to read data from an interface. + * + * This method returns an error only if there is a 'low level' error on the interface. + * If no byte is read at all or less bytes than bytesToRead, this method returns SBG_NO_ERROR. + * You have to check pReadBytes field to know the number of bytes actually read. + * + * \param[in] pInterface Interface instance. + * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. + * \param[out] pReadBytes Returns the number of bytes actually read (can be zero and up to bytesToRead). + * \param[in] bytesToRead Maximum number of bytes to try to read on the interface. + * \return SBG_NO_ERROR if zero or some bytes have been read successfully. + */ +typedef SbgErrorCode (*SbgInterfaceReadFunc)(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead); + +/*! + * Make an interface flush pending input and/or output data. + * + * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. + * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. + * + * WARNING: The method has no action if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. + */ +typedef SbgErrorCode (*SbgInterfaceFlushFunc)(SbgInterface *pInterface, uint32_t flags); + +/*! + * Change an interface input and output speed in bps (bit per second) + * + * This method will try to change the speed immediately even if there are + * pending bytes in the send buffer. + * + * If you would like to make sure that all bytes in the Tx buffer have been + * sent before changing the speed, please flush the interface before. + * + * WARNING: The method has no action if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \param[in] speed The new interface speed to set in bps. + * \return SBG_NO_ERROR if successful. + */ +typedef SbgErrorCode (*SbgInterfaceSetSpeed)(SbgInterface *pInterface, uint32_t speed); + +/*! + * Returns the current interface baud rate in bps (bit per second) + * + * WARNING: The method will returns zero if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \return The current interface baud rate in bps or zero if not applicable. + */ +typedef uint32_t (*SbgInterfaceGetSpeed)(const SbgInterface *pInterface); + +/*! + * Compute and return the delay needed by the interface to transmit / receive X number of bytes. + * + * WARNING: The method will returns zero if not applicable for a type of interface. + * + * \param[in] pInterface Interface instance. + * \param[in] numBytes The number of bytes to transmit / receive to evaluate the needed delay. + * \return The expected delay in us needed to transmit / receive the specified number of bytes or 0 if not applicable. + */ +typedef uint32_t (*SbgInterfaceGetDelayFunc)(const SbgInterface *pInterface, size_t numBytes); + +//----------------------------------------------------------------------// +//- Structures definitions -// +//----------------------------------------------------------------------// + +/*! + * Interface definition that stores methods used to communicate on the interface. + * + * The interface class is designed to allow custom user implementations. The type member stores + * a type identifier allowing the identification of the underlying type, including custom + * implementations. Standard interfaces provided by this library use types from 1 up to + * and including SBG_IF_TYPE_LAST_RESERVED. Greater values are intended to identify custom + * types that are normally specific to the project using this library. The value 0 identifies + * an unknown interface type, usually indicating that the interface was not correctly initialized. + */ +struct _SbgInterface +{ + SbgInterfaceHandle handle; /*!< Internal interface handle used to access the media. */ + uint32_t type; /*!< Opaque interface type. */ + char name[SBG_IF_NAME_MAX_SIZE]; /*!< The interface name as passed during the creation */ + + SbgInterfaceDestroyFunc pDestroyFunc; /*!< Optional method used to destroy an interface. */ + SbgInterfaceWriteFunc pWriteFunc; /*!< Optional method used to write some data to this interface. */ + SbgInterfaceReadFunc pReadFunc; /*!< Optional method used to read some data to this interface. */ + SbgInterfaceFlushFunc pFlushFunc; /*!< Optional method used to make this interface flush all pending data. */ + SbgInterfaceSetSpeed pSetSpeedFunc; /*!< Optional method used to set the interface speed in bps. */ + SbgInterfaceGetSpeed pGetSpeedFunc; /*!< Optional method used to retrieve the interface speed in bps. */ + SbgInterfaceGetDelayFunc pDelayFunc; /*!< Optional method used to compute an expected delay to transmit/receive X bytes */ +}; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Initialize an interface instance to zero. + * + * \param[in] pInterface The interface instance. + */ +SBG_COMMON_LIB_API void sbgInterfaceZeroInit(SbgInterface *pInterface); + +/*! + * Close and destroy the interface gracefully. + * + * This method will call the specialized interface destructor if any. + * + * \param[in] pInterface The interface instance. + * \return SBG_NO_ERROR if the interface has been destroyed successfully. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceDestroy(SbgInterface *pInterface); + +/*! + * Returns the interface type. + * + * \param[in] pInterface Interface instance + * \return The interface type. + */ +SBG_INLINE uint32_t sbgInterfaceTypeGet(const SbgInterface *pInterface) +{ + assert(pInterface); + + return pInterface->type; +} + +/*! + * Returns the interface as string. + * + * \param[in] pInterface Interface instance + * \return The interface type. + */ +SBG_COMMON_LIB_API const char *sbgInterfaceTypeGetAsString(const SbgInterface *pInterface); + +/*! + * Returns the interface name string. + * + * \param[in] pInterface Interface instance + * \return The interface name as a NULL terminated C string. + */ +SBG_INLINE const char *sbgInterfaceNameGet(const SbgInterface *pInterface) +{ + assert(pInterface); + + return pInterface->name; +} + +/*! + * Define the interface name as a NULL terminated C string. + * + * This method make sure that the provided string will always fit within + * the allocated name buffer. + * + * If the interface name you would like to set is too long, only the end + * of the string will be kept. + * + * \param[in] pInterface Interface instance + * \param[in] pName The interface name to set as a NULL terminated C string + */ +SBG_COMMON_LIB_API void sbgInterfaceNameSet(SbgInterface *pInterface, const char *pName); + +/*! + * Write some data to an interface. + * + * This method should return an error only if all bytes were not written successfully. + * If you try to write zero byte, the method shouldn't return any error. + * + * \param[in] pInterface Interface instance. + * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write + * \param[in] bytesToWrite Number of bytes we would like to write (can be zero). + * \return SBG_NO_ERROR if exactly bytesToWrite have been written successfully. + * SBG_INVALID_PARAMETER if the interface doesn't support write operations. + */ +SBG_INLINE SbgErrorCode sbgInterfaceWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) +{ + SbgErrorCode errorCode; + + assert(pInterface); + assert(pBuffer); + + if (pInterface->pWriteFunc) + { + errorCode = pInterface->pWriteFunc(pInterface, pBuffer, bytesToWrite); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +/*! + * Try to read some data from an interface. + * + * This method returns an error only if there is a 'low level' error on the interface. + * If no byte is read at all or less bytes than bytesToRead, this method returns SBG_NO_ERROR. + * You have to check pReadBytes field to know the number of bytes actually read. + * + * \param[in] pInterface Interface instance. + * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. + * \param[out] pReadBytes Returns the number of bytes actually read (can be zero and up to bytesToRead). + * \param[in] bytesToRead Maximum number of bytes to try to read on the interface. + * \return SBG_NO_ERROR if zero or some bytes have been read successfully. + * SBG_INVALID_PARAMETER if the interface doesn't support read operations. + */ +SBG_INLINE SbgErrorCode sbgInterfaceRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) +{ + SbgErrorCode errorCode; + + assert(pInterface); + assert(pBuffer); + assert(pReadBytes); + + if (pInterface->pReadFunc) + { + errorCode = pInterface->pReadFunc(pInterface, pBuffer, pReadBytes, bytesToRead); + } + else + { + *pReadBytes = 0; + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +/*! + * Make an interface flush pending input and/or output data. + * + * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. + * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. + * + * WARNING: The method has no action if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. + */ +SBG_INLINE SbgErrorCode sbgInterfaceFlush(SbgInterface *pInterface, uint32_t flags) +{ + SbgErrorCode errorCode; + + assert(pInterface); + assert((flags & ~SBG_IF_FLUSH_ALL) == 0); + + if (pInterface->pFlushFunc) + { + errorCode = pInterface->pFlushFunc(pInterface, flags); + } + else + { + errorCode = SBG_NO_ERROR; + } + + return errorCode; +} + +/*! + * Change an interface input and output speed in bps (bit per second) + * + * This method will try to change the speed immediately even if there are + * pending bytes in the send buffer. + * + * If you would like to make sure that all bytes in the Tx buffer have been + * sent before changing the speed, please flush the interface before. + * + * WARNING: The method has no action if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \param[in] speed The new interface speed to set in bps. + * \return SBG_NO_ERROR if successful. + */ +SBG_INLINE SbgErrorCode sbgInterfaceSetSpeed(SbgInterface *pInterface, uint32_t speed) +{ + assert(pInterface); + assert(speed > 0); + + if (pInterface->pSetSpeedFunc) + { + return pInterface->pSetSpeedFunc(pInterface, speed); + } + else + { + return SBG_NO_ERROR; + } +} + +/*! + * Returns the current interface baud rate in bps (bit per second) + * + * WARNING: The method will returns zero if not applicable for a type of interface + * + * \param[in] pInterface Interface instance. + * \return The current interface baud rate in bps or zero if not applicable. + */ +SBG_INLINE uint32_t sbgInterfaceGetSpeed(const SbgInterface *pInterface) +{ + assert(pInterface); + + if (pInterface->pGetSpeedFunc) + { + return pInterface->pGetSpeedFunc(pInterface); + } + else + { + return 0; + } +} + +/*! + * Compute and return the delay needed by the interface to transmit / receive X number of bytes. + * + * WARNING: The method will returns zero if not applicable for a type of interface. + * + * \param[in] pInterface Interface instance. + * \param[in] numBytes The number of bytes to transmit / receive to evaluate the needed delay. + * \return The expected delay in us needed to transmit / receive the specified number of bytes or 0 if not applicable. + */ +SBG_INLINE uint32_t sbgInterfaceGetDelay(const SbgInterface *pInterface, size_t numBytes) +{ + assert(pInterface); + + if (pInterface->pDelayFunc) + { + return pInterface->pDelayFunc(pInterface, numBytes); + } + else + { + return 0; + } +} + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_INTERFACE_H */ diff --git a/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.c b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.c new file mode 100644 index 0000000..7478b9e --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.c @@ -0,0 +1,305 @@ +// sbgCommnonLib headers +#include + +// Local headers +#include "sbgInterface.h" +#include "sbgInterfaceFile.h" + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Returns the interface FILE descriptor. + * + * \param[in] pInterface Interface instance. + * \return The associated FILE descriptor. + */ +static FILE *sbgInterfaceFileGetDesc(SbgInterface *pInterface) +{ + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pInterface->handle); + + return (FILE*)pInterface->handle; +} + +/*! + * Destroy the interface by closing the file descriptor. + * + * \param[in] pInterface Interface instance. + * \return SBG_NO_ERROR if the interface has been closed successfully. + */ +static SbgErrorCode sbgInterfaceFileDestroy(SbgInterface *pInterface) +{ + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + fclose(pInputFile); + sbgInterfaceZeroInit(pInterface); + + return SBG_NO_ERROR; +} + +/*! + * Write some data the the file + * + * \param[in] pInterface Valid handle on an initialized interface. + * \param[in] pBuffer Pointer on an allocated buffer that contains the data to write + * \param[in] bytesToWrite Number of bytes we would like to write. + * \return SBG_NO_ERROR if all bytes have been written successfully. + */ +static SbgErrorCode sbgInterfaceFileWrite(SbgInterface *pInterface, const void *pBuffer, size_t bytesToWrite) +{ + FILE *pOutputFile; + size_t bytesWritten; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pBuffer); + + pOutputFile = sbgInterfaceFileGetDesc(pInterface); + + // + // Write the data and check if all bytes have been written + // + bytesWritten = fwrite(pBuffer, sizeof(uint8_t), bytesToWrite, pOutputFile); + + if (bytesWritten == bytesToWrite) + { + + return SBG_NO_ERROR; + } + else + { + return SBG_WRITE_ERROR; + } +} + +/*! + * Try to read some data from an interface. + * + * \param[in] pInterface Valid handle on an initialized interface. + * \param[in] pBuffer Pointer on an allocated buffer that can hold at least bytesToRead bytes of data. + * \param[out] pReadBytes Pointer on an uint32_t used to return the number of read bytes. + * \param[in] bytesToRead Number of bytes we would like to read. + * \return SBG_NO_ERROR if no error occurs, please check the number of received bytes. + */ +static SbgErrorCode sbgInterfaceFileRead(SbgInterface *pInterface, void *pBuffer, size_t *pReadBytes, size_t bytesToRead) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + assert(pBuffer); + assert(pReadBytes); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + // + // Read some bytes from the file and check if an error has occurred + // + *pReadBytes = fread(pBuffer, sizeof(uint8_t), bytesToRead, pInputFile); + + if (*pReadBytes < bytesToRead) + { + // + // Don't report an error if we have reached the end of the file to comply with sbgInterface specification + // + if (ferror(pInputFile) != 0) + { + errorCode = SBG_READ_ERROR; + SBG_LOG_ERROR(errorCode, "File read error %u", ferror(pInputFile)); + } + } + + return errorCode; +} + +/*! + * Make an interface flush all pending input or output data. + * + * If flags include SBG_IF_FLUSH_INPUT, all pending input data is discarded. + * If flags include SBG_IF_FLUSH_OUTPUT, the function blocks until all output data has been written out. + * + * \param[in] pInterface Valid handle on an initialized interface. + * \param[in] flags Combination of the SBG_IF_FLUSH_INPUT and SBG_IF_FLUSH_OUTPUT flags. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgInterfaceFileFlush(SbgInterface *pInterface, uint32_t flags) +{ + SbgErrorCode errorCode; + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + if ((pInterface->pReadFunc && (flags & SBG_IF_FLUSH_INPUT)) || + (pInterface->pWriteFunc && (flags & SBG_IF_FLUSH_OUTPUT))) + { + int ret; + + ret = fflush(pInputFile); + + if (ret == 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + } + else + { + errorCode = SBG_NO_ERROR; + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileOpen(SbgInterface *pInterface, const char *filePath) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(filePath); + + // + // Always call the underlying zero init method to make sure we can correctly handle SbgInterface evolutions + // + sbgInterfaceZeroInit(pInterface); + + // + // Try to open the file + // + pInputFile = fopen(filePath, "rb"); + + // + // Test if the input file has been opened + // + if (pInputFile) + { + // + // Define base interface members + // + pInterface->handle = pInputFile; + pInterface->type = SBG_IF_TYPE_FILE; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, filePath); + + // + // Define all specialized members + // + pInterface->pDestroyFunc = sbgInterfaceFileDestroy; + pInterface->pReadFunc = sbgInterfaceFileRead; + pInterface->pWriteFunc = NULL; + pInterface->pFlushFunc = sbgInterfaceFileFlush; + } + else + { + // + // Unable to open the input file + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteOpen(SbgInterface *pInterface, const char *filePath) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + FILE *pInputFile; + + assert(pInterface); + assert(filePath); + + // + // Try to open the file + // + pInputFile = fopen(filePath, "wb"); + + // + // Test if the input file has been opened + // + if (pInputFile) + { + // + // Define base interface members + // + pInterface->handle = pInputFile; + pInterface->type = SBG_IF_TYPE_FILE; + + // + // Define the interface name + // + sbgInterfaceNameSet(pInterface, filePath); + + // + // Define all specialized members + // + pInterface->pDestroyFunc = sbgInterfaceFileDestroy; + pInterface->pReadFunc = NULL; + pInterface->pWriteFunc = sbgInterfaceFileWrite; + pInterface->pFlushFunc = sbgInterfaceFileFlush; + } + else + { + // + // Unable to open the input file + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API size_t sbgInterfaceFileGetSize(SbgInterface *pInterface) +{ + FILE *pInputFile; + long cursorPos; + long fileSize; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc(pInterface); + + // + // Compute the file size + // + cursorPos = ftell(pInputFile); + fseek(pInputFile, 0, SEEK_END); + fileSize = ftell(pInputFile); + fseek(pInputFile, cursorPos, SEEK_SET); + + return (size_t)fileSize; +} + +SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pInterface) +{ + FILE *pInputFile; + + assert(pInterface); + assert(pInterface->type == SBG_IF_TYPE_FILE); + + pInputFile = sbgInterfaceFileGetDesc((SbgInterface*)pInterface); + + return (size_t)ftell(pInputFile); +} diff --git a/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.h b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.h new file mode 100644 index 0000000..ea3b9b0 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceFile.h @@ -0,0 +1,93 @@ +/*! + * \file sbgInterfaceFile.h + * \ingroup common + * \author SBG Systems (Raphael Siryani) + * \date 01 April 2013 + * + * \brief This file implements a file interface for read only operations. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ +#ifndef SBG_INTERFACE_FILE_H +#define SBG_INTERFACE_FILE_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +// sbgCommonLib headers +#include + +// Local headers +#include "sbgInterface.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Open a file as an interface for read only operations. + * + * \param[in] pInterface Pointer on an allocated interface instance to initialize. + * \param[in] filePath File path to open. + * \return SBG_NO_ERROR if the interface has been created. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileOpen(SbgInterface *pInterface, const char *filePath); + +/*! + * Open a file as an interface for write only operations. + * + * \param[in] pInterface Pointer on an allocated interface instance to initialize. + * \param[in] filePath File path to open. + * \return SBG_NO_ERROR if the interface has been created. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceFileWriteOpen(SbgInterface *pInterface, const char *filePath); + +/*! + * Returns the file size in bytes. + * + * \param[in] pInterface Valid handle on an initialized interface. + * \return The file size in bytes. + */ +SBG_COMMON_LIB_API size_t sbgInterfaceFileGetSize(SbgInterface *pInterface); + +/*! + * Returns the current cursor position in the file in bytes. + * + * \param[in] pInterface Valid handle on an initialized interface. + * \return The current cursor position in bytes. + */ +SBG_COMMON_LIB_API size_t sbgInterfaceFileGetCursor(const SbgInterface *pInterface); + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_INTERFACE_FILE_H */ diff --git a/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceSerial.h b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceSerial.h new file mode 100644 index 0000000..2fcbc11 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/interfaces/sbgInterfaceSerial.h @@ -0,0 +1,71 @@ +/*! + * \file sbgInterfaceSerial.h + * \ingroup common + * \author SBG Systems + * \date 06 February 2013 + * + * \brief This file implements a serial interface. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_INTERFACE_SERIAL_H +#define SBG_INTERFACE_SERIAL_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +// sbgCommonLib headers +#include +#include + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +#define SBG_IF_SERIAL_TX_BUFFER_SIZE (4096) /*!< Define the transmission buffer size for the serial port. */ +#define SBG_IF_SERIAL_RX_BUFFER_SIZE (4096) /*!< Define the reception buffer size for the serial port. */ + +/*! + * Initialize a serial interface for read and write operations. + * + * \param[in] pInterface Pointer on an allocated interface instance to initialize. + * \param[in] deviceName Serial interface location (COM21 , /dev/ttys0, depending on platform). + * \param[in] baudRate Serial interface baud rate in bps. + * \return SBG_NO_ERROR if the interface has been created. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterface, const char *deviceName, uint32_t baudRate); + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif // SBG_INTERFACE_SERIAL_H diff --git a/crates/sbg-rs/sbgECom/common/network/sbgNetwork.c b/crates/sbg-rs/sbgECom/common/network/sbgNetwork.c new file mode 100644 index 0000000..188d0ac --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/network/sbgNetwork.c @@ -0,0 +1,127 @@ +// Project headers +#include +#include + + +// Local headers +#include "sbgNetwork.h" + +//----------------------------------------------------------------------// +//- IP manipulation methods -// +//----------------------------------------------------------------------// + +/*! + * Convert an ip to a string of the form A.B.C.D + * \param[in] ipAddr IP address to convert to a string. + * \param[out] pBuffer Pointer on an allocated buffer than can hold ip address as a string. + * \param[in] maxSize Maximum number of chars that can be stored in pBuffer including the NULL char. + */ +SBG_COMMON_LIB_API void sbgNetworkIpToString(sbgIpAddress ipAddr, char *pBuffer, size_t maxSize) +{ + // + // Check input arguments + // + assert(pBuffer); + assert(maxSize >= 16); + + SBG_UNUSED_PARAMETER(maxSize); + + // + // Write the IP address + // + snprintf(pBuffer, maxSize, "%" PRIu8 ".%" PRIu8 ".%" PRIu8 ".%" PRIu8, sbgIpAddrGetA(ipAddr), sbgIpAddrGetB(ipAddr), sbgIpAddrGetC(ipAddr), sbgIpAddrGetD(ipAddr)); +} + +/*! + * Convert an ip address stored in a string of the form A.B.C.D to an sbgIpAddress object. + * \param[in] pBuffer IP address as a string of the form A.B.C.D + * \return IP address parsed from the string or 0.0.0.0 if the IP is invalid. + */ +SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer) +{ + int ipAddrA; + int ipAddrB; + int ipAddrC; + int ipAddrD; + int numReadParams; + sbgIpAddress ip; + sbgIpAddress ret; + char checkBuffer[SBG_NETWORK_IPV4_STRING_SIZE]; + + assert(pBuffer); + + ret = SBG_IPV4_UNSPECIFIED_ADDR; + + numReadParams = sscanf(pBuffer, "%d.%d.%d.%d", &ipAddrA, &ipAddrB, &ipAddrC, &ipAddrD); + + if ((numReadParams == 4) && (ipAddrA >= 0) && (ipAddrA <= 255) && (ipAddrB >= 0) && (ipAddrB <= 255) && (ipAddrC >= 0) && (ipAddrC <= 255) && (ipAddrD >= 0) && (ipAddrD <= 255)) + { + ip = sbgIpAddr((uint8_t)ipAddrA, (uint8_t)ipAddrB, (uint8_t)ipAddrC, (uint8_t)ipAddrD); + + sbgNetworkIpToString(ip, checkBuffer, SBG_ARRAY_SIZE(checkBuffer)); + + if (strcmp(pBuffer, checkBuffer) == 0) + { + ret = ip; + } + } + + return ret; +} + +//----------------------------------------------------------------------// +//- IP validation methods -// +//----------------------------------------------------------------------// + +/*! +* Check if an IpV4 netmask is valid, the mask should be contiguous (1111 followed by 0) +* \param[in] netmask The netmask stored in an uint32_t (host endianness). +* \return true if the netmask is valid ie contiguous. +*/ +SBG_COMMON_LIB_API bool sbgIpNetMaskValid(sbgIpAddress netmask) +{ + uint32_t y; + uint32_t z; + + // + // First test that the netmask is not zero, if yes, it's valid + // + if (netmask != 0) + { + // + // We are doing arithmetic so we have to make sure the netmask is using the platform endianness + // The IP address is always stored in big endian so we have to swap it for little endian platforms + // +#if SBG_CONFIG_BIG_ENDIAN == 0 + netmask = sbgSwap32(netmask); +#endif + + // + // Compute the bitwise inverse + // + y = ~netmask; + + // + // Add one to the inverse so if netmask was a proper one, there will be at most 1 bit set in this. + // + z = y + 1; + + // + // Test that, simply and z with z - 1, which happens to be y. The result will be zero if all is OK, non zero otherwise. + // + if ((z&y) == 0) + { + // + // We have a valid netmask + // + return true; + } + else + { + return false; + } + } + + return true; +} + diff --git a/crates/sbg-rs/sbgECom/common/network/sbgNetwork.h b/crates/sbg-rs/sbgECom/common/network/sbgNetwork.h new file mode 100644 index 0000000..3df7532 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/network/sbgNetwork.h @@ -0,0 +1,287 @@ +/*! + * \file sbgNetwork.h + * \ingroup common + * \author SBG Systems + * \date September 15, 2015 + * + * \brief Network related tools + * + * IP v4 address is stored in memory with a uint32_t. + * Each address component A.B.C.D is stored in 8 bits using the network + * endianess ie Big Endian. + * + * We thus have the following memory organisation: + * + * In Little Endian: + * |LSB| | |MSB| + * | A | B | C | D | + * + * In Big Endian: + * |MSB| | |LSB| + * | A | B | C | D | + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_NETWORK_H +#define SBG_NETWORK_H + +// sbgCommonLib headers +#include + + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Common IPv4 definitions -// +//----------------------------------------------------------------------// +#define SBG_IPV4_UNSPECIFIED_ADDR sbgIpAddr(0, 0, 0, 0) /*!< This represents an undefined IP address. */ +#define SBG_IPV4_BROADCAST_ADDR sbgIpAddr(255, 255, 255, 255) /*!< Broadcast IP address used to address all devices within the same network. */ + +#define SBG_NETWORK_IPV4_STRING_SIZE (16) /*!< String size for representation of an IPV4 */ + +//----------------------------------------------------------------------// +//- IP setters / getters -// +//----------------------------------------------------------------------// + +/*! + * Build an IP V4 address in the form a.b.c.d + * \param[in] a First 8 bits IP address. + * \param[in] b Second 8 bits IP address. + * \param[in] c Third 8 bits IP address. + * \param[in] d Last 8 bits IP address. + * \return An initialized IP address object. + */ +SBG_INLINE sbgIpAddress sbgIpAddr(uint8_t a, uint8_t b, uint8_t c, uint8_t d) +{ +#if SBG_CONFIG_BIG_ENDIAN == 1 + return (a << 24) | (b << 16) | (c << 8) | d; +#else + return a | (b << 8) | (c << 16) | (d << 24); +#endif +} + +/*! + * Return the first A field of of an IP v4 address of the form A.B.C.D + * \param[in] ipAddr An sbgIpAddress to convert. + * \return The A field of the IP address. + */ +SBG_INLINE uint8_t sbgIpAddrGetA(sbgIpAddress ipAddr) +{ +#if SBG_CONFIG_BIG_ENDIAN == 1 + return (uint8_t)((ipAddr & 0xFF000000) >> 24); +#else + return (uint8_t)((ipAddr & 0x000000FF)); +#endif +} + +/*! +* Return the first B field of of an IP v4 address of the form A.B.C.D +* \param[in] ipAddr An sbgIpAddress to convert. +* \return The B field of the IP address. +*/ +SBG_INLINE uint8_t sbgIpAddrGetB(sbgIpAddress ipAddr) +{ +#if SBG_CONFIG_BIG_ENDIAN == 1 + return (uint8_t)((ipAddr & 0x00FF0000) >> 16); +#else + return (uint8_t)((ipAddr & 0x0000FF00) >> 8); +#endif +} + +/*! +* Return the first C field of of an IP v4 address of the form A.B.C.D +* \param[in] ipAddr An sbgIpAddress to convert. +* \return The C field of the IP address. +*/ +SBG_INLINE uint8_t sbgIpAddrGetC(sbgIpAddress ipAddr) +{ +#if SBG_CONFIG_BIG_ENDIAN == 1 + return (uint8_t)((ipAddr & 0x0000FF00) >> 8); +#else + return (uint8_t)((ipAddr & 0x00FF0000) >> 16); +#endif +} + +/*! +* Return the first D field of of an IP v4 address of the form A.B.C.D +* \param[in] ipAddr An sbgIpAddress to convert. +* \return The D field of the IP address. +*/ +SBG_INLINE uint8_t sbgIpAddrGetD(sbgIpAddress ipAddr) +{ +#if SBG_CONFIG_BIG_ENDIAN == 1 + return (uint8_t)((ipAddr & 0x000000FF)); +#else + return (uint8_t)((ipAddr & 0xFF000000) >> 24); +#endif +} + +//----------------------------------------------------------------------// +//- IP manipulation methods -// +//----------------------------------------------------------------------// + +/*! + * Convert an ip to a string of the form A.B.C.D + * \param[in] ipAddr IP address to convert to a string. + * \param[out] pBuffer Pointer on an allocated buffer than can hold ip address as a string. + * \param[in] maxSize Maximum number of chars that can be stored in pBuffer including the NULL char. + */ +SBG_COMMON_LIB_API void sbgNetworkIpToString(sbgIpAddress ipAddr, char *pBuffer, size_t maxSize); + +/*! + * Convert an ip address stored in a string of the form A.B.C.D to an sbgIpAddress object. + * \param[in] pBuffer IP address as a string of the form A.B.C.D + * \return IP address parsed from the string or 0.0.0.0 if the IP is invalid. + */ +SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer); + +//----------------------------------------------------------------------// +//- IP operations -// +//----------------------------------------------------------------------// + +/*! + * Given an ip address and the netmask, returns the network part (ip & subnetMask) + * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). + * \param[in] netmask The netmask stored in an uint32_t (host endianness). + * \return The network part of the ip address. + */ +SBG_INLINE sbgIpAddress sbgIpGetNetworkAddr(sbgIpAddress ipAddress, sbgIpAddress netmask) +{ + return (ipAddress & netmask); +} + +/*! + * Given an ip address and the netmask, returns the host part (ip & ~subnetMask) + * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). + * \param[in] netmask The netmask stored in an uint32_t (host endianness). + * \return The host part of the ip address. + */ +SBG_INLINE sbgIpAddress sbgIpGetHostAddr(sbgIpAddress ipAddress, sbgIpAddress netmask) +{ + return (ipAddress & ~netmask); +} + +//----------------------------------------------------------------------// +//- IP validation methods -// +//----------------------------------------------------------------------// + +/*! + * Returns true if the provided IP address is unspecified ie (0.0.0.0) + * \param[in] ipAddress The ip address to test + * \return true if the ip address is unspecified or false otherwise. + */ +SBG_INLINE bool sbgIpAddressIsUnspecified(sbgIpAddress ipAddress) +{ + if (ipAddress == 0) + { + return true; + } + else + { + return false; + } +} + +/*! + * Check if an IpV4 address is valid. The ip address format is A.B.C.D and A should respect 0 < A < 224 + * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). + * \return true if the ip address is valid ie contiguous. + */ +SBG_INLINE bool sbgIpAddressValid(sbgIpAddress ipAddress) +{ + // + // Check the if A part of the ip address is within 1 and 223 + // + if ((sbgIpAddrGetA(ipAddress) > 0) && (sbgIpAddrGetA(ipAddress) < 224)) + { + // + // The ip address is valid + // + return true; + } + else + { + // + // The ip address is not valid + // + return false; + } +} + +/*! + * Given an ip address and the netmask, returns true if this ip address is within the subnet. + * \param[in] ipAddress The ip address stored in an uint32_t (host endianness). + * \param[in] netmask The netmask stored in an uint32_t (host endianness). + * \return true if this ip address is within the subnet or false otherwise. + */ +SBG_INLINE sbgIpAddress sbgIpAddrWithinSubnet(sbgIpAddress ipAddress, sbgIpAddress netmask) +{ + // + // Just check if the host part is equals to zero + // + if (sbgIpGetHostAddr(ipAddress, netmask) == 0) + { + return false; + } + else + { + return true; + } +} + +/*! + * Check if two ip addresses are in the same network given the subnet. + * \param[in] firstIpAddr The first IP address to check. + * \param[in] secondIpAddr The second IP address to check. + * \param[in] netmask The netmask of the network. + * \return true if the two ip addresses are in the same network. + */ +SBG_INLINE bool sbgIpAddrIsSameNetwork(sbgIpAddress firstIpAddr, sbgIpAddress secondIpAddr, sbgIpAddress netmask) +{ + if ((firstIpAddr & netmask) == (secondIpAddr & netmask)) + { + return true; + } + else + { + return false; + } +} + +/*! + * Check if an IpV4 netmask is valid, the mask should be contiguous (1111 followed by 0) + * \param[in] netmask The netmask stored in an uint32_t (host endianness). + * \return true if the netmask is valid ie contiguous. + */ +SBG_COMMON_LIB_API bool sbgIpNetMaskValid(sbgIpAddress netmask); + + +#ifdef __cplusplus +} +#endif + +#endif // SBG_NETWORK_H diff --git a/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.c b/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.c new file mode 100644 index 0000000..7284e43 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.c @@ -0,0 +1,30 @@ +// sbgCommonLib headers +#include + +//----------------------------------------------------------------------// +//- Include specific header for WIN32 and UNIX platforms -// +//----------------------------------------------------------------------// + + +//----------------------------------------------------------------------// +//- Global singleton for the log callback -// +//----------------------------------------------------------------------// + +/*! + * Unique singleton used to log error messages. + */ +SbgCommonLibOnLogFunc gLogCallback = NULL; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + + +SBG_COMMON_LIB_API void sbgCommonLibSetLogCallback(SbgCommonLibOnLogFunc logCallback) +{ + // + // TODO: should we implement lock / sync mechanisms ? + // + gLogCallback = logCallback; +} + diff --git a/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.h b/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.h new file mode 100644 index 0000000..6adce07 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/platform/sbgPlatform.h @@ -0,0 +1,104 @@ +/*! + * \file sbgPlatform.h + * \ingroup common + * \author SBG Systems + * \date March 17, 2015 + * + * \brief Platform-specific functions. + * + * This file should be modified to each targeted platform. + * For example, all common headers should be included from this file. + * + * The platform endianness should be defined here. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_PLATFORM_H +#define SBG_PLATFORM_H + +// sbgCommonLib headers +#include +#include +#include + +//----------------------------------------------------------------------// +//- Function pointer definitions -// +//----------------------------------------------------------------------// + +/*! + * Type for logging functions. + * + * \param[in] pFileName File name where the error occurred. + * \param[in] pFunctionName Function name where the error occurred. + * \param[in] line Line number where the error occurred. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] logType Define if we have an error, a warning, an info or a debug log. + * \param[in] errorCode The error code associated with the message. + * \param[in] pMessage The message to log. + */ +typedef void (*SbgCommonLibOnLogFunc)(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pMessage); + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Get the current time. + * + * \return The current time, in ms. + */ +SBG_COMMON_LIB_API uint32_t sbgGetTime(void); + +/*! + * Sleep. + * + * \param[in] ms Time to wait, in ms. + */ +SBG_COMMON_LIB_API void sbgSleep(uint32_t ms); + +/*! + * Set the log function. + * + * Some platforms may not provide the ability to set a user-provided log function, in which + * case this function does nothing. + * + * \param[in] logCallback Log function. + */ +SBG_COMMON_LIB_API void sbgCommonLibSetLogCallback(SbgCommonLibOnLogFunc logCallback); + +/*! + * Log a message. + * + * \param[in] pFileName File name where the error occurred. + * \param[in] pFunctionName Function name where the error occurred. + * \param[in] line Line number where the error occurred. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] logType Define if we have an error, a warning, an info or a debug log. + * \param[in] errorCode The error code associated with the message. + * \param[in] pFormat The error message that will be used with the variable list of arguments. + */ +SBG_COMMON_LIB_API void sbgPlatformDebugLogMsg(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 7, 8); + +#endif // SBG_PLATFORM_H diff --git a/crates/sbg-rs/sbgECom/common/sbgCommon.c b/crates/sbg-rs/sbgECom/common/sbgCommon.c new file mode 100644 index 0000000..9691fde --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgCommon.c @@ -0,0 +1,28 @@ +// sbgCommonLib headers +#include + +#ifdef USE_BUILD_NUMBER_GENERATOR +#include +#endif + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API uint32_t sbgCommonLibGetVersion(void) +{ +#ifdef USE_BUILD_NUMBER_GENERATOR + return SBG_COMMON_LIB_VERSION; +#else + return 0; +#endif +} + +SBG_COMMON_LIB_API bool sbgCommonLibIsDebug(void) +{ +#ifdef NDEBUG + return false; +#else + return true; +#endif +} diff --git a/crates/sbg-rs/sbgECom/common/sbgCommon.h b/crates/sbg-rs/sbgECom/common/sbgCommon.h new file mode 100644 index 0000000..c2cc8a1 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgCommon.h @@ -0,0 +1,148 @@ +/*! + * \file sbgCommon.h + * \ingroup common + * \author SBG Systems + * \date March 17, 2015 + * + * \brief Main header for the SBG Systems common C library. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +/*! + * \defgroup common Common + * \brief SBG Systems foundation framework that is common to all C/C++ projects. + */ + +#ifndef SBG_COMMON_H +#define SBG_COMMON_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "sbgConfig.h" + +//----------------------------------------------------------------------// +//- Default configuration -// +//----------------------------------------------------------------------// + +/*! + * If set to 0, the platform support only aligned memory access. + * If set to 1, the platform support unaligned memory access. + * Default: Support only Aligned access - Disabled + */ +#ifndef SBG_CONFIG_UNALIGNED_ACCESS_AUTH +#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) +#endif + +/*! + * If set to 0, the platform is using little endian. + * If set to 1, the platform is using big endian. + * Default: Little Endian - Disabled + */ +#ifndef SBG_CONFIG_BIG_ENDIAN +#define SBG_CONFIG_BIG_ENDIAN (0) +#endif + +/*! + * If set to 1, error logs level will be thrown. + * Default: Enabled + */ +#ifndef SBG_CONFIG_ENABLE_LOG_ERROR +#define SBG_CONFIG_ENABLE_LOG_ERROR (1) +#endif + +/*! + * If set to 1, warning logs level will be thrown. + * Default: Enabled + */ +#ifndef SBG_CONFIG_ENABLE_LOG_WARNING +#define SBG_CONFIG_ENABLE_LOG_WARNING (1) +#endif + +/*! + * If set to 1, information logs level will be thrown. + * Default: Enabled + */ +#ifndef SBG_CONFIG_ENABLE_LOG_INFO +#define SBG_CONFIG_ENABLE_LOG_INFO (1) +#endif + +/*! + * If set to 1, debug logs level will be thrown. + * Default: Enabled + */ +#ifndef SBG_CONFIG_ENABLE_LOG_DEBUG +#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) +#endif + +/*! + * Maximum error message size in bytes that can be generated including the NULL Char. + * Default: 1024 + */ +#ifndef SBG_CONFIG_LOG_MAX_SIZE +#define SBG_CONFIG_LOG_MAX_SIZE ((size_t)(1024)) +#endif + +/*! + * Maximum number of chars for a file name including the NULL char. + * Default: 256 + */ +#ifndef SBG_CONFIG_PATH_MAX_SIZE +#define SBG_CONFIG_PATH_MAX_SIZE ((size_t)(256)) +#endif + +//----------------------------------------------------------------------// +//- Headers -// +//----------------------------------------------------------------------// + +#include "sbgDefines.h" +#include "sbgErrorCodes.h" +#include "sbgTypes.h" + +#include "debug/sbgDebug.h" +#include "platform/sbgPlatform.h" + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Retreive the current sbgCommonLib encoded version. + * \return The current sbgCommonLib version + */ +SBG_COMMON_LIB_API uint32_t sbgCommonLibGetVersion(void); + +/*! + * Tell if the library is compiled in debug mode or not + * \return True if it's compiled in debug, False otherwise + */ +SBG_COMMON_LIB_API bool sbgCommonLibIsDebug(void); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_COMMON_H diff --git a/crates/sbg-rs/sbgECom/common/sbgConfig.h b/crates/sbg-rs/sbgECom/common/sbgConfig.h new file mode 100644 index 0000000..b6763b5 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgConfig.h @@ -0,0 +1,64 @@ +/*! + * \file sbgConfig.h + * \author SBG Systems (Raphael Siryani) + * \date 17 March 2015 + * + * \brief Header file used to configure the framework. + * + * You can configure for example the logging system. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_CONFIG_H +#define SBG_CONFIG_H + +//----------------------------------------------------------------------// +//- Platform configuration -// +//----------------------------------------------------------------------// + +/*! + * Windows x86 & x64 support both aligned and unaligned access + */ +#define SBG_CONFIG_UNALIGNED_ACCESS_AUTH (0) + +/*! + * Windows is using little endianess + */ +#define SBG_CONFIG_BIG_ENDIAN (0) + +//----------------------------------------------------------------------// +//- Logging configuration -// +//----------------------------------------------------------------------// + +/*! + * Enable all error logs + * Default: Enabled + */ +#define SBG_CONFIG_ENABLE_LOG_ERROR (1) +#define SBG_CONFIG_ENABLE_LOG_WARNING (1) +#define SBG_CONFIG_ENABLE_LOG_INFO (1) +#define SBG_CONFIG_ENABLE_LOG_DEBUG (1) + +#endif // SBG_CONFIG_H diff --git a/crates/sbg-rs/sbgECom/common/sbgDefines.h b/crates/sbg-rs/sbgECom/common/sbgDefines.h new file mode 100644 index 0000000..c019c8a --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgDefines.h @@ -0,0 +1,524 @@ +/*! + * \file sbgDefines.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 + * + * \brief Header file that contains all common definitions. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_DEFINES_H +#define SBG_DEFINES_H + +// Standard headers +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Local headers +#include "sbgConfig.h" + +// +// XXX If NDEBUG is defined, most libraries define assert() as ((void)0), which may +// cause "defined but not used" warnings. Redefine assert() in a way that safely +// prevents this warning, i.e. without triggering the expression side effects. +// + +#undef assert +#define assert(expression) ((void)sizeof(expression)) + +/*! + * Macro used to handle export and import methods of the sbgCommon library + */ +#ifdef _MSC_VER + #ifdef SBG_COMMON_STATIC_USE + #define SBG_COMMON_LIB_API + #else + #ifdef SBG_COMMON_LIB_API_EXPORT + #define SBG_COMMON_LIB_API __declspec(dllexport) + #else + #define SBG_COMMON_LIB_API __declspec(dllimport) + #endif + #endif +#else + #define SBG_COMMON_LIB_API +#endif + +//----------------------------------------------------------------------// +//- Global definitions -// +//----------------------------------------------------------------------// +#ifndef SBG_DISABLE + #define SBG_DISABLE (0) +#endif + +#ifndef SBG_ENABLE + #define SBG_ENABLE (1) +#endif + +#ifndef FALSE + #define FALSE (0) +#endif + +#ifndef TRUE + #define TRUE (1) +#endif + +#ifndef NULL + #define NULL (0) +#endif + +#ifndef SBG_INVALID_HANDLE + #define SBG_INVALID_HANDLE (NULL) +#endif + +#ifndef SBG_ARRAY_SIZE + #define SBG_ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) +#endif + +#ifndef SBG_STRLEN + #define SBG_STRLEN(s) (sizeof(s) - 1) +#endif + +#ifndef SBG_QUOTE_NX + #define SBG_QUOTE_NX(x) #x +#endif + +#ifndef SBG_QUOTE + #define SBG_QUOTE(x) SBG_QUOTE_NX(x) +#endif + +#ifndef SBG_CONCAT_NX + #define SBG_CONCAT_NX(a, b) a ## b +#endif + +#ifndef SBG_CONCAT + #define SBG_CONCAT(a, b) SBG_CONCAT_NX(a, b) +#endif + +#ifndef SBG_UNPACK + #define SBG_UNPACK(...) __VA_ARGS__ +#endif + +#ifndef SBG_CONTAINER_OF + #define SBG_CONTAINER_OF(ptr, type, member) ((type *)((char *)(ptr) - offsetof(type, member))) +#endif + +#ifndef SBG_LIKELY + #if defined(__GNUC__) || defined(__clang__) + #define SBG_LIKELY(expr) __builtin_expect((bool)(expr), true) + #else + #define SBG_LIKELY(expr) (expr) + #endif +#endif + +#ifndef SBG_UNLIKELY + #if defined(__GNUC__) || defined(__clang__) + #define SBG_UNLIKELY(expr) __builtin_expect((bool)(expr), false) + #else + #define SBG_UNLIKELY(expr) (expr) + #endif +#endif + +#ifndef SBG_CHECK_FORMAT + #if defined(__GNUC__) || defined(__clang__) + #define SBG_CHECK_FORMAT(style, format_index, va_args) __attribute__((format(style, format_index, va_args))) + #else + #define SBG_CHECK_FORMAT(style, format_index, va_args) + #endif +#endif + +/*! + * GCC typeof C extension + * + * XXX Visual C (not C++) doesn't provide anything to implement typeof(). + * As a result, this macro is private and shouldn't be relied on. + */ +#ifndef __SBG_TYPEOF + #ifdef __cplusplus + #define __SBG_TYPEOF(x) decltype(x) + #elif defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) + #define __SBG_TYPEOF(x) typeof(x) + #endif +#endif + +#ifndef SBG_CONST_CAST_AA + #ifdef __SBG_TYPEOF + #define SBG_CONST_CAST_AA(x) ((const __SBG_TYPEOF((x)[0][0])(*)[SBG_ARRAY_SIZE((x)[0])])(x)) + #else + #define SBG_CONST_CAST_AA(x) x + #endif +#endif + +#ifndef SBG_CONST_CAST_PP + #ifdef __SBG_TYPEOF + #define SBG_CONST_CAST_PP(x) ((const __SBG_TYPEOF(**(x))**)(x)) + #else + #define SBG_CONST_CAST_PP(x) x + #endif +#endif + +/*! + * __BASE_FILE__ is gcc specific + */ +#if !defined(__GNUC__) && !defined(__clang__) +#ifndef __BASE_FILE__ + #define __BASE_FILE__ __FILE__ +#endif +#endif + +#ifdef __cplusplus + #define SBG_DELETE(p) if (p){delete (p); (p) = NULL;} + #define SBG_DELETE_ARRAY(p) if (p){delete[] (p); (p) = NULL;} + #define SBG_FREE(p) if (p){free(p); (p) = NULL;} + #define SBG_FREE_ARRAY(p) if (p){free(p); (p) = NULL;} +#else + #define SBG_DELETE if (p){free(p); (p) = NULL;} + #define SBG_DELETE_ARRAY if (p){free(p); (p) = NULL;} + #define SBG_FREE(p) if (p){free(p); (p) = NULL;} + #define SBG_FREE_ARRAY(p) if (p){free(p); (p) = NULL;} +#endif + +//----------------------------------------------------------------------// +//- Compiler definitions -// +//----------------------------------------------------------------------// + +/*! + * Macro used to abstract the compiler specific inline keyword. + */ +#ifndef SBG_INLINE + #if defined(_MSC_VER) + #define SBG_INLINE __inline + #else + #define SBG_INLINE static inline + #endif +#endif + +/*! + * Macro used to avoid compiler warning when a variable is not used. + */ +#ifndef SBG_UNUSED_PARAMETER + #define SBG_UNUSED_PARAMETER(x) (void)(x) +#endif + +/*! + * Compiler independent switch/case fallthrough attribute + * + * The fallthrough attribute is used to avoid compiler warning in swith case statements + * when an intentional break is missing + */ +#ifndef SBG_FALLTHROUGH + #if __cplusplus >= 201703L + #define SBG_FALLTHROUGH [[fallthrough]] /* introduced in C++ 17 */ + #elif defined(__GNUC__) || defined(__clang__) + #if (__GNUC__ >= 7) || defined(__clang__) + #define SBG_FALLTHROUGH __attribute__((fallthrough)) + #else + #define SBG_FALLTHROUGH ((void)0) + #endif + #else + #define SBG_FALLTHROUGH + #endif +#endif + +//----------------------------------------------------------------------// +//- Macro used to defined packed structures -// +//----------------------------------------------------------------------// + +/*! + * Compiler independent struct members packing attribute + * + * This macro is used to define a new section of packed structures. + * All structures defined after this macro will be packed. + */ +#if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) + #define SBG_BEGIN_PACKED() +#elif defined(_MSC_VER) + #define SBG_BEGIN_PACKED() __pragma(pack(push, 1)) +#else + #error you must byte-align these structures with the appropriate compiler directives +#endif + +/*! + * Compiler independent struct members packing attribute + * + * This macro is used to specify that a structure is packed. + */ +#if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) + #define SBG_PACKED __attribute__((packed)) +#elif defined(_MSC_VER) + #define SBG_PACKED +#else + #error you must byte-align these structures with the appropriate compiler directives +#endif + +/*! + * Compiler independent struct members packing attribute + * + * This macro is used to close the section of packed structures and return to the default packing. + */ +#if defined(__GNUC__) || defined(__clang__) || defined(__TI_COMPILER_VERSION__) + #define SBG_END_PACKED() +#elif defined(_MSC_VER) + #define SBG_END_PACKED() __pragma(pack(pop)) +#else + #error you must byte-align these structures with the appropriate compiler directives +#endif + +//----------------------------------------------------------------------// +//- Deprecation definitions -// +//----------------------------------------------------------------------// + +/*! + * Macro used to indicate that a function is deprecated. + */ +#if defined(__GNUC__) || defined(__clang__) + #define SBG_DEPRECATED(func) __attribute__((deprecated)) func +#elif defined(__TI_COMPILER_VERSION__) + #define SBG_DEPRECATED(func) func __attribute__((deprecated)) +#elif defined(_MSC_VER) + #define SBG_DEPRECATED(func) __declspec(deprecated) func +#else + #define SBG_DEPRECATED(func) func +#endif + +/*! + * Macro used to indicate that a macro is deprecated. + */ +#if defined(__GNUC__) || defined(__clang__) + #define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) +#elif defined(_MSC_VER) +#define SBG_DEPRECATED_MACRO(func) __pragma(deprecated(func)) +#else + #define SBG_DEPRECATED_MACRO(func) func +#endif + +/*! + * Set the default value of SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES. + */ +#ifndef SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES +#define SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES (1) +#endif + +/*! + * Macro used to indicate that a type definition is deprecated. + * + * XXX In order to avoid excessive noise caused by deprecation warnings, the attribute + * may currently be disabled by defining SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES to 0. + */ +#if SBG_CONFIG_WARN_ABOUT_DEPRECATED_TYPES != 0 + #if defined(__GNUC__) || defined(__clang__) + #define SBG_DEPRECATED_TYPEDEF(decl) decl __attribute__((deprecated)) + #elif defined(_MSC_VER) + #define SBG_DEPRECATED_TYPEDEF(decl) __declspec(deprecated) decl + #else + #define SBG_DEPRECATED_TYPEDEF(decl) decl + #endif +#else + #define SBG_DEPRECATED_TYPEDEF(decl) decl +#endif + +//----------------------------------------------------------------------// +//- Basic maths definitions -// +//----------------------------------------------------------------------// +#ifndef SBG_PI + #define SBG_PI 3.14159265358979323846 +#endif + +#ifndef SBG_PI_F + #define SBG_PI_F 3.14159265358979323846f +#endif + +/*! + * Returns the absolute value of x. + * + * \param[in] x Signed integer value. + * \return The absolute value of x. + */ +#ifndef sbgAbs + #define sbgAbs(x) (((x) < 0) ? -(x) : (x)) +#endif + +/*! + * Returns the maximum between a and b + * + * \param[in] a First operand. + * \param[in] b Second operand. + * \return The maximum between a and b. + */ +#ifndef sbgMax + #define sbgMax(a,b) (((a) > (b)) ? (a) : (b)) +#endif + +/*! + * Returns the minimum between a and b + * + * \param[in] a First operand. + * \param[in] b Second operand. + * \return The minimum between a and b. + */ +#ifndef sbgMin + #define sbgMin(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +/*! + * Clamp a value between minValue and maxValue ie minValue <= value <= maxValue + * + * \param[in] value First operand. + * \param[in] minValue First operand. + * \param[in] maxValue Second operand. + * \return The clamped value. + */ +#ifndef sbgClamp + #define sbgClamp(value, minValue, maxValue) (((value) < (minValue))?(minValue): ((value) > (maxValue)?maxValue:value)) +#endif + +/*! + * Integer division with a result rounded up. + * + * \param[in] n Dividend. + * \param[in] d Divisor. + * \return Rounded division + */ +#ifndef sbgDivCeil + #define sbgDivCeil(n, d) (((n) + (d) - 1) / (d)) +#endif + +/*! + * Convert an angle from radians to degrees using double precision. + * + * \param[in] angle The angle to convert in radians. + * \return The converted angle in degrees. + */ +SBG_INLINE double sbgRadToDegd(double angle) +{ + return angle * 180.0 / SBG_PI; +} + +/*! + * Convert an angle from degrees to radians using double precision. + * + * \param[in] angle The angle to convert in degrees. + * \return The converted angle in radians. + */ +SBG_INLINE double sbgDegToRadd(double angle) +{ + return angle * SBG_PI / 180.0; +} + +/*! + * Convert an angle from radians to degrees using single (float) precision. + * + * \param[in] angle The angle to convert in radians. + * \return The converted angle in degrees. + */ +SBG_INLINE float sbgRadToDegf(float angle) +{ + return angle * 180.0f / SBG_PI_F; +} + +/*! + * Convert an angle from degrees to radians using single (float) precision. + * + * \param[in] angle The angle to convert in degrees. + * \return The converted angle in radians. + */ +SBG_INLINE float sbgDegToRadf(float angle) +{ + return angle * SBG_PI_F / 180.0f; +} + +/*! + * Test if two floating single-point numbers are equals or not. + * + * \param[in] leftValue The first operand to test for equality. + * \param[in] rightValue The second operand to test for equality. + * \return true if both left and right operands are almost equal. + */ +SBG_INLINE bool sbgAlmostEqualsFloat(float leftValue, float rightValue) +{ + // + // The IEEE standard says that any comparison operation involving a NAN must return false. + // + if (isnan(leftValue) || isnan(rightValue)) + { + return false; + } + + // + // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods + // + if (fabsf(leftValue - rightValue) < FLT_EPSILON) + { + return true; + } + else + { + return false; + } +} + +/*! + * Test if two floating double-point numbers are equals or not using the epsilon technique + * + * \param[in] leftValue The first operand to test for equality. + * \param[in] rightValue The second operand to test for equality. + * \return true if both left and right operands are almost equal. + */ +SBG_INLINE bool sbgAlmostEqualsDouble(double leftValue, double rightValue) +{ + // + // The IEEE standard says that any comparison operation involving a NAN must return false. + // + if (isnan(leftValue) || isnan(rightValue)) + { + return false; + } + + // + // This method is not good enough and should be updated using DistanceBetweenSignAndMagnitudeNumbers methods + // + if (fabs(leftValue - rightValue) < DBL_EPSILON) + { + return true; + } + else + { + return false; + } +} + +#endif // SBG_DEFINES_H diff --git a/crates/sbg-rs/sbgECom/common/sbgErrorCodes.h b/crates/sbg-rs/sbgECom/common/sbgErrorCodes.h new file mode 100644 index 0000000..b31d7c9 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgErrorCodes.h @@ -0,0 +1,119 @@ +/*! + * \file sbgErrorCodes.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 + * + * \brief Header file that defines all error codes for SBG Systems libraries. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ERROR_CODES_H +#define SBG_ERROR_CODES_H + +// Local headers +#include "sbgDefines.h" + +//----------------------------------------------------------------------// +//- Errors code definitions -// +//----------------------------------------------------------------------// + +/*! + * Generic errors definitions for SBG Systems projects. + */ +typedef enum _SbgErrorCode +{ + SBG_NO_ERROR = 0, /*!< The operation was successfully executed. */ + SBG_ERROR, /*!< We have a generic error. */ + SBG_NULL_POINTER, /*!< A pointer is null. */ + SBG_INVALID_CRC, /*!< The received frame has an invalid CRC. */ + SBG_INVALID_FRAME, /*!< The received frame is invalid
*/ + /*!< We have received an unexpected frame (not the cmd we are waiting for or with an invalid data size.
*/ + /*!< This could be caused by a desync between questions and answers.
*/ + /*!< You should flush the serial port to fix this. */ + SBG_TIME_OUT, /*!< We have started to receive a frame but not the end. */ + SBG_WRITE_ERROR, /*!< All bytes hasn't been written. */ + SBG_READ_ERROR, /*!< All bytes hasn't been read. */ + SBG_BUFFER_OVERFLOW, /*!< A buffer is too small to contain so much data. */ + SBG_INVALID_PARAMETER, /*!< An invalid parameter has been found. */ + SBG_NOT_READY, /*!< A device isn't ready (Rx isn't ready for example). */ + SBG_MALLOC_FAILED, /*!< Failed to allocate a buffer. */ + SGB_CALIB_MAG_NOT_ENOUGH_POINTS, /*!< Not enough points were available to perform magnetometers calibration. */ + SBG_CALIB_MAG_INVALID_TAKE, /*!< The calibration procedure could not be properly executed due to insufficient precision. */ + SBG_CALIB_MAG_SATURATION, /*!< Saturation were detected when attempt to calibrate magnetos. */ + SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE, /*!< 2D calibration procedure could not be performed. */ + + SBG_DEVICE_NOT_FOUND, /*!< A device couldn't be founded or opened PC only error code */ + SBG_OPERATION_CANCELLED, /*!< An operation was canceled. PC only error code*/ + SBG_NOT_CONTINUOUS_FRAME, /*!< We have received a frame that isn't a continuous one. PC only error code*/ + + SBG_INCOMPATIBLE_HARDWARE, /*!< Hence valid; the command cannot be executed because of hardware incompatibility */ + SBG_INVALID_VERSION /*!< Incompatible version */ +} SbgErrorCode; + +//----------------------------------------------------------------------// +//- Error codes to string litteral conversion -// +//----------------------------------------------------------------------// + +/*! + * According to an error code, returns a human readable string. + * \param[in] errorCode The errorCode to convert to a string. + * \return Read only corresponding string. + */ +static inline const char *sbgErrorCodeToString(SbgErrorCode errorCode) +{ + /*! + * Array of string litterals that should be exactly ordered as the SbgErrorCode enum. + */ + static const char *sbgErrorCodeString[] = + { + "SBG_NO_ERROR", + "SBG_ERROR", + "SBG_NULL_POINTER", + "SBG_INVALID_CRC", + "SBG_INVALID_FRAME", + "SBG_TIME_OUT", + "SBG_WRITE_ERROR", + "SBG_READ_ERROR", + "SBG_BUFFER_OVERFLOW", + "SBG_INVALID_PARAMETER", + "SBG_NOT_READY", + "SBG_MALLOC_FAILED", + "SGB_CALIB_MAG_NOT_ENOUGH_POINTS", + "SBG_CALIB_MAG_INVALID_TAKE", + "SBG_CALIB_MAG_SATURATION", + "SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE", + "SBG_DEVICE_NOT_FOUND", + "SBG_OPERATION_CANCELLED", + "SBG_NOT_CONTINUOUS_FRAME", + "SBG_INCOMPATIBLE_HARDWARE", + "SBG_INVALID_VERSION" + }; + + assert(errorCode < SBG_ARRAY_SIZE(sbgErrorCodeString)); + return sbgErrorCodeString[errorCode]; +} + +#endif /* SBG_ERROR_CODES_H */ diff --git a/crates/sbg-rs/sbgECom/common/sbgTypes.h b/crates/sbg-rs/sbgECom/common/sbgTypes.h new file mode 100644 index 0000000..ca068ca --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/sbgTypes.h @@ -0,0 +1,194 @@ +/*! + * \file sbgTypes.h + * \ingroup common + * \author SBG Systems + * \date 17 March 2015 + * + * \brief Header file that defines all scalar types. + * + * The platform endianness should be defined here. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_TYPES_H +#define SBG_TYPES_H + +// Standard headers +#include + +// Local headers +#include "sbgDefines.h" + +//----------------------------------------------------------------------// +//- Limits definitions -// +//----------------------------------------------------------------------// +#define SBG_MIN_INT_24 (-8388608l) +#define SBG_MAX_INT_24 (8388607l) +#define SBG_MAX_UINT_24 (16777215ul) + +#define SBG_MIN_INT_40 (-549755813887ll - 1) +#define SBG_MAX_INT_40 (549755813887ll) +#define SBG_MAX_UINT_40 (1099511627775ull) + +#define SBG_MIN_INT_48 (-140737488355327ll - 1) +#define SBG_MAX_INT_48 (140737488355327ll) +#define SBG_MAX_UINT_48 (281474976710655ull) + +#define SBG_MIN_INT_56 (-36028797018963967ll - 1) +#define SBG_MAX_INT_56 (36028797018963967ll) +#define SBG_MAX_UINT_56 (72057594037927935ull) + +//----------------------------------------------------------------------// +//- DEPRECATED: Scalar types definitions -// +//----------------------------------------------------------------------// +SBG_DEPRECATED_TYPEDEF(typedef unsigned char uint8); // 8 bits +SBG_DEPRECATED_TYPEDEF(typedef unsigned short uint16); // 16 bits +SBG_DEPRECATED_TYPEDEF(typedef unsigned int uint32); // 32 bits +SBG_DEPRECATED_TYPEDEF(typedef unsigned long long int uint64); // 64 bits + +SBG_DEPRECATED_TYPEDEF(typedef signed char int8); // 8 bits +SBG_DEPRECATED_TYPEDEF(typedef signed short int16); // 16 bits +SBG_DEPRECATED_TYPEDEF(typedef signed int int32); // 32 bits +SBG_DEPRECATED_TYPEDEF(typedef signed long long int int64); // 64 bits + + +//----------------------------------------------------------------------// +//- Misc types definitions -// +//----------------------------------------------------------------------// +typedef uint32_t sbgIpAddress; /*!< Define an IP v4 address stored in 4 bytes. The format is A.B.C.D, each component is 8 bits and stored in Big Endian. */ + +//------------------------------------------------------------------// +//- Type punning safe conversion unions -// +//------------------------------------------------------------------// + +/*! + * Used to get a uint32_t from a uint8_t array. + */ +typedef union _Uint8PtrToUint32Ptr +{ + uint8_t *m_pointerUint8; /*!< Set the address used to access the uint32_t. */ + uint32_t *m_pointerUint32; /*!< Store the unint32 value. */ +} Uint8PtrToUint32Ptr; + +/*! + * Union used to convert a buffer or 2 unit8 two's complement values to a int16_t + */ +typedef union _Uint8ToInt16 +{ + int16_t value; + uint8_t buffer[2]; +} Uint8ToInt16; + +/*! + * Union used to convert a buffer or 2 unit8 values to a uint16_t + */ +typedef union _Uint8ToUint16 +{ + uint16_t value; + uint8_t buffer[2]; +} Uint8ToUint16; + +/*! + * Union used to convert a buffer or 4 unit8 two's complement values to a int32_t + */ +typedef union _Uint8ToInt32 +{ + int32_t value; + uint8_t buffer[4]; +} Uint8ToInt32; + +/*! + * Union used to convert a buffer or 4 unit8 values to a uint32_t + */ +typedef union _Uint8ToUint32 +{ + uint32_t value; + uint8_t buffer[4]; +} Uint8ToUint32; + +/*! + * Union used to convert a buffer or 8 unit8 two's complement values to a int64_t + */ +typedef union _Uint8ToInt64 +{ + int64_t value; + uint8_t buffer[8]; +} Uint8ToInt64; + +/*! + * Union used to convert a buffer or 8 unit8 values to a uint64_t + */ +typedef union _Uint8ToUint64 +{ + uint64_t value; + uint8_t buffer[8]; +} Uint8ToUint64; + +/*! + * Union that allows type punning (access to a floating point number bits) + */ +typedef union _FloatNint +{ + float valF; + int32_t valI; + uint32_t valU; +} FloatNint; + +/*! + * Union that allows type punning (access to a double number bits) + */ +typedef union _DoubleNint +{ + double valF; + uint64_t valU; + int64_t valI; +} DoubleNint; + +/*! + * Structure that splits a 64bits + */ +typedef struct _Split64 +{ + uint32_t high; + uint32_t low; +} Split64; + +/*! + * Set of 3 int32_t + */ + typedef struct _SbgVector3i + { + int32_t v[3]; + } SbgVector3i; + + /*! + * Set of 3 int64_t + */ + typedef struct _SbgVector3ll + { + int64_t v[3]; + } SbgVector3ll; + +#endif /* SBG_TYPES_H */ diff --git a/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.c b/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.c new file mode 100644 index 0000000..e43ad8d --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.c @@ -0,0 +1 @@ +#include "sbgSplitBuffer.h" diff --git a/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.h b/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.h new file mode 100644 index 0000000..fb32425 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/splitBuffer/sbgSplitBuffer.h @@ -0,0 +1,252 @@ +/*! + * \file sbgSplitBuffer.h + * \ingroup common + * \author SBG Systems + * \date 19 November 2013 + * + * \brief Helper methods used to handle a splittable buffer. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_SPLIT_BUFFER_H +#define SBG_SPLIT_BUFFER_H + +#include + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Structs definitions -// +//----------------------------------------------------------------------// + +/*! + * Defines the ECom transfer states + */ +typedef struct _SbgSplitBuffer +{ + uint8_t *pLinkedBuffer; /*!< Pointer on the buffer that needs to be split */ + size_t linkedBufferSize; /*!< Size of the original buffer */ + size_t subBufferSize; /*!< The size of the sub buffers */ + size_t subBufferNbr; /*!< The number of sub buffers in this split buffer */ +} SbgSplitBuffer; + +//----------------------------------------------------------------------// +//- Public methods definitions -// +//----------------------------------------------------------------------// + +/*! + * Initialize a split buffer for read operations. + * \param[in] pSplitBuffer Pointer to an allocated split buffer instance. + * \param[in] pBuffer The buffer that needs to be split (doesn't take the ownership). + * \param[in] bufferSize The total size of the buffer in bytes. + * \param[in] subBufferSize The size of each sub buffer in bytes. + */ +SBG_INLINE void sbgSplitBufferInitForRead(SbgSplitBuffer *pSplitBuffer, const void *pBuffer, size_t bufferSize, size_t subBufferSize) +{ + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Initialize split buffer parameters + // + pSplitBuffer->pLinkedBuffer = (uint8_t*)pBuffer; + pSplitBuffer->linkedBufferSize = bufferSize; + pSplitBuffer->subBufferSize = subBufferSize; + + // + // Compute sub buffer number + // + pSplitBuffer->subBufferNbr = (bufferSize + (subBufferSize - 1)) / subBufferSize; +} + +/*! + * Initialize a split buffer for write operations. + * \param[in] pSplitBuffer Pointer to an allocated split buffer instance. + * \param[in] pBuffer The buffer that needs to be split (doesn't take the ownership). + * \param[in] bufferSize The total size of the buffer in bytes. + * \param[in] subBufferSize The size of each sub buffer in bytes. + */ +SBG_INLINE void sbgSplitBufferInitForWrite(SbgSplitBuffer *pSplitBuffer, void *pBuffer, size_t bufferSize, size_t subBufferSize) +{ + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Initialize split buffer parameters + // + pSplitBuffer->pLinkedBuffer = (uint8_t*)pBuffer; + pSplitBuffer->linkedBufferSize = bufferSize; + pSplitBuffer->subBufferSize = subBufferSize; + + // + // Compute sub buffer number + // + pSplitBuffer->subBufferNbr = (bufferSize + (subBufferSize - 1)) / subBufferSize; +} + +/*! + * Returns the number of sub buffers that compose the whole buffer. + * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. + * \return The number of sub buffer the buffer has or 0 if there is an error. + */ +SBG_INLINE size_t sbgSplitBufferGetSubBufferNbr(const SbgSplitBuffer *pSplitBuffer) +{ + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Return subBufferNbr parameter + // + return pSplitBuffer->subBufferNbr; +} + +/*! + * Get one sub buffer given its index. + * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. + * \param[in] subBufferIdx Index of the sub buffer required. + * \return Pointer to the sub buffer or NULL if the subBuffer index is invalid. + */ +SBG_INLINE void *sbgSplitBufferGetSubBuffer(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) +{ + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Test input parameters + // + if (subBufferIdx < pSplitBuffer->subBufferNbr) + { + // + // Return pointer to buffer + // + return ((uint8_t*)pSplitBuffer->pLinkedBuffer + pSplitBuffer->subBufferSize*subBufferIdx); + } + else + { + // + // Invalid index + // + return NULL; + } +} + +/*! + * Return the offset in bytes of a sub buffer from the start of the buffer. + * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. + * \param[in] subBufferIdx Index of the sub buffer required. + * \return Offset to the sub buffer or 0 if the subBuffer index is invalid. + */ +SBG_INLINE size_t sbgSplitBufferGetSubBufferOffset(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) +{ + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Test input parameters + // + if (subBufferIdx < pSplitBuffer->subBufferNbr) + { + // + // Return pointer to buffer + // + return (pSplitBuffer->subBufferSize * subBufferIdx); + } + else + { + // + // Invalid index + // + return 0; + } +} + +/*! + * Get the size of a sub buffer given its index. + * \param[in] pSplitBuffer Valid pointer to a Split Buffer instance. + * \param[in] subBufferIdx Index of the sub buffer required. + * \return The size of the sub buffer of index subBufferIdx, or 0 if the subBuffer index is invalid. + */ +SBG_INLINE size_t sbgSplitBufferGetSubBufferSize(const SbgSplitBuffer *pSplitBuffer, size_t subBufferIdx) +{ + size_t subBufferSize = 0; + + // + // Test input arguments + // + assert(pSplitBuffer); + + // + // Test input parameters + // + if (pSplitBuffer->subBufferNbr > 0) + { + // + // Test that the sub buffer index is not the last one + // + if (subBufferIdx < (pSplitBuffer->subBufferNbr-1)) + { + // + // We can just return the sub buffer size because it's not the last sub buffer + // + subBufferSize = pSplitBuffer->subBufferSize; + } + else if (subBufferIdx == (pSplitBuffer->subBufferNbr-1) ) + { + // + // It's the last sub buffer so return the remaining size + // + subBufferSize = pSplitBuffer->linkedBufferSize - (subBufferIdx * pSplitBuffer->subBufferSize); + } + } + + // + // Return computed size + // + return subBufferSize; +} + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_SPLIT_BUFFER_H */ diff --git a/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.c b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.c new file mode 100644 index 0000000..9f4955a --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.c @@ -0,0 +1 @@ +#include "sbgStreamBuffer.h" diff --git a/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.h b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.h new file mode 100644 index 0000000..cebc45f --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBuffer.h @@ -0,0 +1,53 @@ +/*! + * \file sbgStreamBuffer.h + * \ingroup common + * \author SBG Systems + * \date 02 January 2013 + * + * \brief Used to read/write data from/to a memory buffer stream. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_STREAM_BUFFER_H +#define SBG_STREAM_BUFFER_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +#include "sbgStreamBufferLE.h" +#include "sbgStreamBufferBE.h" + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_STREAM_BUFFER_H */ diff --git a/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferBE.h b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferBE.h new file mode 100644 index 0000000..c14d89c --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferBE.h @@ -0,0 +1,1924 @@ +/*! + * \file sbgStreamBufferBE.h + * \ingroup common + * \author SBG Systems + * \date 17 February 2015 + * + * \brief Specific method of stream buffer for little endian readings/writings. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_STREAM_BUFFER_BE_H +#define SBG_STREAM_BUFFER_BE_H + +#include "sbgStreamBufferCommon.h" + +//----------------------------------------------------------------------// +//- Read operations methods -// +//----------------------------------------------------------------------// + +/*! + * Read an int16_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int16_t sbgStreamBufferReadInt16BE(SbgStreamBuffer *pHandle) +{ + int16_t bytesValues[2]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + bytesValues[0] = *((int16_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int16_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[0] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[1] | (bytesValues[0] << 8); + #else + return bytesValues[0] | (bytesValues[1] << 8); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint16_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint16_t sbgStreamBufferReadUint16BE(SbgStreamBuffer *pHandle) +{ + uint16_t bytesValues[2]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint16_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[0] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[1] | (bytesValues[0] << 8); + #else + return bytesValues[0] | (bytesValues[1] << 8); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int24 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int32_t sbgStreamBufferReadInt24BE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt32 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 24 bits + // + return value.value >> (32-24); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint24 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint32_t sbgStreamBufferReadUint24BE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint32 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 24 bits + // + return value.value >> (32-24); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int32_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int32_t sbgStreamBufferReadInt32BE(SbgStreamBuffer *pHandle) +{ + int32_t bytesValues[4]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int32_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[3] = *(pHandle->pCurrentPtr++); + bytesValues[2] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[0] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); + #else + return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint32_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint32_t sbgStreamBufferReadUint32BE(SbgStreamBuffer *pHandle) +{ + uint32_t bytesValues[4]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint32_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[3] = *(pHandle->pCurrentPtr++); + bytesValues[2] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[0] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); + #else + return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int40 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt40BE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 40 bits + // + return value.value >> (64-40); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint40 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint40BE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 40 bits + // + return value.value >> (64-40); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int48 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt48BE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 48 bits + // + return value.value >> (64-48); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint48 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint48BE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 48 bits + // + return value.value >> (64-48); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int56 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt56BE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 56 bits + // + return value.value >> (64-56); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint56 from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadUint56BE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB + #else + // + // Read the each bytes + // + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + #endif + + // + // Shift the value to handle the sign correctly for a 56 bits + // + return value.value >> (64-56); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int64_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt64BE(SbgStreamBuffer *pHandle) +{ + int64_t lowPart; + int64_t highPart; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + lowPart = *((int64_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int64_t); + + return lowPart; + #else + // + // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations + // + highPart = sbgStreamBufferReadUint32BE(pHandle); + lowPart = sbgStreamBufferReadUint32BE(pHandle); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return (lowPart << 32) | highPart; + #else + return lowPart | (highPart << 32); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0ll; +} + +/*! + * Read an uint64_t from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint64BE(SbgStreamBuffer *pHandle) +{ + uint64_t lowPart; + uint64_t highPart; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the current value + // + lowPart = *((uint64_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint64_t); + + return lowPart; + #else + // + // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations + // + highPart = sbgStreamBufferReadUint32BE(pHandle); + lowPart = sbgStreamBufferReadUint32BE(pHandle); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return (lowPart << 32) | highPart; + #else + return lowPart | (highPart << 32); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0ll; +} + +/*! + * Read a size_t from a stream buffer that has been stored in a uint32_t (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE size_t sbgStreamBufferReadSizeT32BE(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Just call the read method for uint32_t + // We assume that a size_t is at least 32 bits on all platforms + // + return (size_t)sbgStreamBufferReadUint32BE(pHandle); +} + +/*! + * Read a size_t from a stream buffer that has been stored in a uint64_t (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE size_t sbgStreamBufferReadSizeT64BE(SbgStreamBuffer *pHandle) +{ + uint64_t size; + + assert(pHandle); + + // + // Just call the read method for uint64_t + // + size = sbgStreamBufferReadUint64BE(pHandle); + + // + // Make sure the read size can fit in the size_t in size_t is 32 bits + // + assert((sizeof(size_t) == 8) || ((sizeof(size_t) == 4) && (size <= UINT32_MAX))); + + // + // Return the read value + // + return (size_t)size; +} + +/*! + * Read an float from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE float sbgStreamBufferReadFloatBE(SbgStreamBuffer *pHandle) +{ + FloatNint floatInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float)) + { + // + // Read the float as an uint32_t + // + floatInt.valU = sbgStreamBufferReadUint32BE(pHandle); + + // + // Return the float using an union to avoid compiller cast + // + return floatInt.valF; + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0.0f; +} + +/*! + * Read an double from a stream buffer (Big endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE double sbgStreamBufferReadDoubleBE(SbgStreamBuffer *pHandle) +{ + DoubleNint doubleInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double)) + { + // + // Read the float as an uint64_t + // + doubleInt.valU = sbgStreamBufferReadUint64BE(pHandle); + + // + // Return the double using an union to avoid compiller cast + // + return doubleInt.valF; + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0.0; +} + +//----------------------------------------------------------------------// +//- Write operations methods -// +//----------------------------------------------------------------------// + +/*! + * Write an int16_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt16BE(SbgStreamBuffer *pHandle, int16_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((int16_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int16_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint16_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16BE(SbgStreamBuffer *pHandle, uint16_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((uint16_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint16_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an int24 into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt24BE(SbgStreamBuffer *pHandle, int32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Make sure that the value is within 24 bit bonds + // + if ( (value >= SBG_MIN_INT_24) && (value <= SBG_MAX_INT_24) ) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(int8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + else + { + // + // The input value is not within a 24 bit integer bounds + // + pHandle->errorCode = SBG_INVALID_PARAMETER; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint24 into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint24BE(SbgStreamBuffer *pHandle, uint32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Make sure that the value is within 24 bit bonds + // + if (value <= SBG_MAX_UINT_24) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + else + { + // + // The input value is not within a 24 bit integer bounds + // + pHandle->errorCode = SBG_INVALID_PARAMETER; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an int32_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt32BE(SbgStreamBuffer *pHandle, int32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((int32_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int32_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint32_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32BE(SbgStreamBuffer *pHandle, uint32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((uint32_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint32_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint48 into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint48BE(SbgStreamBuffer *pHandle, uint64_t value) +{ + assert(pHandle); + assert(value < ((uint64_t)1 << 48)); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6 * sizeof(uint8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an int64_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt64BE(SbgStreamBuffer *pHandle, int64_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((int64_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int64_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint64_t into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint64BE(SbgStreamBuffer *pHandle, uint64_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Write the value + // + *((uint64_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint64_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an size_t into a stream buffer as a uint32_t (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT32BE(SbgStreamBuffer *pHandle, size_t value) +{ + assert(pHandle); + + // + // Make sure the provided size_t value doesn't exceed a uint32_t storage + // + assert(value <= UINT32_MAX); + + // + // Call the write method to store a uint32_t + // + return sbgStreamBufferWriteUint32BE(pHandle, (uint32_t)value); +} + +/*! + * Write an size_t into a stream buffer as a uint64_t (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64BE(SbgStreamBuffer *pHandle, size_t value) +{ + // + // Check input parameters + // + assert(pHandle); + + // + // Call the write method to store a uint64_t + // + return sbgStreamBufferWriteUint64BE(pHandle, (uint64_t)value); +} + +/*! + * Write an float into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatBE(SbgStreamBuffer *pHandle, float value) +{ + FloatNint floatInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We use an union to avoid compiler cast + // + floatInt.valF = value; + + // + // Write this float as an uint32_t + // + return sbgStreamBufferWriteUint32BE(pHandle, floatInt.valU); + } + + return pHandle->errorCode; +} + +/*! + * Write an double into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleBE(SbgStreamBuffer *pHandle, double value) +{ + DoubleNint doubleInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We use an union to avoid compiler cast + // + doubleInt.valF = value; + + // + // Write this float as an uint64_t + // + return sbgStreamBufferWriteUint64BE(pHandle, doubleInt.valU); + } + + return pHandle->errorCode; +} + +/*! + * Read a C String from a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \param[out] pString Buffer that can hold the read NULL terminated C string. + * \param[in] maxSize Maximum number of bytes that can be stored in pString (including the NULL char). + * \return SBG_NO_ERROR if the string has been read successfully from the stream buffer. + * SBG_BUFFER_OVERFLOW if the provided string isn't big enough to hold the read string + */ +SBG_INLINE SbgErrorCode sbgStreamBufferReadStringBE(SbgStreamBuffer *pHandle, char *pString, size_t maxSize) +{ + size_t stringLength; + + assert(pHandle); + assert(pString); + assert(maxSize > 0); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // The C string are stored in a stream buffer with a 32 bit size length and then the buffer itself + // + stringLength = sbgStreamBufferReadSizeT32BE(pHandle); + + if (stringLength <= maxSize) + { + // + // Read the string buffer itself + // + sbgStreamBufferReadBuffer(pHandle, pString, stringLength); + } + else + { + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + SBG_LOG_ERROR(pHandle->errorCode, "Trying to store a string of %zu bytes into a buffer of %zu bytes.", stringLength, maxSize); + } + } + + return pHandle->errorCode; +} + +/*! + * Write a NULL terminated C String into a stream buffer (Big Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] pString NULL terminated C String to write to the stream buffer. + * \return SBG_NO_ERROR if the string has been written successfully to the stream buffer. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteStringBE(SbgStreamBuffer *pHandle, const char *pString) +{ + size_t stringLength; + + assert(pHandle); + assert(pString); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We write C string using a 32 bit size_t as the string length including the NULL char + // We should thus make sure the provided string isn't too big to fit in a 32 bits size_t + // + stringLength = strlen(pString)+1; + + if (stringLength <= UINT32_MAX) + { + // + // Write the string length + // + if (sbgStreamBufferWriteSizeT32BE(pHandle, stringLength) == SBG_NO_ERROR) + { + // + // Write the string buffer itself + // + sbgStreamBufferWriteBuffer(pHandle, pString, stringLength); + } + } + else + { + pHandle->errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(pHandle->errorCode, "The provided string is too big to fit in a 32 bit size_t"); + } + } + + return pHandle->errorCode; +} + +#endif /* SBG_STREAM_BUFFER_BE_H */ diff --git a/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h new file mode 100644 index 0000000..e6c8af4 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferCommon.h @@ -0,0 +1,831 @@ +/*! + * \file sbgStreamBufferCommon.h + * \ingroup common + * \author SBG Systems + * \date 02 January 2013 + * + * \brief Used to read/write data from/to a memory buffer stream. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_STREAM_BUFFER_COMMON_H +#define SBG_STREAM_BUFFER_COMMON_H + +#include + +//----------------------------------------------------------------------// +//- General definitions -// +//----------------------------------------------------------------------// + +/*! + * The default method should read and write using the platform endianness + */ +#if SBG_CONFIG_BIG_ENDIAN == 1 + /*! + * The platform is a big endian one so default methods should use big endian byte order. + */ + #define sbgStreamBufferReadUint16 sbgStreamBufferReadUint16BE + #define sbgStreamBufferReadInt16 sbgStreamBufferReadInt16BE + + #define sbgStreamBufferReadUint24 sbgStreamBufferReadUint24BE + #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24BE + + #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32BE + #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32BE + + #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40BE + #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40BE + + #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48BE + #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48BE + + #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56BE + #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56BE + + #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64BE + #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64BE + + #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32BE + #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64BE + + #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatBE + #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleBE + + #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16BE + #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16BE + + #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24BE + #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24BE + + #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32BE + #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32BE + + #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64BE + #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64BE + + #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32BE + #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64BE + + #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatBE + #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleBE + + #define sbgStreamBufferReadString sbgStreamBufferReadStringBE + #define sbgStreamBufferWriteString sbgStreamBufferWriteStringBE +#else + /*! + * The platform is a little endian one so default methods should use little endian byte order. + */ + #define sbgStreamBufferReadUint16 sbgStreamBufferReadUint16LE + #define sbgStreamBufferReadInt16 sbgStreamBufferReadInt16LE + + #define sbgStreamBufferReadUint24 sbgStreamBufferReadUint24LE + #define sbgStreamBufferReadInt24 sbgStreamBufferReadInt24LE + + #define sbgStreamBufferReadUint32 sbgStreamBufferReadUint32LE + #define sbgStreamBufferReadInt32 sbgStreamBufferReadInt32LE + + #define sbgStreamBufferReadUint40 sbgStreamBufferReadUint40LE + #define sbgStreamBufferReadInt40 sbgStreamBufferReadInt40LE + + #define sbgStreamBufferReadUint48 sbgStreamBufferReadUint48LE + #define sbgStreamBufferReadInt48 sbgStreamBufferReadInt48LE + + #define sbgStreamBufferReadUint56 sbgStreamBufferReadUint56LE + #define sbgStreamBufferReadInt56 sbgStreamBufferReadInt56LE + + #define sbgStreamBufferReadUint64 sbgStreamBufferReadUint64LE + #define sbgStreamBufferReadInt64 sbgStreamBufferReadInt64LE + + #define sbgStreamBufferReadSizeT32 sbgStreamBufferReadSizeT32LE + #define sbgStreamBufferReadSizeT64 sbgStreamBufferReadSizeT64LE + + #define sbgStreamBufferReadFloat sbgStreamBufferReadFloatLE + #define sbgStreamBufferReadDouble sbgStreamBufferReadDoubleLE + + #define sbgStreamBufferWriteUint16 sbgStreamBufferWriteUint16LE + #define sbgStreamBufferWriteInt16 sbgStreamBufferWriteInt16LE + + #define sbgStreamBufferWriteUint24 sbgStreamBufferWriteUint24LE + #define sbgStreamBufferWriteInt24 sbgStreamBufferWriteInt24LE + + #define sbgStreamBufferWriteUint32 sbgStreamBufferWriteUint32LE + #define sbgStreamBufferWriteInt32 sbgStreamBufferWriteInt32LE + + #define sbgStreamBufferWriteUint64 sbgStreamBufferWriteUint64LE + #define sbgStreamBufferWriteInt64 sbgStreamBufferWriteInt64LE + + #define sbgStreamBufferWriteSizeT32 sbgStreamBufferWriteSizeT32LE + #define sbgStreamBufferWriteSizeT64 sbgStreamBufferWriteSizeT64LE + + #define sbgStreamBufferWriteFloat sbgStreamBufferWriteFloatLE + #define sbgStreamBufferWriteDouble sbgStreamBufferWriteDoubleLE + + #define sbgStreamBufferReadString sbgStreamBufferReadStringLE + #define sbgStreamBufferWriteString sbgStreamBufferWriteStringLE +#endif + +/*! + * Some methods are common between big and little endian. + * This definitions just unify the API. + */ +#define sbgStreamBufferReadUint8LE sbgStreamBufferReadUint8 +#define sbgStreamBufferReadInt8LE sbgStreamBufferReadInt8 +#define sbgStreamBufferReadBooleanLE sbgStreamBufferReadBoolean +#define sbgStreamBufferReadBufferLE sbgStreamBufferReadBuffer + +#define sbgStreamBufferWriteUint8LE sbgStreamBufferWriteUint8 +#define sbgStreamBufferWriteInt8LE sbgStreamBufferWriteInt8 +#define sbgStreamBufferWriteBooleanLE sbgStreamBufferWriteBoolean +#define sbgStreamBufferWriteBufferLE sbgStreamBufferWriteBuffer + +#define sbgStreamBufferReadUint8BE sbgStreamBufferReadUint8 +#define sbgStreamBufferReadInt8BE sbgStreamBufferReadInt8 +#define sbgStreamBufferReadBooleanBE sbgStreamBufferReadBoolean +#define sbgStreamBufferReadBufferBE sbgStreamBufferReadBuffer + +#define sbgStreamBufferWriteUint8BE sbgStreamBufferWriteUint8 +#define sbgStreamBufferWriteInt8BE sbgStreamBufferWriteInt8 +#define sbgStreamBufferWriteBooleanBE sbgStreamBufferWriteBoolean +#define sbgStreamBufferWriteBufferBE sbgStreamBufferWriteBuffer + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Stream buffer modes. + */ +typedef enum _SbgSBMode +{ + SB_MODE_READ, /*!< This stream buffer can perform read operations. */ + SB_MODE_WRITE /*!< This stream buffer can perform write operations. */ +} SbgSBMode; + +/*! + * Enum used to define all seek modes + */ +typedef enum _SbgSBSeekOrigin +{ + SB_SEEK_SET, /*!< The offset is referenced to the begining of the stream. */ + SB_SEEK_CUR_INC, /*!< The offset is referenced to the current cursor position and increment the current cursor. */ + SB_SEEK_CUR_DEC, /*!< The offset is referenced to the current cursor position and decrement the current cursor. */ + SB_SEEK_END /*!< The offset is referenced to the end of the stream. */ +} SbgSBSeekOrigin; + +/*! + * Defines a stream buffer. + */ +typedef struct _SbgStreamBuffer +{ + SbgSBMode modes; /*!< Defines the stream buffer modes (read/write). */ + size_t bufferSize; /*!< Size in bytes of the linked buffer. */ + uint8_t *pBufferPtr; /*!< Pointer to the buffer linked with this stream. */ + uint8_t *pCurrentPtr; /*!< Current pointer within the buffer. */ + SbgErrorCode errorCode; /*!< Current error code on stream buffer. */ +} SbgStreamBuffer; + +//----------------------------------------------------------------------// +//- Common operations methods -// +//----------------------------------------------------------------------// + +/*! + * Initialize a stream buffer for both read and write operations and link it to a buffer. + * + * \param[in] pHandle Handle on an allocated stream buffer. + * \param[in] pLinkedBuffer Pointer on an allocated buffer to link with this stream. + * \param[in] bufferSize Size in bytes of the linked buffer. + * \return SBG_NO_ERROR if the stream buffer has been initialized successfully. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferInitForWrite(SbgStreamBuffer *pHandle, void *pLinkedBuffer, size_t bufferSize) +{ + assert(pHandle); + assert(pLinkedBuffer); + + // + // Initialize stream parameters + // + pHandle->modes = SB_MODE_WRITE; + pHandle->bufferSize = bufferSize; + pHandle->errorCode = SBG_NO_ERROR; + + // + // Initialize the buffer + // + pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer; + pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer; + + // + // For now, we don't handle any error, maybe we could add checks in debug mode only + // + return SBG_NO_ERROR; +} + +/*! + * Initialize a stream buffer for both read and write operations and link it to a buffer. + * + * \param[in] pHandle Handle on an allocated stream buffer. + * \param[in] pLinkedBuffer Pointer on an allocated buffer to link with this stream. + * \param[in] bufferSize Size in bytes of the linked buffer. + * \return SBG_NO_ERROR if the stream buffer has been initialized successfully. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize) +{ + assert(pHandle); + assert(pLinkedBuffer); + + // + // Initialize stream parameters + // + pHandle->modes = SB_MODE_READ; + pHandle->bufferSize = bufferSize; + pHandle->errorCode = SBG_NO_ERROR; + + // + // Initialize the buffer + // + pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer; + pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer; + + // + // For now, we don't handle any error, maybe we could add checks in debug mode only + // + return SBG_NO_ERROR; +} + +/*! + * Return the error code that has occurred on the last stream buffer operation. + * + * \param[in] pHandle Pointer to a valid Stream Buffer handle + * \return Last stream buffer error code + */ +SBG_INLINE SbgErrorCode sbgStreamBufferGetLastError(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Return error code + // + return pHandle->errorCode; +} + +/*! + * Clear the last error code that has occurred on the last stream buffer operation. + * + * \param[in] pHandle Pointer to a valid Stream Buffer handle + */ +SBG_INLINE void sbgStreamBufferClearLastError(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Return error code + // + pHandle->errorCode = SBG_NO_ERROR; +} + +/*! + * Returns the size in bytes of this stream. + * + * The size is the linked buffer total size in bytes. + * For example, for a SbgStreamBuffer linked with a buffer of 256 bytes, + * this method will always returns 256 even if no data has been written or read. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return The allocated size of the linked buffer in bytes. + */ +SBG_INLINE size_t sbgStreamBufferGetSize(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Return the linked buffer size + // + return pHandle->bufferSize; +} + +/*! + * Returns the length in bytes of this stream. + * + * The length is computed using the current cursor position. + * If no data has been read or written, this method will return 0. + * If 4 uint32_t has been written, it should return 16. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return The current cursor position in bytes. + */ +SBG_INLINE size_t sbgStreamBufferGetLength(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Return the number of bytes between the begin of the stream and the current pointer + // + return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr); +} + +/*! + * Returns the available space in this stream. + * + * The available space is just the delta between the linked buffer size + * and the current buffer length (cursor position). + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return The space available in this stream buffer in bytes. + */ +SBG_INLINE size_t sbgStreamBufferGetSpace(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Return the space left in bytes + // + return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle); +} + +/*! + * Move the current cursor position. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \param[in] offset Offset in bytes to apply (only positive). + * \param[in] origin Origin reference point to apply the offset from. + * \return SBG_NO_ERROR if the stream current cursor position has been moved. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // According to the origin reference point + // + switch (origin) + { + case SB_SEEK_SET: + pHandle->pCurrentPtr = pHandle->pBufferPtr + offset; + break; + case SB_SEEK_CUR_INC: + pHandle->pCurrentPtr += offset; + break; + case SB_SEEK_CUR_DEC: + pHandle->pCurrentPtr -= offset; + break; + case SB_SEEK_END: + pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset); + break; + default: + pHandle->errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter"); + } + + // + // Make sure that no error has occurred + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if the current ptr is still within the buffer bounds + // + if (pHandle->pCurrentPtr < pHandle->pBufferPtr) + { + // + // We are before the buffer so clamp to the begining of the buffer and raise an error + // + pHandle->pCurrentPtr = pHandle->pBufferPtr; + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + + // + // Stream buffer underflow + // + SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer"); + } + else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize) + { + // + // We are after the buffer so clamp to the end of the buffer and raise an error + // + pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize; + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + + // + // Stream buffer overflow + // + SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer"); + } + } + } + + return pHandle->errorCode; +} + +/*! + * Returns the current offset in bytes from the beginning of the stream. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return Current offset in bytes from the beginning. + */ +SBG_INLINE size_t sbgStreamBufferTell(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr; +} + +/*! + * Returns a pointer on the internal buffer. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return Pointer on the begining of the internal buffer. + */ +SBG_INLINE void *sbgStreamBufferGetLinkedBuffer(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + return pHandle->pBufferPtr; +} + +/*! + * Returns a pointer on the internal buffer at the current cursor. + * + * \param[in] pHandle Valid handle on a stream buffer. + * \return Pointer on the current cursor of the internal buffer. + */ +SBG_INLINE void *sbgStreamBufferGetCursor(const SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + return pHandle->pCurrentPtr; +} + +//----------------------------------------------------------------------// +//- Read operations methods -// +//----------------------------------------------------------------------// + +/*! + * Read an int8_t from a stream buffer. + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int8_t sbgStreamBufferReadInt8(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t)) + { + // + // Read the byte + // + return *((int8_t*)(pHandle->pCurrentPtr++)); + } + else + { + // + // We have a buffer overflow + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint8_t from a stream buffer. + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint8_t sbgStreamBufferReadUint8(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) + { + // + // Read the byte + // + return *((uint8_t*)(pHandle->pCurrentPtr++)); + } + else + { + // + // We have a buffer overflow + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read a boolean from a stream buffer. + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or false if we have an error. + */ +SBG_INLINE bool sbgStreamBufferReadBoolean(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) + { + // + // Read the byte and check if the value is different than zero or not + // + if (*((uint8_t*)(pHandle->pCurrentPtr++))) + { + return true; + } + else + { + return false; + } + } + else + { + // + // We have a buffer overflow + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return false + // + return false; +} + +/*! + * Read a buffer from a stream buffer. + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \param[out] pBuffer Allocated buffer used to hold read data. + * \param[in] numBytesToRead Number of bytes to read from the stream buffer and to store in pBuffer. + * \return SBG_NO_ERROR if the data has been read. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferReadBuffer(SbgStreamBuffer *pHandle, void *pBuffer, size_t numBytesToRead) +{ + assert(pHandle); + assert((pBuffer) || (numBytesToRead == 0)); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if enough bytes in stream + // + if (sbgStreamBufferGetSpace(pHandle) >= numBytesToRead) + { + // + // Copy from the stream buffer to the output buffer + // + memcpy(pBuffer, pHandle->pCurrentPtr, numBytesToRead); + + // + // Update the current pointer + // + pHandle->pCurrentPtr += numBytesToRead; + } + else + { + // + // Not enough data in stream + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +//----------------------------------------------------------------------// +//- Write operations methods -// +//----------------------------------------------------------------------// + +/*! + * Write an int8_t into a stream buffer + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt8(SbgStreamBuffer *pHandle, int8_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t)) + { + // + // Write each byte + // + *(pHandle->pCurrentPtr++) = (int8_t)(value); + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint8_t into a stream buffer + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint8(SbgStreamBuffer *pHandle, uint8_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) + { + // + // Write each byte + // + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write a boolean into a stream buffer + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteBoolean(SbgStreamBuffer *pHandle, bool value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t)) + { + // + // Write the boolean as an uint8_t value (1 byte) + // + if (value) + { + *(pHandle->pCurrentPtr++) = 1; + } + else + { + *(pHandle->pCurrentPtr++) = 0; + } + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write a buffer to a stream buffer. + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[out] pBuffer Buffer to write into the stream buffer. + * \param[in] numBytesToWrite Number of bytes to write to the stream buffer. + * \return SBG_NO_ERROR if the data has been written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteBuffer(SbgStreamBuffer *pHandle, const void *pBuffer, size_t numBytesToWrite) +{ + assert(pHandle); + assert((pBuffer) || (numBytesToWrite == 0)); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= numBytesToWrite) + { + // + // Copy from the stream buffer to the output buffer + // + memcpy(pHandle->pCurrentPtr, pBuffer, numBytesToWrite); + + // + // Update the current pointer + // + pHandle->pCurrentPtr += numBytesToWrite; + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +#endif // SBG_STREAM_BUFFER_COMMON_H diff --git a/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferLE.h b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferLE.h new file mode 100644 index 0000000..e3dc97e --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/streamBuffer/sbgStreamBufferLE.h @@ -0,0 +1,1922 @@ +/*! + * \file sbgStreamBufferLE.h + * \ingroup common + * \author SBG Systems + * \date 17 February 2015 + * + * \brief Specific method of stream buffer for little endian readings/writings. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_STREAM_BUFFER_LE_H +#define SBG_STREAM_BUFFER_LE_H + +#include "sbgStreamBufferCommon.h" + +//----------------------------------------------------------------------// +//- Read operations methods -// +//----------------------------------------------------------------------// + +/*! + * Read an int16_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int16_t sbgStreamBufferReadInt16LE(SbgStreamBuffer *pHandle) +{ + int16_t bytesValues[2]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + bytesValues[0] = *((int16_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int16_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[0] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[1] | (bytesValues[0] << 8); + #else + return bytesValues[0] | (bytesValues[1] << 8); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint16_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint16_t sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle) +{ + uint16_t bytesValues[2]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint16_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[0] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[1] | (bytesValues[0] << 8); + #else + return bytesValues[0] | (bytesValues[1] << 8); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int24 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int32_t sbgStreamBufferReadInt24LE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt32 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 24 bits + // + return value.value >> (32-24); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint24 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint32_t sbgStreamBufferReadUint24LE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint32 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 24 bits + // + return value.value >> (32-24); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int32_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int32_t sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle) +{ + int32_t bytesValues[4]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int32_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[0] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[2] = *(pHandle->pCurrentPtr++); + bytesValues[3] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); + #else + return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint32_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint32_t sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle) +{ + uint32_t bytesValues[4]; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint32_t); + + return bytesValues[0]; + #else + // + // Read the each bytes + // + bytesValues[0] = *(pHandle->pCurrentPtr++); + bytesValues[1] = *(pHandle->pCurrentPtr++); + bytesValues[2] = *(pHandle->pCurrentPtr++); + bytesValues[3] = *(pHandle->pCurrentPtr++); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24); + #else + return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int40 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt40LE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 40 bits + // + return value.value >> (64-40); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint40 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadUint40LE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 5*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[4] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[3] = *(pHandle->pCurrentPtr++); // MSB + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 40 bits + // + return value.value >> (64-40); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int48 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt48LE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 48 bits + // + return value.value >> (64-48); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint48 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint48LE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[5] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[2] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 48 bits + // + return value.value >> (64-48); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int56 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt56LE(SbgStreamBuffer *pHandle) +{ + Uint8ToInt64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 56 bits + // + return value.value >> (64-56); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an uint56 from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint56LE(SbgStreamBuffer *pHandle) +{ + Uint8ToUint64 value; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 7*sizeof(uint8_t)) + { + // + // Make sure the value is zero init + // + value.value = 0; + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + // + // Read the each bytes + // + value.buffer[6] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[1] = *(pHandle->pCurrentPtr++); + value.buffer[0] = *(pHandle->pCurrentPtr++); // MSB + #else + // + // Read the each bytes + // + value.buffer[1] = *(pHandle->pCurrentPtr++); // LSB + value.buffer[2] = *(pHandle->pCurrentPtr++); + value.buffer[3] = *(pHandle->pCurrentPtr++); + value.buffer[4] = *(pHandle->pCurrentPtr++); + value.buffer[5] = *(pHandle->pCurrentPtr++); + value.buffer[6] = *(pHandle->pCurrentPtr++); + value.buffer[7] = *(pHandle->pCurrentPtr++); // MSB + #endif + + // + // Shift the value to handle the sign correctly for a 56 bits + // + return value.value >> (64-56); + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0; +} + +/*! + * Read an int64_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE int64_t sbgStreamBufferReadInt64LE(SbgStreamBuffer *pHandle) +{ + int64_t lowPart; + int64_t highPart; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + lowPart = *((int64_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int64_t); + + return lowPart; + #else + // + // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations + // + lowPart = sbgStreamBufferReadUint32LE(pHandle); + highPart = sbgStreamBufferReadUint32LE(pHandle); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return (lowPart << 32) | highPart; + #else + return lowPart | (highPart << 32); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0ll; +} + +/*! + * Read an uint64_t from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE uint64_t sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle) +{ + uint64_t lowPart; + uint64_t highPart; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Read the current value + // + lowPart = *((uint64_t*)pHandle->pCurrentPtr); + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint64_t); + + return lowPart; + #else + // + // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations + // + lowPart = sbgStreamBufferReadUint32LE(pHandle); + highPart = sbgStreamBufferReadUint32LE(pHandle); + + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + return (lowPart << 32) | highPart; + #else + return lowPart | (highPart << 32); + #endif + #endif + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0ull; +} + +/*! + * Read a size_t from a stream buffer that has been stored in a uint32_t (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE size_t sbgStreamBufferReadSizeT32LE(SbgStreamBuffer *pHandle) +{ + assert(pHandle); + + // + // Just call the read method for uint32_t + // We assume that a size_t is at least 32 bits on all platforms + // + return (size_t)sbgStreamBufferReadUint32LE(pHandle); +} + +/*! + * Read a size_t from a stream buffer that has been stored in a uint64_t (Little endian version). + + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE size_t sbgStreamBufferReadSizeT64LE(SbgStreamBuffer *pHandle) +{ + uint64_t size; + + assert(pHandle); + + // + // Just call the read method for uint64_t + // + size = sbgStreamBufferReadUint64LE(pHandle); + + // + // Make sure the read size can fit in the size_t in size_t is 32 bits + // + assert((sizeof(size_t) == 8) || ((sizeof(size_t) == 4) && (size <= UINT32_MAX))); + + // + // Return the read value + // + return (size_t)size; +} + +/*! + * Read an float from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE float sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle) +{ + FloatNint floatInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float)) + { + // + // Read the float as an uint32_t + // + floatInt.valU = sbgStreamBufferReadUint32LE(pHandle); + + // + // Return the float using an union to avoid compiler cast + // + return floatInt.valF; + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0.0f; +} + +/*! + * Read an double from a stream buffer (Little endian version). + * + * \param[in] pHandle Valid stream buffer handle that supports read operations. + * \return The read value or 0 if we have an error. + */ +SBG_INLINE double sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle) +{ + DoubleNint doubleInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double)) + { + // + // Read the float as an uint64_t + // + doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle); + + // + // Return the double using an union to avoid compiler cast + // + return doubleInt.valF; + } + else + { + // + // We have a buffer overflow so return 0 + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // If we are here, it means we have an error so return 0 + // + return 0.0; +} + +//----------------------------------------------------------------------// +//- Write operations methods -// +//----------------------------------------------------------------------// + +/*! + * Write an int16_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt16LE(SbgStreamBuffer *pHandle, int16_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((int16_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int16_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint16_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint16LE(SbgStreamBuffer *pHandle, uint16_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((uint16_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint16_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + + +/*! + * Write an int24 into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt24LE(SbgStreamBuffer *pHandle, int32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Make sure that the value is within 24 bit bonds + // + if ( (value >= SBG_MIN_INT_24) && (value <= SBG_MAX_INT_24) ) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(int8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + else + { + // + // The input value is not within a 24 bit integer bounds + // + pHandle->errorCode = SBG_INVALID_PARAMETER; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint24 into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint24LE(SbgStreamBuffer *pHandle, uint32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Make sure that the value is within 24 bit bonds + // + if (value <= SBG_MAX_UINT_24) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 3*sizeof(uint8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + else + { + // + // The input value is not within a 24 bit integer bounds + // + pHandle->errorCode = SBG_INVALID_PARAMETER; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an int32_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt32LE(SbgStreamBuffer *pHandle, int32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((int32_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int32_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint32_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint32LE(SbgStreamBuffer *pHandle, uint32_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((uint32_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint32_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint48 into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint48LE(SbgStreamBuffer *pHandle, uint64_t value) +{ + assert(pHandle); + assert(value < ((uint64_t)1 << 48)); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= 6 * sizeof(uint8_t)) + { + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an int64_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteInt64LE(SbgStreamBuffer *pHandle, int64_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((int64_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(int64_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an uint64_t into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteUint64LE(SbgStreamBuffer *pHandle, uint64_t value) +{ + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // Test if we can access this item + // + if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t)) + { + // + // Test if the platform supports un-aligned access and if the endianness is the same + // + #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0) + // + // Write the value + // + *((uint64_t*)(pHandle->pCurrentPtr)) = value; + + // + // Increment the current pointer + // + pHandle->pCurrentPtr += sizeof(uint64_t); + #else + // + // Store data according to platform endianness + // + #if (SBG_CONFIG_BIG_ENDIAN == 1) + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + #else + *(pHandle->pCurrentPtr++) = (uint8_t)(value); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 8); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 16); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 24); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 32); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 40); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 48); + *(pHandle->pCurrentPtr++) = (uint8_t)(value >> 56); + #endif + #endif + } + else + { + // + // We are accessing a data that is outside the stream buffer + // + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + } + } + + return pHandle->errorCode; +} + +/*! + * Write an size_t into a stream buffer as a uint32_t (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT32LE(SbgStreamBuffer *pHandle, size_t value) +{ + assert(pHandle); + + // + // Make sure the provided size_t value doesn't exceed a uint32_t storage + // + assert(value <= UINT32_MAX); + + // + // Call the write method to store a uint32_t + // + return sbgStreamBufferWriteUint32LE(pHandle, (uint32_t)value); +} + +/*! + * Write an size_t into a stream buffer as a uint64_t (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteSizeT64LE(SbgStreamBuffer *pHandle, size_t value) +{ + assert(pHandle); + + // + // Call the write method to store a uint64_t + // + return sbgStreamBufferWriteUint64LE(pHandle, (uint64_t)value); +} + +/*! + * Write an float into a stream buffer (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteFloatLE(SbgStreamBuffer *pHandle, float value) +{ + FloatNint floatInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We use an union to avoid compiler cast + // + floatInt.valF = value; + + // + // Write this float as an uint32_t + // + return sbgStreamBufferWriteUint32LE(pHandle, floatInt.valU); + } + + return pHandle->errorCode; +} + +/*! + * Write an double into a stream buffer. (Little Endian Version). + * + * \param[in] pHandle Valid stream buffer handle that supports write operations. + * \param[in] value The value to write. + * \return SBG_NO_ERROR if the value has been successfully written. + */ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteDoubleLE(SbgStreamBuffer *pHandle, double value) +{ + DoubleNint doubleInt; + + assert(pHandle); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We use an union to avoid compiler cast + // + doubleInt.valF = value; + + // + // Write this float as an uint64_t + // + return sbgStreamBufferWriteUint64LE(pHandle, doubleInt.valU); + } + + return pHandle->errorCode; +} + +/*! +* Read a C String from a stream buffer (Little Endian Version). +* +* \param[in] pHandle Valid stream buffer handle that supports read operations. +* \param[out] pString Buffer that can hold the read NULL terminated C string. +* \param[in] maxSize Maximum number of bytes that can be stored in pString (including the NULL char). +* \return SBG_NO_ERROR if the string has been read successfully from the stream buffer. +* SBG_BUFFER_OVERFLOW if the provided string isn't big enough to hold the read string +*/ +SBG_INLINE SbgErrorCode sbgStreamBufferReadStringLE(SbgStreamBuffer *pHandle, char *pString, size_t maxSize) +{ + size_t stringLength; + + assert(pHandle); + assert(pString); + assert(maxSize > 0); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // The C string are stored in a stream buffer with a 32 bit size length and then the buffer itself + // + stringLength = sbgStreamBufferReadSizeT32LE(pHandle); + + if (stringLength <= maxSize) + { + // + // Read the string buffer itself + // + sbgStreamBufferReadBuffer(pHandle, pString, stringLength); + } + else + { + pHandle->errorCode = SBG_BUFFER_OVERFLOW; + SBG_LOG_ERROR(pHandle->errorCode, "Trying to store a string of %zu bytes into a buffer of %zu bytes.", stringLength, maxSize); + } + } + + return pHandle->errorCode; +} + +/*! +* Write a NULL terminated C String into a stream buffer (Little Endian Version). +* +* \param[in] pHandle Valid stream buffer handle that supports write operations. +* \param[in] pString NULL terminated C String to write to the stream buffer. +* \return SBG_NO_ERROR if the string has been written successfully to the stream buffer. +*/ +SBG_INLINE SbgErrorCode sbgStreamBufferWriteStringLE(SbgStreamBuffer *pHandle, const char *pString) +{ + size_t stringLength; + + assert(pHandle); + assert(pString); + + // + // Test if we haven't already an error + // + if (pHandle->errorCode == SBG_NO_ERROR) + { + // + // We write C string using a 32 bit size_t as the string length including the NULL char + // We should thus make sure the provided string isn't too big to fit in a 32 bits size_t + // + stringLength = strlen(pString) + 1; + + if (stringLength <= UINT32_MAX) + { + // + // Write the string length + // + if (sbgStreamBufferWriteSizeT32LE(pHandle, stringLength) == SBG_NO_ERROR) + { + // + // Write the string buffer itself + // + sbgStreamBufferWriteBuffer(pHandle, pString, stringLength); + } + } + else + { + pHandle->errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(pHandle->errorCode, "The provided string is too big to fit in a 32 bit size_t"); + } + } + + return pHandle->errorCode; +} + +#endif /* SBG_STREAM_BUFFER_LE_H */ diff --git a/crates/sbg-rs/sbgECom/common/string/sbgString.c b/crates/sbg-rs/sbgECom/common/string/sbgString.c new file mode 100644 index 0000000..b7d36cd --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/string/sbgString.c @@ -0,0 +1,1545 @@ +// Standard headers +#include +#include + +// sbgCommonLib headers +#include + +// Local headers +#include "sbgString.h" + +//----------------------------------------------------------------------// +// Private functions // +//----------------------------------------------------------------------// + +/*! + * Count the number of leading zeros in an unsigned integer. + * + * \param[in] value Integer value, must not be zero. + * \return Number of leading zeros. + */ +static uint32_t sbgStringCountLeadingZeros(size_t value) +{ + uint32_t result; + + assert(value != 0); + +#if defined(__GNUC__) || defined(__clang__) + result = __builtin_clzl((unsigned long)value); +#else + size_t count; + size_t tmp; + + tmp = value; + count = 0; + + while (tmp != 0) + { + tmp >>= 1; + count++; + } + + result = (sizeof(size_t) * 8) - count; +#endif // defined(__GNUC__) || defined(__clang__) + + return result; +} + +/*! + * Compute the capacity required to store a string of the given size, in bytes. + * + * The size includes the terminating null character. + * + * The size must not be zero. + * + * \param[in] pString String. + * \param[in] size Size, in bytes. + * \param[out] pCapacity Capacity, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgStringComputeCapacity(const SbgString *pString, size_t size, size_t *pCapacity) +{ + SbgErrorCode errorCode; + + assert(pString); + + SBG_UNUSED_PARAMETER(pString); + + // + // The computed capacity is the power-of-two equal to or immediately greater than the size. + // In other words, it's the value with a single bit set where the index of that bit is one + // more than the MSB bit of the size, unless the size is already a power-of-two. + // + // If the size already requires all bits of the size_t type to be encoded, then there's no + // extra bit available to encode the capacity. + // + + if (size <= ((size_t)1 << ((sizeof(size_t) * 8) - 1))) + { + *pCapacity = (size_t)1 << ((sizeof(size_t) * 8) - sbgStringCountLeadingZeros(size)); + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to compute capacity"); + } + + return errorCode; +} + +/*! + * Resize the buffer of a string. + * + * This function only resizes the buffer, it doesn't guarantee the content is null-terminated. + * + * If an allocation error occurs, the string is left unchanged. + * + * If an allocation error occurs, and the new size is smaller than the current size, the + * operation is considered successful. + * + * \param[in] pString String. + * \param[in] size Required size, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgStringResizeBuffer(SbgString *pString, size_t size) +{ + SbgErrorCode errorCode; + + assert(pString); + assert(!pString->readOnly); + + if (pString->isStatic) + { + if (size <= pString->capacity) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + SBG_LOG_ERROR(errorCode, "insufficient static space for string: requested %zu, capacity %zu", size, pString->capacity); + } + } + else + { + size_t capacity; + + // + // XXX If a user performs frequent operations on a string that change its size + // below and above the size of the internal buffer, the performance impact of + // copying the content to and from the internal buffer would cause hysteresis. + // + // There is no special handling of this situation as it is assumed that the + // internal buffer is small enough that the cost of the copy operation is + // negligible. + // + + errorCode = sbgStringComputeCapacity(pString, size, &capacity); + + if (errorCode == SBG_NO_ERROR) + { + if (capacity != pString->capacity) + { + if (capacity <= sizeof(pString->internalBuffer)) + { + if (pString->pBuffer != pString->internalBuffer) + { + memcpy(pString->internalBuffer, pString->pBuffer, capacity); + free(pString->pBuffer); + + pString->pBuffer = pString->internalBuffer; + pString->capacity = sizeof(pString->internalBuffer); + } + } + else + { + if (pString->pBuffer == pString->internalBuffer) + { + char *pBuffer; + + pBuffer = malloc(capacity); + + if (pBuffer) + { + memcpy(pBuffer, pString->internalBuffer, pString->capacity); + pString->pBuffer = pBuffer; + pString->capacity = capacity; + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate buffer"); + } + } + else + { + char *pBuffer; + + pBuffer = realloc(pString->pBuffer, capacity); + + if (pBuffer) + { + pString->pBuffer = pBuffer; + pString->capacity = capacity; + } + else + { + if (capacity < pString->capacity) + { + errorCode = SBG_NO_ERROR; + SBG_LOG_WARNING(SBG_MALLOC_FAILED, "unable to reallocate buffer"); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to reallocate buffer"); + } + } + } + } + } + } + } + + return errorCode; +} + +/*! + * Get a pointer to a character in a string. + * + * \param[in] pString String. + * \param[in] index Index. + * \return Pointer to the character denoted by the index. + */ +static char * sbgStringGetCharPtr(const SbgString *pString, size_t index) +{ + assert(pString); + assert(index < pString->length); + + return &pString->pBuffer[index]; +} + +/*! + * Check if the start and end indexes of a substring are valid. + * + * \param[in] pString String. + * \param[in] startIndex Start index of the substring. + * \param[in] endIndex End index of the substring. + * \return True if these indexes are valid. + */ +static bool sbgStringIndexesAreValid(const SbgString *pString, size_t startIndex, size_t endIndex) +{ + assert(pString); + + return ((startIndex <= endIndex) && (endIndex <= (pString->length))); +} + +/*! + * Assign a buffer of characters to a string. + * + * \param[in] pString String. + * \param[in] pBuffer Buffer. + * \param[in] length Length of the buffer, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgStringAssignBuffer(SbgString *pString, const char *pBuffer, size_t length) +{ + assert(pString); + assert(pBuffer); + + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(pString->pBuffer, pBuffer, length) ; + pString->pBuffer[length] = '\0'; + pString->length = length; + } + } + + return pString->errorCode; +} + +/*! + * Remove all spacing characters at the beginning of a string. + * + * This function doesn't resize the string buffer. + * + * \param[in] pString String. + */ +static bool sbgStringTrimLeftCommon(SbgString *pString) +{ + size_t index; + bool trimmed; + + assert(pString); + assert(!pString->readOnly); + + index = pString->length; + + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; + + pCharPtr = sbgStringGetCharPtr(pString, i); + + if (!isspace((unsigned char)*pCharPtr)) + { + index = i; + break; + } + } + + if (index != 0) + { + size_t length; + + length = pString->length - index; + + memmove(pString->pBuffer, &pString->pBuffer[index], length + 1); + pString->length = length; + + trimmed = true; + } + else + { + trimmed = false; + } + + return trimmed; +} + +/*! + * Remove all spacing characters at the end of a string. + * + * This function doesn't resize the string buffer. + * + * \param[in] pString String. + */ +static bool sbgStringTrimRightCommon(SbgString *pString) +{ + size_t index; + bool trimmed; + + assert(pString); + assert(!pString->readOnly); + + index = pString->length; + + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; + + pCharPtr = sbgStringGetCharPtr(pString, pString->length - i - 1); + + if (!isspace((unsigned char)*pCharPtr)) + { + index = i; + break; + } + } + + if (index != 0) + { + size_t length; + + length = pString->length - index; + + pString->pBuffer[length] = '\0'; + pString->length = length; + + trimmed = true; + } + else + { + trimmed = false; + } + + return trimmed; +} + +/*! + * Compare a string to a buffer of characters, ignoring case differences. + * + * \param[in] pString String. + * \param[in] pBuffer Buffer. + * \param[in] length Length of the buffer, in bytes. + * \return The return value is 0 if the string value is equal to the buffer value, + * less than 0 if the string value is less than the buffer value, + * greater than 0 if the string value is greater than the buffer value. + */ +static int32_t sbgStringCompareIgnoreCaseCommon(const SbgString *pString, const char *pBuffer, size_t length) +{ + int32_t result; + size_t minLength; + + assert(pString); + assert(pBuffer); + + result = 0; + + if (length > pString->length) + { + minLength = pString->length; + } + else + { + minLength = length; + } + + for (size_t i = 0; i <= minLength; i++) + { + unsigned char c1; + unsigned char c2; + + c1 = (unsigned char)tolower((unsigned char)pString->pBuffer[i]); + c2 = (unsigned char)tolower((unsigned char)pBuffer[i]); + + if (c1 != c2) + { + result = c1 - c2; + break; + } + } + + return result; +} + +//----------------------------------------------------------------------// +// Public functions // +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API void sbgStringConstructEmpty(SbgString *pString) +{ + assert(pString); + + pString->internalBuffer[0] = '\0'; + + pString->pBuffer = pString->internalBuffer; + pString->capacity = sizeof(pString->internalBuffer); + pString->length = 0; + + pString->readOnly = false; + pString->isStatic = false; + + pString->errorCode = SBG_NO_ERROR; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstruct(SbgString *pString, const SbgString *pSourceString) +{ + sbgStringConstructEmpty(pString); + + if (pSourceString) + { + sbgStringAssign(pString, pSourceString); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructCString(SbgString *pString, const char *pCString) +{ + sbgStringConstructEmpty(pString); + return sbgStringAssignCString(pString, pCString); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructVF(SbgString *pString, const char *pFormat, va_list args) +{ + sbgStringConstructEmpty(pString); + return sbgStringAssignVF(pString, pFormat, args); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructF(SbgString *pString, const char *pFormat, ...) +{ + SbgErrorCode errorCode; + va_list args; + + sbgStringConstructEmpty(pString); + + va_start(args, pFormat); + errorCode = sbgStringAssignVF(pString, pFormat, args); + va_end(args); + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructStatic(SbgString *pString, const char *pCString, char *pBuffer, size_t size) +{ + assert(pString); + assert(pBuffer); + + pString->pBuffer = pBuffer; + pString->capacity = size; + + pString->readOnly = false; + pString->isStatic = true; + + pString->errorCode = SBG_NO_ERROR; + + if (pCString) + { + sbgStringAssignCString(pString, pCString); + } + else + { + pString->length = strlen(pBuffer); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API void sbgStringConstructReadOnly(SbgString *pString, const char *pCString) +{ + assert(pString); + assert(pCString); + + pString->pBuffer = (char *)pCString; + pString->length = strlen(pCString); + pString->capacity = pString->length + 1; + + pString->readOnly = true; + pString->isStatic = true; + + pString->errorCode = SBG_NO_ERROR; +} + +SBG_COMMON_LIB_API void sbgStringDestroy(SbgString *pString) +{ + assert(pString); + + if (!pString->isStatic) + { + if (pString->pBuffer != pString->internalBuffer) + { + free(pString->pBuffer); + } + } +} + +SBG_COMMON_LIB_API int32_t sbgStringCompare(const SbgString *pString, const SbgString *pOtherString) +{ + assert(pString); + assert(pOtherString); + + return (int32_t)strcmp(pString->pBuffer, pOtherString->pBuffer); +} + +SBG_COMMON_LIB_API int32_t sbgStringCompareCString(const SbgString *pString, const char *pCString) +{ + assert(pString); + assert(pCString); + + return (int32_t)strcmp(pString->pBuffer, pCString); +} + +SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCase(const SbgString *pString, const SbgString *pOtherString) +{ + assert(pOtherString); + + return sbgStringCompareIgnoreCaseCommon(pString, pOtherString->pBuffer, pOtherString->length); +} + +SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCaseCString(const SbgString *pString, const char *pCString) +{ + assert(pCString); + + return sbgStringCompareIgnoreCaseCommon(pString, pCString, strlen(pCString)); +} + +SBG_COMMON_LIB_API size_t sbgStringHash(const SbgString *pString) +{ + size_t hash; + + assert(pString); + + // + // The algorithm is a simple polynomial accumulation : + // s[0]*31^(n-1) + s[1]*31^(n-2) + ... + s[n-1]. + // + // See java.lang.String.hashCode(). + // + + hash = 0; + + for (size_t i = 0; i < pString->length; i++) + { + char *pCharPtr; + + pCharPtr = sbgStringGetCharPtr(pString, i); + + hash = ((hash << 5) - hash) + *pCharPtr; + } + + return hash; +} + +SBG_COMMON_LIB_API size_t sbgStringGetLength(const SbgString *pString) +{ + assert(pString); + + return pString->length; +} + +SBG_COMMON_LIB_API const char *sbgStringGetCString(const SbgString *pString) +{ + assert(pString); + + return pString->pBuffer; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringCharAt(const SbgString *pString, size_t index, char *pChar) +{ + SbgErrorCode errorCode; + + assert(pString); + assert(pChar); + + if (index < pString->length) + { + char *pCharPtr; + + pCharPtr = sbgStringGetCharPtr(pString, index); + *pChar = *pCharPtr; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API size_t sbgStringFindCString(const SbgString *pString, const char *pCString) +{ + char *pChar; + + assert(pString); + assert(pCString); + + pChar = strstr(pString->pBuffer, pCString); + + if (pChar) + { + return (size_t)(pChar - pString->pBuffer); + } + else + { + return SIZE_MAX; + } +} + +SBG_COMMON_LIB_API size_t sbgStringFind(const SbgString *pString, const SbgString *pOtherString) +{ + assert(pOtherString); + + return sbgStringFindCString(pString, pOtherString->pBuffer); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringSubstring(const SbgString *pString, size_t startIndex, size_t endIndex, SbgString *pSubstring) +{ + SbgErrorCode errorCode; + + if (sbgStringIndexesAreValid(pString, startIndex, endIndex)) + { + size_t length; + + length = endIndex - startIndex; + + errorCode = sbgStringAssignBuffer(pSubstring, &pString->pBuffer[startIndex], length); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API bool sbgStringStartsWith(const SbgString *pString, const char *pCString) +{ + size_t cStringLength; + bool match; + + assert(pString); + assert(pCString); + + cStringLength = strlen(pCString); + + if (cStringLength <= pString->length) + { + int result; + + result = memcmp(pString->pBuffer, pCString, cStringLength); + + if (result == 0) + { + match = true; + } + else + { + match = false; + } + } + else + { + match = false; + } + + return match; +} + +SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char *pCString) +{ + size_t cStringLength; + bool match; + + assert(pString); + assert(pCString); + + cStringLength = strlen(pCString); + + if (cStringLength <= pString->length) + { + size_t endIndex; + int result; + + endIndex = pString->length - cStringLength; + + result = memcmp(&pString->pBuffer[endIndex], pCString, cStringLength); + + if (result == 0) + { + match = true; + } + else + { + match = false; + } + } + else + { + match = false; + } + + return match; +} + +//----------------------------------------------------------------------// +//- Modification methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgStringGetLastError(const SbgString *pString) +{ + assert(pString); + + return pString->errorCode; +} + +SBG_COMMON_LIB_API void sbgStringClearLastError(SbgString *pString) +{ + assert(pString); + + pString->errorCode = SBG_NO_ERROR; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringSetCharAt(SbgString *pString, size_t index, char c) +{ + SbgErrorCode errorCode; + + assert(pString); + + if ((index < pString->length) && (c != '\0')) + { + char *pCharPtr; + + pCharPtr = sbgStringGetCharPtr(pString, index); + *pCharPtr = c; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppend(SbgString *pString, const SbgString *pAppendString) +{ + assert(pString); + assert(pAppendString); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + + length = pString->length + pAppendString->length; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(&pString->pBuffer[pString->length], pAppendString->pBuffer, pAppendString->length + 1); + pString->length = length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendCString(SbgString *pString, const char *pCString) +{ + assert(pString); + assert(pCString); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t cStringLength; + size_t length; + + cStringLength = strlen(pCString); + length = pString->length + cStringLength; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + memcpy(&pString->pBuffer[pString->length], pCString, cStringLength + 1); + pString->length = length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendVF(SbgString *pString, const char *pFormat, va_list args) +{ + assert(pString); + assert(!pString->readOnly); + assert(pFormat); + + if (pString->errorCode == SBG_NO_ERROR) + { + va_list argsCopy; + int result; + size_t remainingSize; + size_t newLength; + + remainingSize = pString->capacity - pString->length; + + va_copy(argsCopy, args); + result = vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, argsCopy); + assert(result >= 0); + va_end(argsCopy); + + newLength = pString->length + (size_t)result; + + if ((size_t)result < remainingSize) + { + pString->length = newLength; + } + else + { + pString->errorCode = sbgStringResizeBuffer(pString, newLength + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + remainingSize = pString->capacity - pString->length; + vsnprintf(&pString->pBuffer[pString->length], remainingSize, pFormat, args); + pString->length = newLength; + } + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendF(SbgString *pString, const char *pFormat, ...) +{ + assert(pString); + assert(pFormat); + + if (pString->errorCode == SBG_NO_ERROR) + { + va_list args; + + va_start(args, pFormat); + sbgStringAppendVF(pString, pFormat, args); + va_end(args); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssign(SbgString *pString, const SbgString *pAssignString) +{ + assert(pAssignString); + + return sbgStringAssignBuffer(pString, pAssignString->pBuffer, pAssignString->length); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignCString(SbgString *pString, const char *pCString) +{ + assert(pCString); + + return sbgStringAssignBuffer(pString, pCString, strlen(pCString)); +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignVF(SbgString *pString, const char *pFormat, va_list args) +{ + assert(pString); + assert(pFormat); + + if (pString->errorCode == SBG_NO_ERROR) + { + va_list argsCopy; + int result; + + va_copy(argsCopy, args); + result = vsnprintf(pString->pBuffer, pString->capacity, pFormat, argsCopy); + assert(result >= 0); + va_end(argsCopy); + + if ((size_t)result < pString->capacity) + { + pString->length = (size_t)result; + } + else + { + pString->errorCode = sbgStringResizeBuffer(pString, (size_t)result + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + vsnprintf(pString->pBuffer, pString->capacity, pFormat, args); + pString->length = (size_t)result; + } + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignF(SbgString *pString, const char *pFormat, ...) +{ + assert(pString); + assert(pFormat); + + if (pString->errorCode == SBG_NO_ERROR) + { + va_list args; + + va_start(args, pFormat); + sbgStringAssignVF(pString, pFormat, args); + va_end(args); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringClear(SbgString *pString) +{ + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + sbgStringAssignCString(pString, ""); + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUpperCase(SbgString *pString) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pString->length; i++) + { + pString->pBuffer[i] = (char)toupper((unsigned char)pString->pBuffer[i]); + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToLowerCase(SbgString *pString) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pString->length; i++) + { + pString->pBuffer[i] = (char)tolower((unsigned char)pString->pBuffer[i]); + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimLeft(SbgString *pString) +{ + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmed; + + trimmed = sbgStringTrimLeftCommon(pString); + + if (trimmed) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimRight(SbgString *pString) +{ + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmed; + + trimmed = sbgStringTrimRightCommon(pString); + + if (trimmed) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrim(SbgString *pString) +{ + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + bool trimmedLeft; + bool trimmedRight; + + trimmedLeft = sbgStringTrimLeftCommon(pString); + trimmedRight = sbgStringTrimRightCommon(pString); + + if (trimmedLeft || trimmedRight) + { + sbgStringResizeBuffer(pString, pString->length + 1); + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length < pString->length) + { + pString->pBuffer[length] = '\0'; + pString->length = length; + + sbgStringResizeBuffer(pString, length + 1); + } + } +} + +//----------------------------------------------------------------------// +//- Conversion methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt32(SbgString *pString, int32_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId32, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt32(const SbgString *pString, int32_t *pValue) +{ + SbgErrorCode errorCode; + int result; + int32_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNd32, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint32(SbgString *pString, uint32_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu32, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint32(const SbgString *pString, uint32_t *pValue) +{ + SbgErrorCode errorCode; + int result; + uint32_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNu32, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt64(SbgString *pString, int64_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRId64, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt64(const SbgString *pString, int64_t *pValue) +{ + SbgErrorCode errorCode; + int result; + int64_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNd64, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint64(SbgString *pString, uint64_t value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%" PRIu64, value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint64(const SbgString *pString, uint64_t *pValue) +{ + SbgErrorCode errorCode; + int result; + uint64_t value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%" SCNu64, &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromDouble(SbgString *pString, double value) +{ + assert(pString); + assert(!pString->readOnly); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t length; + size_t oldCapacity; + int result; + + result = snprintf(pString->pBuffer, pString->capacity, "%lf", value); + + assert(result >= 0); + + length = (size_t)result; + + oldCapacity = pString->capacity; + + pString->errorCode = sbgStringResizeBuffer(pString, length + 1); + + if (pString->errorCode == SBG_NO_ERROR) + { + if (length >= oldCapacity) + { + snprintf(pString->pBuffer, pString->capacity, "%lf", value); + } + + pString->length = (size_t)length; + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToDouble(const SbgString *pString, double *pValue) +{ + SbgErrorCode errorCode; + int result; + double value; + + assert(pString); + + result = sscanf(pString->pBuffer, "%lf", &value); + + if (result == 1) + { + *pValue = value; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromStreamBuffer(SbgString *pString, SbgStreamBuffer *pStream) +{ + assert(pString); + + if (pString->errorCode == SBG_NO_ERROR) + { + size_t size; + + size = sbgStreamBufferReadSizeT32(pStream); + + pString->errorCode = sbgStreamBufferGetLastError(pStream); + + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStringResizeBuffer(pString, size); + + if (pString->errorCode == SBG_NO_ERROR) + { + pString->errorCode = sbgStreamBufferReadBuffer(pStream, pString->pBuffer, size); + + if (pString->errorCode == SBG_NO_ERROR) + { + pString->length = size - 1; + } + } + } + } + + return pString->errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString, SbgStreamBuffer *pStream) +{ + SbgErrorCode errorCode; + + assert(pString); + + errorCode = sbgStreamBufferWriteSizeT32(pStream, pString->length + 1); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgStreamBufferWriteBuffer(pStream, pString->pBuffer, pString->length + 1); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Iterator methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API void sbgStringIteratorConstruct(SbgStringIterator *pIterator, const SbgString *pString) +{ + assert(pString); + assert(pIterator); + + pIterator->pCursor = pString->pBuffer; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterator, const char *pSeparators, bool skipEmptyTokens, SbgString *pToken) +{ + SbgErrorCode errorCode; + + assert(pIterator); + assert(pSeparators); + assert(pToken); + + if (pIterator->pCursor) + { + size_t length; + char *pSeparator; + char *pNext; + + pSeparator = strpbrk(pIterator->pCursor, pSeparators); + + if (pSeparator) + { + length = pSeparator - pIterator->pCursor; + + if (skipEmptyTokens) + { + size_t nrSeparators; + + nrSeparators = strspn(pSeparator, pSeparators); + pNext = &pSeparator[nrSeparators]; + } + else + { + pNext = &pSeparator[1]; + } + } + else + { + length = strlen(pIterator->pCursor); + pNext = NULL; + } + + errorCode = sbgStringAssignBuffer(pToken, pIterator->pCursor, length); + + if (errorCode == SBG_NO_ERROR) + { + pIterator->pCursor = pNext; + } + } + else + { + errorCode = SBG_NOT_READY; + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Legacy functions -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgStringCopy(char *pDestination, const char *pSource, size_t destMaxSize) +{ + size_t srcLength; + + // + // Check input arguments + // + assert(pDestination); + assert(pSource); + assert(destMaxSize > 0); + + // + // Get the source string length and add the NULL char + // + srcLength = strlen(pSource) + sizeof(char); + + // + // Make sure the string fits in the provided buffer + // + if (srcLength <= destMaxSize) + { + // + // We can use safely strcpy + // + strcpy(pDestination, pSource); + + return SBG_NO_ERROR; + } + else + { + // + // The destination buffer isn't big enough to hold the provided source string + // Don't forget to save one byte for the NULL terminated char + // + strncpy(pDestination, pSource, destMaxSize-sizeof(char)); + + // + // Append the NULL terminating char + // + pDestination[destMaxSize-1] = '\0'; + + // + // We have a buffer overflow + // + return SBG_BUFFER_OVERFLOW; + } + +} + +SBG_COMMON_LIB_API const char *sbgStringFirstValidChar(const char *pInputStr) +{ + const char *pCurrentStr = pInputStr; + + assert(pInputStr); + + // + // Skip any space or tab chars from the beginning of the string + // + while ((*pCurrentStr != '\0') && isspace((unsigned char)*pCurrentStr) ) + { + pCurrentStr += sizeof(char); + } + + return pCurrentStr; +} diff --git a/crates/sbg-rs/sbgECom/common/string/sbgString.h b/crates/sbg-rs/sbgECom/common/string/sbgString.h new file mode 100644 index 0000000..3810ed1 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/string/sbgString.h @@ -0,0 +1,697 @@ +/*! + * \file sbgString.h + * \ingroup common + * \author SBG Systems + * \date March 20, 2020 + * + * \brief Character string. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_STRING_H +#define SBG_STRING_H + +// sbgCommonLib headers +#include +#include + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Minimum size of the internal buffer, in bytes. + * + * This constant must be strictly positive in order to guarantee that no errors can occur + * during the construction of an empty string. + */ +#define SBG_STRING_INTERNAL_BUFFER_MIN_SIZE (4) + +#if SBG_STRING_INTERNAL_BUFFER_MIN_SIZE == 0 +#error "invalid minimum internal buffer size" +#endif + +/*! + * Default size of the internal buffer, in bytes. + */ +#define SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE (16) + +#ifndef SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE +#define SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE SBG_STRING_INTERNAL_BUFFER_DEFAULT_SIZE +#elif SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE < SBG_STRING_INTERNAL_BUFFER_MIN_SIZE +#error "invalid internal buffer size" +#endif // SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * String. + * + * The buffer contains a C null-terminated string. The null-terminating character is not + * considered to be part of the string content. + * + * Attempts to modify a read-only string lead to undefined behavior. + * + * Attempts to modify a string while the last modification operation error is set are canceled + * and return the last error code. + */ +typedef struct _SbgString +{ + /*! + * Internal buffer. + * + * The internal buffer is used for small strings to avoid dynamic allocation. + */ + char internalBuffer[SBG_CONFIG_STRING_INTERNAL_BUFFER_SIZE]; + + char *pBuffer; /*!< Buffer. */ + size_t capacity; /*!< Capacity of the buffer, in bytes. */ + size_t length; /*!< Length of the string (not including the terminating null character), in bytes. */ + + bool readOnly; /*!< True if the string is read-only. */ + bool isStatic; /*!< True if the buffer is statically allocated. */ + + SbgErrorCode errorCode; /*!< Error code of the last modification operation. */ +} SbgString; + +/*! + * String iterator. + */ +typedef struct _SbgStringIterator +{ + const char *pCursor; /*!< Cursor. */ +} SbgStringIterator; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * String constructor. + * + * The rationale for this constructor is to guarantee that no errors may occur on the + * construction of empty strings. + * + * \param[in] pString String. + */ +SBG_COMMON_LIB_API void sbgStringConstructEmpty(SbgString *pString); + +/*! + * String constructor. + * + * The string content is initialized from the given source string. If that string is NULL, + * the string is initialized with an empty content. + * + * \param[in] pString String. + * \param[in] pSourceString Source string, may be NULL. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstruct(SbgString *pString, const SbgString *pSourceString); + +/*! + * String constructor. + * + * The string content is initialized from the given C null-terminated string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructCString(SbgString *pString, const char *pCString); + +/*! + * String constructor. + * + * The string content is initialized from formatted data from a variable argument list. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); + +/*! + * String constructor. + * + * The string content is initialized from formatted data. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); + +/*! + * String constructor. + * + * The string content is initialized from the given C null-terminated string. + * + * If the C null-terminated string is NULL then the string contains the content of the given + * buffer before construction. In this case the content of the given buffer must be a C + * null-terminated string. + * + * The given buffer is owned by the string and must not be modified while the string is in use. + * After destroying the string, the buffer contains the content of the string immediately before + * destruction. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string, may be NULL. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringConstructStatic(SbgString *pString, const char *pCString, char *pBuffer, size_t size); + +/*! + * String constructor. + * + * The string cannot be modified. + * + * The given C null-terminated string provides both the statically allocated buffer + * for the string, as well as its content. It is owned by the string and must not be modified. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + */ +SBG_COMMON_LIB_API void sbgStringConstructReadOnly(SbgString *pString, const char *pCString); + +/*! + * String destructor. + * + * \param[in] pString String. + */ +SBG_COMMON_LIB_API void sbgStringDestroy(SbgString *pString); + +/*! + * Compare a string to another string. + * + * \param[in] pString String. + * \param[in] pOtherString Other string. + * \return The return value is 0 if both strings are equal, + * less than 0 if the string value is less than the other string value, + * greater than 0 if the string value is greater than the other string value. + */ +SBG_COMMON_LIB_API int32_t sbgStringCompare(const SbgString *pString, const SbgString *pOtherString); + +/*! + * Compare a string to a C null-terminated string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return The return value is 0 if the string is equal to the C null-terminated string, + * less than 0 if the string value is less than the C null-terminated string value, + * greater than 0 if the string value is greater than the C null-terminated string value. + */ +SBG_COMMON_LIB_API int32_t sbgStringCompareCString(const SbgString *pString, const char *pCString); + +/*! + * Compare a string to another string, ignoring case differences. + * + * \param[in] pString String. + * \param[in] pOtherString Other string. + * \return The return value is 0 if both strings are equal, + * less than 0 if the string value is less than the other string value, + * greater than 0 if the string value is greater than the other string value. + */ +SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCase(const SbgString *pString, const SbgString *pOtherString); + +/*! + * Compare a string to a C null-terminated string, ignoring case differences. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return The return value is 0 if the string is equal to the C null-terminated string, + * less than 0 if the string value is less than the C null-terminated string value, + * greater than 0 if the string value is greater than the C null-terminated string value. + */ +SBG_COMMON_LIB_API int32_t sbgStringCompareIgnoreCaseCString(const SbgString *pString, const char *pCString); + +/*! + * Compute the hash value of a string. + * + * \param[in] pString String. + * \return Hash value. + */ +SBG_COMMON_LIB_API size_t sbgStringHash(const SbgString *pString); + +/*! + * Get the length of a string (not including the terminating null character), in bytes. + * + * \param[in] pString String. + * \return Length, in bytes. + */ +SBG_COMMON_LIB_API size_t sbgStringGetLength(const SbgString *pString); + +/*! + * Get a C null-terminated string version of a string. + * + * \param[in] pString String. + * \return C null-terminated string. + */ +SBG_COMMON_LIB_API const char *sbgStringGetCString(const SbgString *pString); + +/*! + * Get a character in a string. + * + * \param[in] pString String. + * \param[in] index Index. + * \param[out] pChar Character. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringCharAt(const SbgString *pString, size_t index, char *pChar); + +/*! + * Get the index of the first occurrence of a string in a string. + * + * \param[in] pString String. + * \param[in] pStringToFind String to find. + * \return Index of the first occurrence or SIZE_MAX if not found. + */ +SBG_COMMON_LIB_API size_t sbgStringFind(const SbgString *pString, const SbgString *pStringToFind); + +/*! + * Get the index of the first occurrence of the given C null-terminated string in a string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return Index of the first occurrence or SIZE_MAX if not found. + */ +SBG_COMMON_LIB_API size_t sbgStringFindCString(const SbgString *pString, const char *pCString); + +/*! + * Get a substring from a string. + * + * The substring must be constructed before calling this function. If an error occurs, the last error + * of the substring is set by this function. + * + * \param[in] pString String. + * \param[in] startIndex Start index. + * \param[in] endIndex End index. + * \param[out] pSubstring Substring. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringSubstring(const SbgString *pString, size_t startIndex, size_t endIndex, SbgString *pSubstring); + +/*! + * Check if a C null-terminated string is at the beginning of a string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return True if the C null-terminated string is at the beginning. + */ +SBG_COMMON_LIB_API bool sbgStringStartsWith(const SbgString *pString, const char *pCString); + +/*! + * Check if a C null-terminated string is at the end of a string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return True if the C null-terminated string is at the end. +*/ +SBG_COMMON_LIB_API bool sbgStringEndsWith(const SbgString *pString, const char *pCString); + +//----------------------------------------------------------------------// +//- Modification methods -// +//----------------------------------------------------------------------// + +/*! + * Get the last error of a modification operation on a string. + * + * \param[in] pString String. + * \return Last modification error. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringGetLastError(const SbgString *pString); + +/*! + * Clear the last modification operation error of a string. + * + * \param[in] pString String. + */ +SBG_COMMON_LIB_API void sbgStringClearLastError(SbgString *pString); + +/*! + * Set a character in a string. + * + * The given index must be strictly lower than the string length. The given character + * must not be a null character. + * + * \param[in] pString String. + * \param[in] index Index. + * \param[in] c Character. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringSetCharAt(SbgString *pString, size_t index, char c); + +/*! + * Append a string to another string. + * + * \param[in] pString String. + * \param[in] pAppendString String to append. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppend(SbgString *pString, const SbgString *pAppendString); + +/*! + * Append a C null-terminated string to a string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendCString(SbgString *pString, const char *pCString); + +/*! + * Append formatted data from a variable argument list to a string. + * + * The string format, as well as the variable argument list, are the same as for vprintf. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); + +/*! + * Append formatted data to a string. + * + * The string format, as well as the variable arguments, are the same as for printf. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAppendF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); + +/*! + * Assign a string to another string. + * + * \param[in] pString String. + * \param[in] pAssignString String to assign. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssign(SbgString *pString, const SbgString *pAssignString); + +/*! + * Assign a C null-terminated string to a string. + * + * \param[in] pString String. + * \param[in] pCString C null-terminated string. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignCString(SbgString *pString, const char *pCString); + +/*! + * Assign formatted data from a variable argument list to a string. + * + * The string format, as well as the variable argument list, are the same as for vprintf. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \param[in] args Variable argument list. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignVF(SbgString *pString, const char *pFormat, va_list args) SBG_CHECK_FORMAT(printf, 2, 0); + +/*! + * Assign formatted data to a string. + * + * The string format, as well as the variable arguments, are the same as for printf. + * + * \param[in] pString String. + * \param[in] pFormat Format. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringAssignF(SbgString *pString, const char *pFormat, ...) SBG_CHECK_FORMAT(printf, 2, 3); + +/*! + * Clear a string. + * + * After returning from this call the string is empty. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringClear(SbgString *pString); + +/*! + * Convert all the characters of a string to upper case. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUpperCase(SbgString *pString); + +/*! + * Convert all the characters of a string to lower case. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToLowerCase(SbgString *pString); + +/*! + * Remove all spacing characters at the beginning of a string. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimLeft(SbgString *pString); + +/*! + * Remove all spacing characters at the end of a string. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrimRight(SbgString *pString); + +/*! + * Remove all spacing characters at the beginning and the end of a string. + * + * \param[in] pString String. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringTrim(SbgString *pString); + +/*! + * Truncate a string. + * + * If the given length is greater than or equal to the length of the string, + * this function doesn't modify the string. + * + * \param[in] pString String. + * \param[in] length Length, in bytes. + */ +SBG_COMMON_LIB_API void sbgStringTruncate(SbgString *pString, size_t length); + +//----------------------------------------------------------------------// +//- Conversion methods -// +//----------------------------------------------------------------------// + +/*! + * Convert and assign a 32-bits signed integer value to a string. + * + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt32(SbgString *pString, int32_t value); + +/*! + * Convert a string to a 32-bits signed integer value. + * + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt32(const SbgString *pString, int32_t *pValue); + +/*! + * Convert and assign a 32-bits unsigned integer value to a string. + * + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint32(SbgString *pString, uint32_t value); + +/*! + * Convert a string to a 32-bits unsigned integer value. + * + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint32(const SbgString *pString, uint32_t *pValue); + +/*! + * Convert and assign a 64-bits signed integer value to a string. + * + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromInt64(SbgString *pString, int64_t value); + +/*! + * Convert a string to a 64-bits signed integer value. + * + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToInt64(const SbgString *pString, int64_t *pValue); + +/*! + * Convert and assign a 64-bits unsigned integer value to a string. + * + * \param[in] pString String. + * \param[in] value Value + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromUint64(SbgString *pString, uint64_t value); + +/*! + * Convert a string to a 64-bits unsigned integer value. + * + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToUint64(const SbgString *pString, uint64_t *pValue); + +/*! + * Convert and assign a double value to a string. + * + * \param[in] pString String. + * \param[in] value Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromDouble(SbgString *pString, double value); + +/*! + * Convert a string to a double value. + * + * \param[in] pString String. + * \param[out] pValue Value. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToDouble(const SbgString *pString, double *pValue); + +/*! + * Read a string from a stream buffer. + * + * \param[in] pString String. + * \param[in] pStream Stream buffer. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringFromStreamBuffer(SbgString *pString, SbgStreamBuffer *pStream); + +/*! + * Write a string to a stream buffer. + * + * \param[in] pString String. + * \param[out] pStream Stream buffer. + * \return SBG_NO_ERROR if successful. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringToStreamBuffer(const SbgString *pString, SbgStreamBuffer *pStream); + +//----------------------------------------------------------------------// +//- Iterator methods -// +//----------------------------------------------------------------------// + +/*! + * String iterator constructor. + * + * If the given string is modified while the iterator is used, the behavior is undefined. + * + * \param[in] pIterator String iterator. + * \param[in] pString String. + */ +SBG_COMMON_LIB_API void sbgStringIteratorConstruct(SbgStringIterator *pIterator, const SbgString *pString); + +/*! + * Walk to the next token of a string. + * + * The token string must be constructed before calling this function. If an error occurs, the last error + * of the token string is set by this function. + * + * \param[in] pIterator String iterator. + * \param[in] pSeparators C null-terminated string containing separator characters. + * \param[in] skipEmptyTokens If true, empty tokens are skipped. + * \param[out] pToken Token. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if there are no more tokens. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringIteratorWalk(SbgStringIterator *pIterator, const char *pSeparators, bool skipEmptyTokens, SbgString *pToken); + +//----------------------------------------------------------------------// +//- Legacy functions -// +//----------------------------------------------------------------------// + +/*! + * Safely copy source string to destination according to destination provided buffer size. + * + * This method tends to replace strncpy where you would like to safely copy a string with buffer overflow checking. + * If the provided output buffer is too small, the method will return a valid NULL terminated C string in destination. + * This condition will be returned using SBG_BUFFER_OVERFLOW error code. + * + * \param[out] pDestination The destination string that should hold the copied input string. + * \param[in] pSource The source NULL terminated C string to copy to the destination. + * \param[in] destMaxSize The destination buffer size including the null terminating char. + * \return SBG_NO_ERROR if the source string has been copied to destination + * SBG_BUFFER_OVERFLOW if the destination is too small to hold the source string. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgStringCopy(char *pDestination, const char *pSource, size_t destMaxSize); + +/*! + * Skip all spacing characters from the beginning of a string. + * + * \param[in] pInputStr Pointer on a read only string to trim. + * \return Pointer to the first useful char. + */ +SBG_COMMON_LIB_API const char *sbgStringFirstValidChar(const char *pInputStr); + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// + +#ifdef __cplusplus +} +#endif + +#endif // SBG_STRING_H diff --git a/crates/sbg-rs/sbgECom/common/swap/sbgSwap.c b/crates/sbg-rs/sbgECom/common/swap/sbgSwap.c new file mode 100644 index 0000000..2ebbd5c --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/swap/sbgSwap.c @@ -0,0 +1 @@ +#include "sbgSwap.h" diff --git a/crates/sbg-rs/sbgECom/common/swap/sbgSwap.h b/crates/sbg-rs/sbgECom/common/swap/sbgSwap.h new file mode 100644 index 0000000..25d2430 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/swap/sbgSwap.h @@ -0,0 +1,144 @@ +/*! + * \file sbgSwap.h + * \ingroup common + * \author SBG Systems + * \date 14 January 2013 + * + * \brief Set of functions used to swap numbers. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_SWAP_H +#define SBG_SWAP_H + +//----------------------------------------------------------------------// +//- Header (open extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +extern "C" { +#endif + +#include + +//----------------------------------------------------------------------// +//- Internal swap functions -// +//----------------------------------------------------------------------// + +/*! + * Swap a uint16_t number. + * \param[in] x The uint16_t to swap. + * \return The swapped value. + */ +SBG_INLINE uint16_t sbgSwap16(uint16_t x) +{ + return ((x<<8)|(x>>8)); +} + +/*! + * Swap a uint32_t number. + * \param[in] x The uint32_t to swap. + * \return The swapped value. + */ +SBG_INLINE uint32_t sbgSwap32(uint32_t x) +{ + return ((x << 24) | ((x << 8) & (0xFF0000)) | ((x >> 8) & (0xFF00)) | (x >> 24)); +} + +/*! + * Swap a uint64_t number. + * \param[in] x The uint64_t to swap. + * \return The swapped value. + */ +SBG_INLINE uint64_t sbgSwap64(uint64_t x) +{ + uint32_t hi, lo; + + // + // Separate into high and low 32-bit values + // + lo = (uint32_t)(x&0xFFFFFFFF); + x >>= 32; + hi = (uint32_t)(x&0xFFFFFFFF); + + // + // Swap each part and rebuild our 64 bit vale + // + x = sbgSwap32(lo); + x <<= 32; + x |= sbgSwap32(hi); + + return x; +} + +/*! + * Swap a float number. + * \param[in] val The float to swap. + * \return The swapped value. + */ +SBG_INLINE float sbgSwapFloat(float val) +{ + FloatNint tmpFloat; + + // + // We use a union to do the type punning + // + tmpFloat.valF = val; + tmpFloat.valU = sbgSwap32(tmpFloat.valU); + + // + // Return the swapped float + // + return tmpFloat.valF; +} + +/*! + * Swap a double number. + * \param[in] val The double to swap. + * \return The swapped value. + */ +SBG_INLINE double sbgSwapDouble(double val) +{ + DoubleNint tmpDouble; + + // + // We use a union to do the type punning + // + tmpDouble.valF = val; + tmpDouble.valU = sbgSwap64(tmpDouble.valU); + + // + // Return the swapped double + // + return tmpDouble.valF; +} + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif /* SBG_SWAP_H */ diff --git a/crates/sbg-rs/sbgECom/common/version/sbgVersion.c b/crates/sbg-rs/sbgECom/common/version/sbgVersion.c new file mode 100644 index 0000000..a24c339 --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/version/sbgVersion.c @@ -0,0 +1,561 @@ +// Project headers +#include + + +// Local headers +#include "sbgVersion.h" + +//----------------------------------------------------------------------// +//- Constructor -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API void sbgVersionCreateBasic(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint8_t revision, uint8_t build) +{ + assert(pVersion); + assert(major < 128); + + pVersion->softwareScheme = false; + + pVersion->major = major; + pVersion->minor = minor; + pVersion->rev = revision; + pVersion->build = build; + + pVersion->qualifier = SBG_VERSION_QUALIFIER_DEV; +} + +SBG_COMMON_LIB_API void sbgVersionCreateSoftware(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint16_t build, SbgVersionQualifier qualifier) +{ + assert(pVersion); + assert(major < 64); + assert(minor < 64); + + pVersion->softwareScheme = true; + + pVersion->major = major; + pVersion->minor = minor; + pVersion->build = build; + pVersion->qualifier = qualifier; + + pVersion->rev = 0; +} + +SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgVersion *pOtherVersion) +{ + assert(pVersion); + assert(pOtherVersion); + + // + // Copy each member individually to avoid warnings about reading uninitialized bytes. + // + pVersion->softwareScheme = pOtherVersion->softwareScheme; + pVersion->qualifier = pOtherVersion->qualifier; + pVersion->major = pOtherVersion->major; + pVersion->minor = pOtherVersion->minor; + pVersion->rev = pOtherVersion->rev; + pVersion->build = pOtherVersion->build; +} + +//----------------------------------------------------------------------// +//- Version encoding / decoding methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API void sbgVersionDecode(uint32_t encodedVersion, SbgVersion *pVersionInfo) +{ + assert(pVersionInfo); + + // + // Test if we have a software version scheme + // + if (encodedVersion&SBG_VERSION_SOFT_SCHEME) + { + // + // We have a software scheme, decode it + // + pVersionInfo->softwareScheme = true; + + // + // Decode the software scheme fields + // + pVersionInfo->qualifier = (SbgVersionQualifier)((encodedVersion >> SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT) & SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK); + pVersionInfo->major = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT) & SBG_VERSION_SOFT_SCHEME_MAJOR_MASK; + pVersionInfo->minor = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT) & SBG_VERSION_SOFT_SCHEME_MINOR_MASK; + pVersionInfo->build = (encodedVersion >> SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT) & SBG_VERSION_SOFT_SCHEME_BUILD_MASK; + + // + // Set the revision to zero as it's not used + // + pVersionInfo->rev = 0; + } + else + { + // + // We have a basic scheme, decode it + // + pVersionInfo->softwareScheme = false; + + // + // Decode the software scheme fields + // + pVersionInfo->major = (encodedVersion >> 24) & 0xFF; + pVersionInfo->minor = (encodedVersion >> 16) & 0xFF; + pVersionInfo->rev = (encodedVersion >> 8) & 0xFF; + pVersionInfo->build = (encodedVersion >> 0) & 0xFF; + + // + // Set the qualifier to zero + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_DEV; + } +} + +SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo) +{ + uint32_t encodedVersion = 0; + + assert(pVersionInfo); + + // + // Test if we have a software scheme or a basic one + // + if (pVersionInfo->softwareScheme) + { + // + // We have a software, scheme, so test that the version is valid + // + assert((pVersionInfo->major <= 63) && (pVersionInfo->minor <= 63) && (pVersionInfo->rev == 0)); + + // + // Indicate that we have a software version scheme + // + encodedVersion = SBG_VERSION_SOFTWARE(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->build, pVersionInfo->qualifier); + } + else + { + // + // We have a basic version scheme so check parameter validity + // + assert(pVersionInfo->major <= 127); + + // + // Encode the basic version information + // + encodedVersion = SBG_VERSION_BASIC(pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); + } + + return encodedVersion; +} + +//----------------------------------------------------------------------// +//- Version comparaison methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThreshold threshold) +{ + int32_t result; + + assert(pVersionA); + assert(pVersionB); + + // + // Check that the two versions are using the same scheme + // + assert(pVersionA->softwareScheme == pVersionB->softwareScheme); + + // + // Do the comparaison according to the selected threshold + // Start by compairing the major field + // + result = pVersionA->major - pVersionB->major; + + // + // Test if we have to also compare the minor field + // + if ( (result == 0) && ((threshold == SBG_VERSION_CMP_THRESHOLD_MINOR) || (threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER)) ) + { + // + // Update the result using the minor indication + // + result = pVersionA->minor - pVersionB->minor; + + // + // Test if we have to also compare the revision field (for basic version only) + // + if ( (result == 0) && ((threshold == SBG_VERSION_CMP_THRESHOLD_REVISION) || (threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER)) ) + { + // + // Test if we have a software scheme or a basic scheme version + // + if (pVersionA->softwareScheme) + { + // + // We have a software scheme so set the result to 0 + // + result = 0; + } + else + { + // + // We have a basic scheme so we can compare the revision field + // + result = pVersionA->rev - pVersionB->rev; + } + + // + // Test if we have to also compare the build field + // + if ( (result == 0) && ((threshold == SBG_VERSION_CMP_THRESHOLD_BUILD) || (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER)) ) + { + // + // Compare the build field + // + result = pVersionA->build - pVersionB->build; + + // + // Test if we have to also compare the qualifier field + // + if ( (result == 0) && (threshold == SBG_VERSION_CMP_THRESHOLD_QUALIFIER) ) + { + // + // Test if we have a software scheme + // + if (pVersionA->softwareScheme) + { + // + // We have a software scheme so set the result to 0 + // + result = pVersionA->qualifier - pVersionB->qualifier; + } + else + { + // + // We have a basic scheme so set the result to 0 + // + result = 0; + } + } + } + } + } + + return result; +} + +SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThreshold threshold) +{ + SbgVersion versionAInfo; + SbgVersion versionBInfo; + + // + // Decode the versions + // + sbgVersionDecode(versionA, &versionAInfo); + sbgVersionDecode(versionB, &versionBInfo); + + // + // Do the comparaison + // + return sbgVersionCompare(&versionAInfo, &versionBInfo, threshold); +} + +SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRange(const SbgVersion *pLowerVersion, const SbgVersion *pHigherVersion, const SbgVersion *pVersion) +{ + assert(pLowerVersion); + assert(pHigherVersion); + assert(pVersion); + + // + // Use the encoded version to speed up the comparaison + // + return sbgVersionIsWithinRangeEncoded(sbgVersionEncode(pLowerVersion), sbgVersionEncode(pHigherVersion), sbgVersionEncode(pVersion)); +} + +SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, uint32_t higherVersion, uint32_t version) +{ + // + // Make sure that all versions are using the same scheme + // + assert(lowerVersion <= higherVersion); + assert(( (sbgVersionIsUsingSoftwareScheme(lowerVersion) == sbgVersionIsUsingSoftwareScheme(higherVersion)) && + (sbgVersionIsUsingSoftwareScheme(lowerVersion) == sbgVersionIsUsingSoftwareScheme(version))) || + ( (sbgVersionIsUsingSoftwareScheme(lowerVersion) != sbgVersionIsUsingSoftwareScheme(higherVersion)) && + (sbgVersionIsUsingSoftwareScheme(lowerVersion) != sbgVersionIsUsingSoftwareScheme(version)))); + + // + // We can compare safely the encoded version directly + // + if (version < lowerVersion) + { + return -1; + } + else if (version > higherVersion) + { + return 1; + } + else + { + return 0; + } +} + +//----------------------------------------------------------------------// +//- Version to string methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgVersionToString(const SbgVersion *pVersionInfo, char *pBuffer, size_t sizeOfBuffer) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pVersionInfo); + assert(pBuffer); + assert(sizeOfBuffer>0); + + // + // Test if we have a basic or software version scheme + // + if (pVersionInfo->softwareScheme) + { + // + // We have a software version scheme + // Test that the buffer is big enough to store the generated string (31.31.65535-hotfix) + // + if (sizeOfBuffer >= 19) + { + // + // Generate the string version + // + sprintf(pBuffer, "%u.%u.%u-", pVersionInfo->major, pVersionInfo->minor, pVersionInfo->build); + + // + // Append the qualifier + // + switch (pVersionInfo->qualifier) + { + case SBG_VERSION_QUALIFIER_DEV: + strcat(pBuffer, "dev"); + break; + case SBG_VERSION_QUALIFIER_ALPHA: + strcat(pBuffer, "alpha"); + break; + case SBG_VERSION_QUALIFIER_BETA: + strcat(pBuffer, "beta"); + break; + case SBG_VERSION_QUALIFIER_RC: + strcat(pBuffer, "rc"); + break; + case SBG_VERSION_QUALIFIER_STABLE: + strcat(pBuffer, "stable"); + break; + case SBG_VERSION_QUALIFIER_HOT_FIX: + strcat(pBuffer, "hotfix"); + break; + default: + break; + } + } + else + { + // + // Output buffer is to small + // + errorCode = SBG_BUFFER_OVERFLOW; + } + + } + else + { + // + // We have a basic version scheme, generate the string + // Test that the buffer is big enough to store the generated string + // + if (sizeOfBuffer >= 16) + { + // + // Generate the string version + // + sprintf(pBuffer, "%u.%u.%u.%u", pVersionInfo->major, pVersionInfo->minor, pVersionInfo->rev, pVersionInfo->build); + } + else + { + // + // Output buffer is to small + // + errorCode = SBG_BUFFER_OVERFLOW; + } + } + + // + // Return status of operation + // + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, size_t sizeOfBuffer) +{ + SbgVersion versionInfo; + + // + // Decode the versions + // + sbgVersionDecode(version, &versionInfo); + + // + // Return the version as a string + // + return sbgVersionToString(&versionInfo, pBuffer, sizeOfBuffer); +} + +//----------------------------------------------------------------------// +//- String to version methods -// +//----------------------------------------------------------------------// + +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + char qualifierStr[32]; + unsigned int major; + unsigned int minor; + unsigned int rev; + unsigned int build; + + assert(pVersionStr); + assert(pVersionInfo); + + // + // Zero init the returned version struct + // + memset(pVersionInfo, 0x00, sizeof(SbgVersion)); + + // + // Try to parse a basic version + // + if (sscanf(pVersionStr, "%u.%u.%u.%u", &major, &minor, &rev, &build) == 4) + { + // + // We have read successfully a basic version + // + pVersionInfo->softwareScheme = false; + + // + // Fill the version numbers + // + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + pVersionInfo->rev = (uint8_t)rev; + pVersionInfo->build = (uint8_t)build; + } + else if (sscanf(pVersionStr, "%u.%u.%u-%s", &major, &minor, &build, qualifierStr) == 4) + { + // + // We have read successfully a software scheme version + // + pVersionInfo->softwareScheme = true; + + // + // Fill the version numbers + // + pVersionInfo->major = (uint8_t)major; + pVersionInfo->minor = (uint8_t)minor; + pVersionInfo->build = (uint16_t)build; + + // + // Also convert the qualifier + // + if (!strcmp(qualifierStr, "dev")) + { + // + // Dev qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_DEV; + } + else if (!strcmp(qualifierStr, "alpha")) + { + // + // Alpha qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_ALPHA; + } + else if (!strcmp(qualifierStr, "beta")) + { + // + // Beta qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_BETA; + } + else if (!strcmp(qualifierStr, "rc")) + { + // + // Release Candidate qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_RC; + } + else if (!strcmp(qualifierStr, "stable")) + { + // + // Stable qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_STABLE; + } + else if (!strcmp(qualifierStr, "hotfix")) + { + // + // Hot Fix qualifier + // + pVersionInfo->qualifier = SBG_VERSION_QUALIFIER_HOT_FIX; + } + else + { + // + // Unknown qualifier + // + errorCode = SBG_INVALID_PARAMETER; + } + } + else + { + // + // Invalid format + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgVersion versionInfo; + + assert(pVersionStr); + assert(pVersion); + + // + // Parse the version from a string + // + errorCode = sbgVersionFromString(pVersionStr, &versionInfo); + + // + // Test that no error has occurred + // + if (errorCode == SBG_NO_ERROR) + { + // + // Encode the version and return it + // + *pVersion = sbgVersionEncode(&versionInfo); + } + else + { + // + // An error has occurred so return a zero version + // + *pVersion = 0; + } + + // + // Return error + // + return errorCode; +} + diff --git a/crates/sbg-rs/sbgECom/common/version/sbgVersion.h b/crates/sbg-rs/sbgECom/common/version/sbgVersion.h new file mode 100644 index 0000000..ce4888a --- /dev/null +++ b/crates/sbg-rs/sbgECom/common/version/sbgVersion.h @@ -0,0 +1,474 @@ +/*! + * \file sbgVersion.h + * \ingroup common + * \author SBG Systems + * \date April 23, 2015 + * + * \brief Helper methods and definitions used to handle version. + * + * Version information is stored within a single uint32_t. + * There are two different versions schemes to better handle software revisions and file format / hardware ones. + * + * Software version are only used for firmware and software. + * This versions is defined as a `Major`.`Minor`.`Build`-`Qualifier` for example: 1.3.1845-rc + * Major and Minor can range from 0 to 63. + * Build is an uint16_t ranging from 0 to 65535. + * Qualifier is an enum encoded on 3 bits. + * + * Basic version is used for hardware or file formats and is the legacy one for software. + * This versions is defined as a `Major`.`Minor`.`Revision`.`Build` + * Each element is stored in an uint8_t. + * The Major version should NEVER go above 31 + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_VERSION_H +#define SBG_VERSION_H + +#ifdef __cplusplus +extern "C" { +#endif + +// Project headers +#include + + +//----------------------------------------------------------------------// +//- Version related definitions -// +//----------------------------------------------------------------------// +#define SBG_VERSION_SOFT_SCHEME (0x00000001u << 31) /*!< Set to 1 to indicate a version number that uses the software scheme. Set to 0 means a basic version scheme. */ + +#define SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK (0x07) /*!< Mask used to keep only the version qualifier enum. */ +#define SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT (28) /*!< Bitshift to apply to get the qualifier enum. */ + +#define SBG_VERSION_SOFT_SCHEME_MAJOR_MASK (0x3F) /*!< Mask used to keep only the major version field. */ +#define SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT (22) /*!< Bitshift used to get the major version field. */ + +#define SBG_VERSION_SOFT_SCHEME_MINOR_MASK (0x3F) /*!< Mask used to keep only the minor version field. */ +#define SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT (16) /*!< Bitshift used to get the minor version field. */ + +#define SBG_VERSION_SOFT_SCHEME_BUILD_MASK (0xFFFF) /*!< Mask used to keep only the build version field. */ +#define SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT (0) /*!< Bitshift used to get the build version field. */ + +/*! + * Version qualifier enum definition. + * This enum is used to identify if we have a basic version scheme or a software one. + * If it's a software one, we can easly know if the software version is a beta, a stable one. + */ +typedef enum _SbgVersionQualifier +{ + SBG_VERSION_QUALIFIER_DEV = 0, /*!< Development only version or pre-alpha. Customer shouldn't get this version qualifier. */ + SBG_VERSION_QUALIFIER_ALPHA = 1, /*!< Alpha version, missing features, can be unstable and could cause crashes or data loss. API can still change. */ + SBG_VERSION_QUALIFIER_BETA = 2, /*!< Beta version, features are freezed, can be unstable and could cause crashes or data loss. API shouldn't change. */ + SBG_VERSION_QUALIFIER_RC = 3, /*!< Release Candidate, features are freezed, with no known bug. API is freezed. */ + SBG_VERSION_QUALIFIER_STABLE = 4, /*!< Stable release, the version is the standard delivered one. */ + SBG_VERSION_QUALIFIER_HOT_FIX = 5 /*!< Hot fixes were applied on a stable release. It should be bug fixes. This qualifier is temporary as the version should return to stable release as soon as the test procedure has been performed. */ +} SbgVersionQualifier; + +/*! + * Version qualifier enum definition. + * This enum is used to identify if we have a basic version scheme or a software one. + * If it's a software one, we can easily know if the software version is a beta, a stable one. + */ +typedef enum _SbgVersionCmpThreshold +{ + SBG_VERSION_CMP_THRESHOLD_MAJOR = 0, /*!< Compare only the major field. */ + SBG_VERSION_CMP_THRESHOLD_MINOR = 1, /*!< Compare the major and minor fields. */ + SBG_VERSION_CMP_THRESHOLD_REVISION = 2, /*!< Compare the major, minor and revision fields (only for basic versions). */ + SBG_VERSION_CMP_THRESHOLD_BUILD = 3, /*!< Compare the major, minor, revision and build fields. */ + SBG_VERSION_CMP_THRESHOLD_QUALIFIER = 4 /*!< Compare the major, minor, revision, build and qualifier fields (only for software scheme). */ +} SbgVersionCmpThreshold; + +/* + * DEPRECATED compatibility definitions + */ +#define SBG_VERSION_CMP_THRESOLD_MAJOR SBG_VERSION_CMP_THRESHOLD_MAJOR +#define SBG_VERSION_CMP_THRESOLD_MINOR SBG_VERSION_CMP_THRESHOLD_MINOR +#define SBG_VERSION_CMP_THRESOLD_REVISION SBG_VERSION_CMP_THRESHOLD_REVISION +#define SBG_VERSION_CMP_THRESOLD_BUILD SBG_VERSION_CMP_THRESHOLD_BUILD +#define SBG_VERSION_CMP_THRESOLD_QUALIFIER SBG_VERSION_CMP_THRESHOLD_QUALIFIER + +/*! + * This structure contains all the fields provided by a version number. + * It handles both basic and software version numbers. + */ +typedef struct _SbgVersion +{ + bool softwareScheme; /*!< Set to true if it's a software scheme or false if it's a basic one. */ + SbgVersionQualifier qualifier; /*!< Qualifier of the current version */ + uint8_t major; /*!< Major version */ + uint8_t minor; /*!< Minor version */ + uint8_t rev; /*!< Revision number - left to 0 in case of software version */ + uint16_t build; /*!< Build number */ +} SbgVersion; + +//----------------------------------------------------------------------// +//- Macro definitions for encode versions -// +//----------------------------------------------------------------------// + +/*! + * Compile a version number using the basic scheme based on major, minor, revision and build information + * + * \param[in] major The major version between 0 to 127. + * \param[in] minor The minor version between 0 to 255. + * \param[in] rev The revision version between 0 to 255. + * \param[in] build The build number between 0 to 255. + * \return The encoded version number. + */ +#define SBG_VERSION_BASIC(major, minor, rev, build) ( (((uint32_t)(major)) << 24) | \ + (((uint32_t)(minor)) << 16) | \ + (((uint32_t)(rev)) << 8) | \ + (((uint32_t)(build))) ) + +/*! + * Compile a version number using the software scheme based on major, minor, build, qualifier. + * + * \param[in] major The major version between 0 to 63. + * \param[in] minor The minor version between 0 to 63. + * \param[in] build The build version between 0 to 65535. + * \param[in] qualifier The version qualifier within the SbgVersionQualifier enum. + * \return The encoded version number. + */ +#define SBG_VERSION_SOFTWARE(major, minor, build, qualifier) ( SBG_VERSION_SOFT_SCHEME | \ + ((((uint32_t)(qualifier)) & SBG_VERSION_SOFT_SCHEME_QUALIFIER_MASK) << SBG_VERSION_SOFT_SCHEME_QUALIFIER_SHIFT) | \ + ((((uint32_t)(major)) & SBG_VERSION_SOFT_SCHEME_MAJOR_MASK) << SBG_VERSION_SOFT_SCHEME_MAJOR_SHIFT) | \ + ((((uint32_t)(minor)) & SBG_VERSION_SOFT_SCHEME_MINOR_MASK) << SBG_VERSION_SOFT_SCHEME_MINOR_SHIFT) | \ + ((((uint32_t)(build)) & SBG_VERSION_SOFT_SCHEME_BUILD_MASK) << SBG_VERSION_SOFT_SCHEME_BUILD_SHIFT) ) +//----------------------------------------------------------------------// +//- Constructor -// +//----------------------------------------------------------------------// + +/*! + * Create a basic scheme version given the components major.minor.rev.build + * + * \param[out] pVersion Version instance + * \param[in] major The major component between 0 to 127 + * \param[in] minor The minor component between 0 to 255 + * \param[in] revision The revision component between 0 to 255 + * \param[in] build The build component between 0 to 255 + */ +SBG_COMMON_LIB_API void sbgVersionCreateBasic(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint8_t revision, uint8_t build); + +/*! + * Create a software scheme version given the components major.minor.build.qualifier + * + * \param[out] pVersion Version instance + * \param[in] major The major component between 0 to 63 + * \param[in] minor The minor component between 0 to 63 + * \param[in] build The build component between 0 to 65535 + * \param[in] qualifier The version qualifier enum value. + */ +SBG_COMMON_LIB_API void sbgVersionCreateSoftware(SbgVersion *pVersion, uint8_t major, uint8_t minor, uint16_t build, SbgVersionQualifier qualifier); + +/*! + * Version copy constructor. + * + * \param[in] pVersion Version. + * \param[in] pOtherVersion Version to copy from. + */ +SBG_COMMON_LIB_API void sbgVersionConstructCopy(SbgVersion *pVersion, const SbgVersion *pOtherVersion); + +//----------------------------------------------------------------------// +//- Version encoding / decoding methods -// +//----------------------------------------------------------------------// + +/*! + * Fill a SbgVersion structure based on an uint32_t that stores the 'compiled' version information. + * + * \param[in] encodedVersion The version information stored within a uint32_t. + * \param[out] pVersionInfo Pointer on an allocated SbgVersion structure to fill. + */ +SBG_COMMON_LIB_API void sbgVersionDecode(uint32_t encodedVersion, SbgVersion *pVersionInfo); + +/*! + * Construct a uint32_t containing a version information based on a SbgVersion structure. + * + * \param[in] pVersionInfo Pointer on a read only version structure to encode. + * \return The encoded version information on an uint32_t or 0 if an error has occurred. + */ +SBG_COMMON_LIB_API uint32_t sbgVersionEncode(const SbgVersion *pVersionInfo); + +//----------------------------------------------------------------------// +//- Version parameters methods -// +//----------------------------------------------------------------------// + +/*! + * Returns true if this encoded version number is using a software scheme. + * + * \param[in] encodedVersion The encoded version number to test stored in a uint32_t. + * \return true if the version number is using a software scheme. + */ +SBG_INLINE bool sbgVersionIsUsingSoftwareScheme(uint32_t encodedVersion) +{ + SbgVersion decodedVersion; + + // + // Decode the version first + // + sbgVersionDecode(encodedVersion, &decodedVersion); + + // + // Returns if the version is using a software scheme + // + return decodedVersion.softwareScheme; +} + +//----------------------------------------------------------------------// +//- Version comparaison methods -// +//----------------------------------------------------------------------// + +/*! + * Compare two version information structures and return if the version A is greater, less or equal than the version B. + * + * The computation is roughly result = version A - version B + * We can define how far we will check if the version A is greater than the version B. + * For example, we can only check the major or major and minor fields. + * + * \param[in] pVersionA The first version to compare. + * \param[in] pVersionB The second version to compare. + * \param[in] threshold The comparaison threshold to know if we are checking the major, minor, revision, build, ... + * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the threshold). + */ +SBG_COMMON_LIB_API int32_t sbgVersionCompare(const SbgVersion *pVersionA, const SbgVersion *pVersionB, SbgVersionCmpThreshold threshold); + +/*! + * Compare two encoded versions and return if the version A is greater, less or equal than the version B. + * + * The computation is roughly result = version A - version B + * We can define how far we will check if the version A is greater than the version B. + * For example, we can only check the major or major and minor fields. + * + * \param[in] versionA The first compiled version to compare. + * \param[in] versionB The second compiled version to compare. + * \param[in] threshold The comparaison threshold to know if we are checking the major, minor, revision, build, ... + * \return A positive value if the version A is greater than B, a negative one if version A is less than B and 0 if the two versions are the same (according to the threshold). + */ +SBG_COMMON_LIB_API int32_t sbgVersionCompareEncoded(uint32_t versionA, uint32_t versionB, SbgVersionCmpThreshold threshold); + +/*! + * Check if the provided version is between the provided version interval. + * All versions should have the same scheme. + * + * \param[in] pLowerVersion The lower version bound of the interval. + * \param[in] pHigherVersion The hiver version bound of the interval. + * \param[in] pVersion The version to check. + * \return A negative value if the version is stricly below the lower version bound + * Zero if the version if equal or greater than lower version and equal or smaller than higer version + * A positive value if the version is strictly above the higher version bound + */ +SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRange(const SbgVersion *pLowerVersion, const SbgVersion *pHigherVersion, const SbgVersion *pVersion); + +/*! + * Check if the provided encoded version is between the provided version interval. + * All versions should have the same scheme. + * + * \param[in] lowerVersion The lower version bound of the interval. + * \param[in] higherVersion The hiver version bound of the interval. + * \param[in] version The version to check. + * \return A negative value if the version is stricly below the lower version bound + * Zero if the version if equal or greater than lower version and equal or smaller than higer version + * A positive value if the version is strictly above the higher version bound + */ +SBG_COMMON_LIB_API int32_t sbgVersionIsWithinRangeEncoded(uint32_t lowerVersion, uint32_t higherVersion, uint32_t version); + + +//----------------------------------------------------------------------// +//- Version to string methods -// +//----------------------------------------------------------------------// + +/*! + * Convert a version information to a human readable string. + * + * \param[in] pVersionInfo Pointer on a read only version structure to convert to a human readable string. + * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. + * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. + * \return SBG_NO_ERROR if the version information has been converted to a string. + * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgVersionToString(const SbgVersion *pVersionInfo, char *pBuffer, size_t sizeOfBuffer); + +/*! + * Convert an encoded version number to a human readable string. + * + * \param[in] version Encoded version value to to convert to a human readable string. + * \param[out] pBuffer Buffer to store the version as a human readable null terminated string. + * \param[in] sizeOfBuffer Maximum buffer size including the null terminated char. + * \return SBG_NO_ERROR if the version information has been converted to a string. + * SBG_BUFFER_OVERFLOW is the buffer isn't big enough to store the converted version string. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, size_t sizeOfBuffer); + +//----------------------------------------------------------------------// +//- String to version methods -// +//----------------------------------------------------------------------// + +/*! + * Convert a human readable string to a version structure. + * + * \param[in] pVersionStr The string containing the version to convert. + * \param[out] pVersionInfo Pointer to a version structure to store the parsed version info. + * \return SBG_NO_ERROR if the version information has been converted from a string. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromString(const char *pVersionStr, SbgVersion *pVersionInfo); + +/*! + * Convert an encoded version number to a human readable string. + * + * \param[in] pVersionStr The string containing the version to convert. + * \param[out] pVersion Returned encoded version value parsed from the string. + * \return SBG_NO_ERROR if the version information has been converted from a string. + */ +SBG_COMMON_LIB_API SbgErrorCode sbgVersionFromStringEncoded(const char *pVersionStr, uint32_t *pVersion); + +//----------------------------------------------------------------------// +//- Legacy version system methods -// +//----------------------------------------------------------------------// + +/*! + * Define all these methods as deprecated. + */ +SBG_DEPRECATED(SBG_INLINE uint32_t SBG_VERSION(uint8_t major, uint8_t minor, uint8_t rev, uint8_t build)); +SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_MAJOR(uint32_t encodedVersion)); +SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_MINOR(uint32_t encodedVersion)); +SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_REV(uint32_t encodedVersion)); +SBG_DEPRECATED(SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion)); + +/*! + * DEPRECATED macro, please update your code + * + * Compile a version number using the basic scheme based on major, minor, revision and build information + * + * \param[in] major The major version between 0 to 127. + * \param[in] minor The minor version between 0 to 255. + * \param[in] rev The revision version between 0 to 255. + * \param[in] build The build number between 0 to 255. + * \return The encoded version number. + */ +SBG_INLINE uint32_t SBG_VERSION(uint8_t major, uint8_t minor, uint8_t rev, uint8_t build) +{ + // + // The SBG_VERSION macro is the basic version scheme. + // + return SBG_VERSION_BASIC(major, minor, rev, build); +} + +/*! + * DEPRECATED method, please update your code. + * + * Extract a basic version scheme using legacy methods. + * + * \param[in] encodedVersion The encoded version to extract the major version. + * \return The major version. + */ +SBG_INLINE uint8_t SBG_VERSION_GET_MAJOR(uint32_t encodedVersion) +{ + SbgVersion versionInfo; + + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); + + // + // Return the major version + // + return versionInfo.major; +} + +/*! + * DEPRECATED method, please update your code. + * + * Extract a basic version scheme using legacy methods. + * + * \param[in] encodedVersion The encoded version to extract the major version. + * \return The major version. + */ +SBG_INLINE uint8_t SBG_VERSION_GET_MINOR(uint32_t encodedVersion) +{ + SbgVersion versionInfo; + + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); + + // + // Return the major version + // + return versionInfo.minor; +} + +/*! + * DEPRECATED method, please update your code. + * + * Extract a basic version scheme using legacy methods. + * + * \param[in] encodedVersion The encoded version to extract the major version. + * \return The major version. + */ +SBG_INLINE uint8_t SBG_VERSION_GET_REV(uint32_t encodedVersion) +{ + SbgVersion versionInfo; + + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); + + // + // Return the major version + // + return versionInfo.rev; +} + +/*! + * DEPRECATED method, please update your code. + * + * Extract a basic version scheme using legacy methods + * + * \param[in] encodedVersion The encoded version to extract the major version. + * \return The major version. + */ +SBG_INLINE uint8_t SBG_VERSION_GET_BUILD(uint32_t encodedVersion) +{ + SbgVersion versionInfo; + + // + // Decode the version information + // + sbgVersionDecode(encodedVersion, &versionInfo); + + // + // Return the major version + // + return (uint8_t)versionInfo.build; +} + + +#ifdef __cplusplus +} +#endif + +#endif // SBG_VERSION_H diff --git a/crates/sbg-rs/sbgECom/doc/migrations.md b/crates/sbg-rs/sbgECom/doc/migrations.md new file mode 100644 index 0000000..09b1050 --- /dev/null +++ b/crates/sbg-rs/sbgECom/doc/migrations.md @@ -0,0 +1,43 @@ +# Migrations +These pages help you easily migrate your code from previous sbgECom major versions. + +## From sbgECom v1.x +The sbgECom version 2.x change the C API even if the low level sbgECom protocol API remains backward compatible. +In otherwords, a C code written with sbgECom version 1.x will not compile directly with sbgECom versions 2.x and higher. +But your old C code using sbgECom versions 1.x will still be able to correctly setup and configure your ELLIPSE product. + +### GNSS module +The _SbgEComGnssModelsStdIds_ enum has been simplified and now ELLIPSE-N and ELLIPSE-D should only use _SBG_ECOM_GNSS_MODEL_INTERNAL_ to select the internal GNSS receiver. +The only exception is for ELLIPSE-N hardware revision 1 & 2 as the ublox Max M8 has two modes of operations, either GPS+GLONASS or GPS+BeiDou. +The default GPS+GLONASS mode is selected with the _SBG_ECOM_GNSS_MODEL_INTERNAL_ model and the GPS+BeiDou one with _SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU_. +The _SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU_ model couldn't be used with ELLIPSE-N with hardware revision 3.x or ELLIPSE-D as the GNSS receiver is tracking GPS+GLONASS+BeiDou+Galileo concurrently. + +The table below helps you update your enums to sbgECom v2 correctly: +| sbgECom 1.x | sbgECom 2.x | Remarks | +| --------------------------------------- | --------------------------------------- | ------------------------------------------------------------------------------ | +| SBG_ECOM_GNSS_MODEL_UBLOX_GPS_GLONASS | SBG_ECOM_GNSS_MODEL_INTERNAL | To use for ELLIPSE-N and ELLIPSE-D | +| SBG_ECOM_GNSS_MODEL_NMEA | SBG_ECOM_GNSS_MODEL_NMEA | ELLIPSE-E, external NMEA receiver (listen only) | +| SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU | SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU | Can only be selected on ELLIPSE-N revision 1 & 2 | +| SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL | SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL | ELLIPSE-E, external ublox receiver (listen only) | +| SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_DYNAMICS | SBG_ECOM_GNSS_MODEL_INTERNAL | For high dynamics applications, please selected the appropriate motion profile | +| SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL | SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL | ELLIPSE-E, external Novatel receiver (listen only) | +| SBG_ECOM_GNSS_MODEL_ELLIPSE_D_INTERNAL | SBG_ECOM_GNSS_MODEL_INTERNAL | Legacy ELLIPSE-D hardware 1 & 2, don't use it anymore | +| SBG_ECOM_GNSS_MODEL_UBLOX_HIGH_SPEED | SBG_ECOM_GNSS_MODEL_INTERNAL | For high speed applications, please selected the appropriate motion profile | +| SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL | SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL | ELLIPSE-E, external Septentrio receiver (listen only) | +| SBG_ECOM_GNSS_MODEL_UBLOX_LOW_SPEED | SBG_ECOM_GNSS_MODEL_INTERNAL | For low dynamics applications, please selected the appropriate motion profile | + +Please also update your code according to the following recommendations and modifications: +- `sbgEComCmdGnss1GetModelInfo` method has been replaced by `sbgEComCmdGnss1GetModelId` with a simpler interface as the version field is no more used in ELLIPSE firmware. +- `sbgEComCmdGnss1GetLeverArmAlignment` and `sbgEComCmdGnss1SetLeverArmAlignment` methods have been suppressed by `sbgEComCmdGnss1InstallationGet` and `sbgEComCmdGnss1InstallationSet` methods. + +### Magnetometers module + +The method `sbgEComCmdMagGetModelInfo` has been replaced by `sbgEComCmdMagGetModelId` with a simpler interface as the version field is no more used in ELLIPSE firmware. + +### Sensor module + +The method `sbgEComCmdSensorGetMotionProfileInfo` has been replaced by `sbgEComCmdSensorGetMotionProfileId` with a simpler interface as the version field is no more used in ELLIPSE firmware. + +### Legacy IG-500 sbgCom + +sbgECom version 2.x drop the legacy IG-500 support so the methods `sbgEComCmdOutputGetLegacyConf` and `sbgEComCmdOutputSetLegacyConf` are removed. Please update your code to use sbgECom protocol instead. diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c new file mode 100644 index 0000000..25d3543 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.c @@ -0,0 +1,66 @@ +#include "sbgEComBinaryLogAirData.h" + +//----------------------------------------------------------------------// +//- Operations -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); + + // + // The true airspeed fields have been added in version 2.0 + // + if (sbgStreamBufferGetSpace(pInputStream) > 0) + { + pOutputData->pressureDiff = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->trueAirspeed = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->airTemperature = sbgStreamBufferReadFloatLE(pInputStream); + } + else + { + pOutputData->pressureDiff = 0.0f; + pOutputData->trueAirspeed = 0.0f; + pOutputData->airTemperature = 0.0f; + } + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureDiff); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->trueAirspeed); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->airTemperature); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h new file mode 100644 index 0000000..8909378 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAirData.h @@ -0,0 +1,105 @@ +/*! + * \file sbgEComBinaryLogAirData.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2019 + * + * \brief Parse received air data measurement logs such as barometer data. + * + * Air Data logs are used to inject / return barometric altitude + * as well as true air speed. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_AIR_DATA_H +#define SBG_ECOM_BINARY_LOG_AIR_DATA_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log Air Data status definitions -// +//----------------------------------------------------------------------// + +/*! + * Air Data sensor status mask definitions + */ +#define SBG_ECOM_AIR_DATA_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the time stamp field represents a delay instead of an absolute time stamp. */ +#define SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID (0x0001u << 1) /*!< Set to 1 if the pressure field is filled and valid. */ +#define SBG_ECOM_AIR_DATA_ALTITUDE_VALID (0x0001u << 2) /*!< Set to 1 if the barometric altitude field is filled and valid. */ +#define SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID (0x0001u << 3) /*!< Set to 1 if the differential pressure field is filled and valid. */ +#define SBG_ECOM_AIR_DATA_AIRPSEED_VALID (0x0001u << 4) /*!< Set to 1 if the true airspeed field is filled and valid. */ +#define SBG_ECOM_AIR_DATA_TEMPERATURE_VALID (0x0001u << 5) /*!< Set to 1 if the output air temperature field is filled and valid. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for AirData. + */ +typedef struct _SbgLogAirData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up OR measurement delay in us. */ + uint16_t status; /*!< Airdata sensor status bitmask. */ + float pressureAbs; /*!< Raw absolute pressure measured by the barometer sensor in Pascals. */ + float altitude; /*!< Altitude computed from barometric altimeter in meters and positive upward. */ + float pressureDiff; /*!< Raw differential pressure measured by the pitot tube in Pascal. */ + float trueAirspeed; /*!< True airspeed measured by a pitot tube in m.s^-1 and positive forward. */ + float airTemperature; /*!< Outside air temperature in °C that could be used to compute true airspeed from differential pressure. */ +} SbgLogAirData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_AIR_DATA message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseAirData(SbgStreamBuffer *pInputStream, SbgLogAirData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_AIR_DATA message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteAirData(SbgStreamBuffer *pOutputStream, const SbgLogAirData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_AIR_DATA_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAutomotiveData.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAutomotiveData.h new file mode 100644 index 0000000..df57e48 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogAutomotiveData.h @@ -0,0 +1,74 @@ +/*! + * \file sbgEComBinaryLogAutomotiveData.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 22 April 2020 + * + * \brief Parse dedicated automotive measurements logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H +#define SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Automotive data status mask definitions + */ +#define SBG_ECOM_AUTO_DATA_TRACK_VALID (0x1u << 0) /*!< Set to 1 if the track angle is valid. */ +#define SBG_ECOM_AUTO_DATA_SLIP_VALID (0x1u << 1) /*!< Set to 1 if the slip angle is valid. */ +#define SBG_ECOM_AUTO_DATA_CURVATURE_VALID (0x1u << 2) /*!< Set to 1 if the curvature radius is valid. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for automotive data. + */ +typedef struct _SbgLogAutoData +{ + uint8_t status; /*!< Status bit mask. */ + float trackAngle; /*!< Track angle, in rad. */ + float slipAngle; /*!< Slip angle, in rad. */ + float curvatureRadius; /*!< Curvature radius, in m, always positive. */ +} SbgLogAutoData; + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_AUTOMOTIVE_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c new file mode 100644 index 0000000..d87d268 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.c @@ -0,0 +1,45 @@ +#include "sbgEComBinaryLogDepth.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->pressureAbs = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->altitude = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pressureAbs); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitude); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h new file mode 100644 index 0000000..df142e3 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDepth.h @@ -0,0 +1,98 @@ +/*! + * \file sbgEComBinaryLogDepth.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2019 + * + * \brief Parse received subsea depth measurement logs. + * + * Depth sensor are used for subsea navigation to improve height. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_DEPTH_H +#define SBG_ECOM_BINARY_LOG_DEPTH_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log Air Data status definitions -// +//----------------------------------------------------------------------// + +/*! + * Air Data sensor status mask definitions + */ +#define SBG_ECOM_DEPTH_TIME_IS_DELAY (0x0001u << 0) /*!< Set to 1 if the time stamp field represents a delay instead of an absolute time stamp. */ +#define SBG_ECOM_DEPTH_PRESSURE_ABS_VALID (0x0001u << 1) /*!< Set to 1 if the pressure field is filled and valid. */ +#define SBG_ECOM_DEPTH_ALTITUDE_VALID (0x0001u << 2) /*!< Set to 1 if the depth altitude field is filled and valid. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for Depth sensor measurement (subsea). + */ +typedef struct _SbgLogDepth +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up OR measurement delay in us. */ + uint16_t status; /*!< Airdata sensor status bitmask. */ + float pressureAbs; /*!< Raw absolute pressure measured by the depth sensor in Pascals. */ + float altitude; /*!< Altitude computed from depth sensor in meters and positive upward. */ +} SbgLogDepth; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_DEPTH message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseDepth(SbgStreamBuffer *pInputStream, SbgLogDepth *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_DEPTH message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteDepth(SbgStreamBuffer *pOutputStream, const SbgLogDepth *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_DEPTH_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c new file mode 100644 index 0000000..dfcf0c3 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.c @@ -0,0 +1,49 @@ +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComBinaryLogDiag.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + pOutputData->timestamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->type = (SbgDebugLogType)sbgStreamBufferReadUint8(pInputStream); + pOutputData->errorCode = (SbgErrorCode)sbgStreamBufferReadUint8(pInputStream); + + sbgStreamBufferReadBuffer(pInputStream, pOutputData->string, sbgStreamBufferGetSpace(pInputStream)); + pOutputData->string[sizeof(pOutputData->string) - 1] = '\0'; + + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData) +{ + size_t length; + + assert(pOutputStream); + assert(pInputData); + + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timestamp); + sbgStreamBufferWriteUint8(pOutputStream, pInputData->type); + sbgStreamBufferWriteUint8(pOutputStream, pInputData->errorCode); + + length = strlen(pInputData->string); + + if (length >= sizeof(pInputData->string)) + { + length = sizeof(pInputData->string) - 1; + } + + sbgStreamBufferWriteBuffer(pOutputStream, pInputData->string, length); + sbgStreamBufferWriteUint8(pOutputStream, 0); + + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h new file mode 100644 index 0000000..4c8dc07 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDiag.h @@ -0,0 +1,98 @@ +/*! + * \file sbgEComBinaryLogDiag.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 June 2019 + * + * \brief Parse diagnostic logs emitted by the device. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_DIAG_H +#define SBG_ECOM_BINARY_LOG_DIAG_H + +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Maximum size of the log string, in bytes. + */ +#define SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE (SBG_ECOM_MAX_PAYLOAD_SIZE - 6) + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Diagnostic log structure. + */ +typedef struct _SbgLogDiagData +{ + uint32_t timestamp; /*!< Timestamp, in microseconds. */ + SbgDebugLogType type; /*!< Log type. */ + SbgErrorCode errorCode; /*!< Error code. */ + char string[SBG_ECOM_LOG_DIAG_MAX_STRING_SIZE]; /*!< Log string, null-terminated. */ +} SbgLogDiagData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for SBG_ECOM_LOG_DIAG messages and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseDiagData(SbgStreamBuffer *pInputStream, SbgLogDiagData *pOutputData); + + +/*! + * Write data for SBG_ECOM_LOG_DIAG messages to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteDiagData(SbgStreamBuffer *pOutputStream, const SbgLogDiagData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_DIAG_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c new file mode 100644 index 0000000..0bdb392 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.c @@ -0,0 +1,55 @@ +#include "sbgEComBinaryLogDvl.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->velocityQuality[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityQuality[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityQuality[2] = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityQuality[2]); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h new file mode 100644 index 0000000..fe7a8bc --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogDvl.h @@ -0,0 +1,96 @@ +/*! + * \file sbgEComBinaryLogDvl.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 05 June 2013 + * + * \brief Parse received DVL (Doppler Velocity Logger) measurement logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_DVL_H +#define SBG_ECOM_BINARY_LOG_DVL_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log DVL status definitions -// +//----------------------------------------------------------------------// + +/*! + * DVL status mask definitions + */ +#define SBG_ECOM_DVL_VELOCITY_VALID (0x0001u << 0) /*!< Set to 1 if the DVL equipment was able to measure a valid velocity. */ +#define SBG_ECOM_DVL_TIME_SYNC (0x0001u << 1) /*!< Set to 1 if the DVL data is correctly synchronized. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for DVL data. + */ +typedef struct _SbgLogDvlData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< DVL status bitmask. */ + float velocity[3]; /*!< X, Y, Z velocities in m.s^-1 expressed in the DVL instrument frame. */ + float velocityQuality[3]; /*!< X, Y, Z velocities quality indicators as provided by the DVL sensor and expressed in m.s^-1. + WARNING: This is typically just a residual information and not a real standard deviation. */ +} SbgLogDvlData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseDvlData(SbgStreamBuffer *pInputStream, SbgLogDvlData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK / SBG_ECOM_LOG_DVL_WATER_TRACK message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteDvlData(SbgStreamBuffer *pOutputStream, const SbgLogDvlData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_DVL_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c new file mode 100644 index 0000000..e95b9dd --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.c @@ -0,0 +1,183 @@ +#include "sbgEComBinaryLogEkf.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + + pOutputData->euler[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->euler[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->euler[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->euler[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); + + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + + pOutputData->quaternion[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->quaternion[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->quaternion[2] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->quaternion[3] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->eulerStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->eulerStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->eulerStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[2]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->quaternion[3]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->eulerStdDev[2]); + + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + + pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->velocityStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->position[0] = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->position[1] = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->position[2] = sbgStreamBufferReadDoubleLE(pInputStream); + + pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->positionStdDev[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->positionStdDev[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->positionStdDev[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityStdDev[2]); + + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[0]); + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[1]); + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->position[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->positionStdDev[2]); + + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h new file mode 100644 index 0000000..896cd4b --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEkf.h @@ -0,0 +1,227 @@ +/*! + * \file sbgEComBinaryLogEkf.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 + * + * \brief Parse EKF measurements such as attitude, position and velocity logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_EKF_H +#define SBG_ECOM_BINARY_LOG_EKF_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Solution status definitions -// +//----------------------------------------------------------------------// + +/*! + * Solution status mode definitions. + */ +#define SBG_ECOM_SOLUTION_MODE_SHIFT (0u) /*!< Shift used to extract the clock status part. */ +#define SBG_ECOM_SOLUTION_MODE_MASK (0x0000000Fu) /*!< Mask used to keep only the clock status part. */ + +/*! + * Solution bit masks definitions. + */ +#define SBG_ECOM_SOL_ATTITUDE_VALID (0x00000001u << 4) /*!< Set to 1 if attitude data is reliable (Roll/Pitch error < 0,5°). */ +#define SBG_ECOM_SOL_HEADING_VALID (0x00000001u << 5) /*!< Set to 1 if geading data is reliable (Heading error < 1°). */ +#define SBG_ECOM_SOL_VELOCITY_VALID (0x00000001u << 6) /*!< Set to 1 if velocity data is reliable (velocity error < 1.5 m/s). */ +#define SBG_ECOM_SOL_POSITION_VALID (0x00000001u << 7) /*!< Set to 1 if position data is reliable (Position error < 10m). */ +#define SBG_ECOM_SOL_VERT_REF_USED (0x00000001u << 8) /*!< Set to 1 if vertical reference is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_MAG_REF_USED (0x00000001u << 9) /*!< Set to 1 if magnetometer is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS1_VEL_USED (0x00000001u << 10) /*!< Set to 1 if GPS1 velocity is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS1_POS_USED (0x00000001u << 11) /*!< Set to 1 if GPS1 Position is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS1_HDT_USED (0x00000001u << 13) /*!< Set to 1 if GPS1 True Heading is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS2_VEL_USED (0x00000001u << 14) /*!< Set to 1 if GPS2 velocity is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS2_POS_USED (0x00000001u << 15) /*!< Set to 1 if GPS2 Position is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_GPS2_HDT_USED (0x00000001u << 17) /*!< Set to 1 if GPS2 True Heading is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_ODO_USED (0x00000001u << 18) /*!< Set to 1 if Odometer is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_DVL_BT_USED (0x00000001u << 19) /*!< Set to 1 if DVL Bottom Tracking is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_DVL_WT_USED (0x00000001u << 20) /*!< Set to 1 if DVL Water Tracking is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_USER_POS_USED (0x00000001u << 21) /*!< Set to 1 if user velocity is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_USER_VEL_USED (0x00000001u << 22) /*!< Set to 1 if user Position is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_USER_HEADING_USED (0x00000001u << 23) /*!< Set to 1 if user Course is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_USBL_USED (0x00000001u << 24) /*!< Set to 1 if USBL / LBL is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_AIR_DATA_USED (0x00000001u << 25) /*!< Set to 1 if AirData (altimeter and/or true airspeed) is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_ZUPT_USED (0x00000001u << 26) /*!< Set to 1 if a ZUPT is used in solution (data used and valid since 3s). */ +#define SBG_ECOM_SOL_ALIGN_VALID (0x00000001u << 27) /*!< Set to 1 if sensor alignment and calibration parameters are valid */ +#define SBG_ECOM_SOL_DEPTH_USED (0x00000001u << 28) /*!< Set to 1 if Depth sensor (for subsea navigation) is used in solution (data used and valid since 3s). */ + +/*! + * Solution filter mode enum. + */ +typedef enum _SbgEComSolutionMode +{ + SBG_ECOM_SOL_MODE_UNINITIALIZED = 0, /*!< The Kalman filter is not initialized and the returned data are all invalid. */ + SBG_ECOM_SOL_MODE_VERTICAL_GYRO = 1, /*!< The Kalman filter only rely on a vertical reference to compute roll and pitch angles. Heading and navigation data drift freely. */ + SBG_ECOM_SOL_MODE_AHRS = 2, /*!< A heading reference is available, the Kalman filter provides full orientation but navigation data drift freely. */ + SBG_ECOM_SOL_MODE_NAV_VELOCITY = 3, /*!< The Kalman filter computes orientation and velocity. Position is freely integrated from velocity estimation. */ + SBG_ECOM_SOL_MODE_NAV_POSITION = 4 /*!< Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided. */ +} SbgEComSolutionMode; + +//----------------------------------------------------------------------// +//- Solution status helpers methods -// +//----------------------------------------------------------------------// + +/*! + * Method used to read the solution mode from a solution status field. + * + * \param[in] status Status uint32_t value to extract the solution mode from it. + * \return The extracted solution mode. + */ +SBG_INLINE SbgEComSolutionMode sbgEComLogEkfGetSolutionMode(uint32_t status) +{ + return (SbgEComSolutionMode)((status >> SBG_ECOM_SOLUTION_MODE_SHIFT) & SBG_ECOM_SOLUTION_MODE_MASK); +} + +/*! + * Method used to write the solution status field. + * + * \param[in] solutionMode The solution mode to set. + * \param[in] masks Bit mask to set. + * \return The build solution status field. + */ +SBG_INLINE uint32_t sbgEComLogEkfBuildSolutionStatus(SbgEComSolutionMode solutionMode, uint32_t masks) +{ + // + // Create the combined status field + // + return ((((uint32_t)solutionMode)&SBG_ECOM_SOLUTION_MODE_MASK) << SBG_ECOM_SOLUTION_MODE_SHIFT) | masks; +} + + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * EKF computed orientation using euler angles. + */ +typedef struct _SbgLogEkfEulerData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + float euler[3]; /*!< Roll, Pitch and Yaw angles in rad. */ + float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ +} SbgLogEkfEulerData; + +/*! + * EFK computed orientation using quaternion. + */ +typedef struct _SbgLogEkfQuatData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + float quaternion[4]; /*!< Orientation quaternion stored in W, X, Y, Z form. */ + float eulerStdDev[3]; /*!< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad. */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ +} SbgLogEkfQuatData; + +/*! + * EFK computed navigation data. + */ +typedef struct _SbgLogEkfNavData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + float velocity[3]; /*!< North, East, Down velocity in m.s^-1. */ + float velocityStdDev[3]; /*!< North, East, Down velocity 1 sigma standard deviation in m.s^-1. */ + double position[3]; /*!< Latitude, Longitude in degrees positive North and East. + Altitude above Mean Sea Level in meters. */ + float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ + float positionStdDev[3]; /*!< Latitude, longitude and altitude 1 sigma standard deviation in meters. */ + uint32_t status; /*!< EKF solution status bitmask and enum. */ +} SbgLogEkfNavData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_EKF_EULER message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseEkfEulerData(SbgStreamBuffer *pInputStream, SbgLogEkfEulerData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_EKF_EULER message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteEkfEulerData(SbgStreamBuffer *pOutputStream, const SbgLogEkfEulerData *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_EKF_QUAT message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseEkfQuatData(SbgStreamBuffer *pInputStream, SbgLogEkfQuatData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_EKF_QUAT message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteEkfQuatData(SbgStreamBuffer *pOutputStream, const SbgLogEkfQuatData *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_EKF_NAV message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseEkfNavData(SbgStreamBuffer *pInputStream, SbgLogEkfNavData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_EKF_NAV message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteEkfNavData(SbgStreamBuffer *pOutputStream, const SbgLogEkfNavData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_EKF_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c new file mode 100644 index 0000000..82ecaff --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.c @@ -0,0 +1,47 @@ +#include "sbgEComBinaryLogEvent.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->timeOffset0 = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->timeOffset1 = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->timeOffset2 = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->timeOffset3 = sbgStreamBufferReadUint16LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset0); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset1); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset2); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->timeOffset3); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h new file mode 100644 index 0000000..c1c40e0 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogEvent.h @@ -0,0 +1,100 @@ +/*! + * \file sbgEComBinaryLogEvent.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 28 October 2013 + * + * \brief Parse event markers logs used to timestamp external signals. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_EVENT_H +#define SBG_ECOM_BINARY_LOG_EVENT_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log marker events definitions -// +//----------------------------------------------------------------------// + +/*! + * Log market events status mask definitions + */ +#define SBG_ECOM_EVENT_OVERFLOW (0x00000001u << 0) /*!< Set to 1 if we have received events at a higher rate than 1 kHz. */ +#define SBG_ECOM_EVENT_OFFSET_0_VALID (0x00000001u << 1) /*!< Set to 1 if at least two events have been received. */ +#define SBG_ECOM_EVENT_OFFSET_1_VALID (0x00000001u << 2) /*!< Set to 1 if at least three events have been received. */ +#define SBG_ECOM_EVENT_OFFSET_2_VALID (0x00000001u << 3) /*!< Set to 1 if at least four events have been received. */ +#define SBG_ECOM_EVENT_OFFSET_3_VALID (0x00000001u << 4) /*!< Set to 1 if at least five events have been received. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_EVENT_# message. + */ +typedef struct _SbgLogEvent +{ + uint32_t timeStamp; /*!< Measurement time since the sensor power up. */ + uint16_t status; /*!< Events status bitmask. */ + uint16_t timeOffset0; /*!< Time offset for the second received event. */ + uint16_t timeOffset1; /*!< Time offset for the third received event. */ + uint16_t timeOffset2; /*!< Time offset for the fourth received event. */ + uint16_t timeOffset3; /*!< Time offset for the fifth received event. */ +} SbgLogEvent; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_EVENT_# message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseEvent(SbgStreamBuffer *pInputStream, SbgLogEvent *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_EVENT_# message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteEvent(SbgStreamBuffer *pOutputStream, const SbgLogEvent *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_EVENT_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c new file mode 100644 index 0000000..d58e3a6 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.c @@ -0,0 +1,206 @@ +#include "sbgEComBinaryLogGps.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->velocity[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocity[2] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityAcc[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityAcc[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->velocityAcc[2] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->course = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->courseAcc = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity[2]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocityAcc[2]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->course); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->courseAcc); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->altitude = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->undulation = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->altitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Test if we have a additional information such as base station id (since version 1.4) + // + if (sbgStreamBufferGetSpace(pInputStream) >= 5) + { + // + // Read the additional information + // + pOutputData->numSvUsed = sbgStreamBufferReadUint8LE(pInputStream); + pOutputData->baseStationId = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->differentialAge = sbgStreamBufferReadUint16LE(pInputStream); + } + else + { + // + // Default the additional information + // + pOutputData->numSvUsed = 0; + pOutputData->baseStationId = 0xFFFF; + pOutputData->differentialAge = 0xFFFF; + } + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->status); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); + + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->altitude); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->undulation); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->altitudeAccuracy); + + // + // Write the additional information added in version 1.4 + // + sbgStreamBufferWriteUint8LE(pOutputStream, pInputData->numSvUsed); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->baseStationId); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->differentialAge); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->timeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->heading = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->headingAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->pitch = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->pitchAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + + // + // The baseline field have been added in version 2.0 + // + if (sbgStreamBufferGetSpace(pInputStream) > 0) + { + pOutputData->baseline = sbgStreamBufferReadFloatLE(pInputStream); + } + else + { + pOutputData->baseline = 0.0f; + } + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeOfWeek); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->heading); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->headingAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitch); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->pitchAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->baseline); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) +{ + return sbgEComBinaryLogParseRawData(pInputStream, pOutputData); +} + +SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) +{ + return sbgEComBinaryLogWriteRawData(pOutputStream, pInputData); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h new file mode 100644 index 0000000..1db301d --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogGps.h @@ -0,0 +1,433 @@ +/*! + * \file sbgEComBinaryLogGps.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2013 + * + * \brief Parse received GNSS logs such as Position, Velocity and True Heading. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_GPS_H +#define SBG_ECOM_BINARY_LOG_GPS_H + +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComBinaryLogRawData.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log GPS velocity const definitions -// +//----------------------------------------------------------------------// + +/*! + * Log GPS velocity status and type definitions. + */ +#define SBG_ECOM_GPS_VEL_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS velocity status part. */ +#define SBG_ECOM_GPS_VEL_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity status part. */ +#define SBG_ECOM_GPS_VEL_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS velocity type part. */ +#define SBG_ECOM_GPS_VEL_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS velocity type part. */ + +//----------------------------------------------------------------------// +//- Log GPS position const definitions -// +//----------------------------------------------------------------------// + +/*! + * GPS position status and type definitions. + */ +#define SBG_ECOM_GPS_POS_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS position status part. */ +#define SBG_ECOM_GPS_POS_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position status part. */ +#define SBG_ECOM_GPS_POS_TYPE_SHIFT (6u) /*!< Shift used to extract the GPS position type part. */ +#define SBG_ECOM_GPS_POS_TYPE_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS position type part. */ + +/*! + * GNSS signals definitions + */ +#define SBG_ECOM_GPS_POS_GPS_L1_USED (0x00000001u << 12) /*!< Set to 1 if GPS L1CA/L1P is used in solution. */ +#define SBG_ECOM_GPS_POS_GPS_L2_USED (0x00000001u << 13) /*!< Set to 1 if GPS L2P/L2C is used in solution. */ +#define SBG_ECOM_GPS_POS_GPS_L5_USED (0x00000001u << 14) /*!< Set to 1 if GPS L5 is used in solution. */ + +#define SBG_ECOM_GPS_POS_GLO_L1_USED (0x00000001u << 15) /*!< Set to 1 if GLONASS L1CA is used in solution. */ +#define SBG_ECOM_GPS_POS_GLO_L2_USED (0x00000001u << 16) /*!< Set to 1 if GLONASS L2C/L2P is used in solution. */ +#define SBG_ECOM_GPS_POS_GLO_L3_USED (0x00000001u << 17) /*!< Set to 1 if GLONASS L3 is used in solution. */ + +#define SBG_ECOM_GPS_POS_GAL_E1_USED (0x00000001u << 18) /*!< Set to 1 if Galileo E1 is used in solution. */ +#define SBG_ECOM_GPS_POS_GAL_E5A_USED (0x00000001u << 19) /*!< Set to 1 if Galileo E5a is used in solution. */ +#define SBG_ECOM_GPS_POS_GAL_E5B_USED (0x00000001u << 20) /*!< Set to 1 if Galileo E5b is used in solution. */ +#define SBG_ECOM_GPS_POS_GAL_E5ALT_USED (0x00000001u << 21) /*!< Set to 1 if Galileo E5 AltBoc is used in solution. */ +#define SBG_ECOM_GPS_POS_GAL_E6_USED (0x00000001u << 22) /*!< Set to 1 if Galileo E6 is used in solution. */ + +#define SBG_ECOM_GPS_POS_BDS_B1_USED (0x00000001u << 23) /*!< Set to 1 if BeiDou B1 is used in solution. */ +#define SBG_ECOM_GPS_POS_BDS_B2_USED (0x00000001u << 24) /*!< Set to 1 if BeiDou B2 is used in solution. */ +#define SBG_ECOM_GPS_POS_BDS_B3_USED (0x00000001u << 25) /*!< Set to 1 if BeiDou B3 is used in solution. */ + +#define SBG_ECOM_GPS_POS_QZSS_L1_USED (0x00000001u << 26) /*!< Set to 1 if QZSS L1CA is used in solution. */ +#define SBG_ECOM_GPS_POS_QZSS_L2_USED (0x00000001u << 27) /*!< Set to 1 if QZSS L2C is used in solution. */ +#define SBG_ECOM_GPS_POS_QZSS_L5_USED (0x00000001u << 28) /*!< Set to 1 if QZSS L5 is used in solution. */ + +//----------------------------------------------------------------------// +//- Log GPS HDT const definitions -// +//----------------------------------------------------------------------// + +/*! + * GPS HDT status definitions. + */ +#define SBG_ECOM_GPS_HDT_STATUS_SHIFT (0u) /*!< Shift used to extract the GPS HDT status part. */ +#define SBG_ECOM_GPS_HDT_STATUS_MASK (0x0000003Fu) /*!< Mask used to keep only the GPS HDT status part. */ + +/*! + * GPS HDT status bitmasks + */ +#define SBG_ECOM_GPS_HDT_BASELINE_VALID (0x0001 << 6) /*!< Set to 1 if the baseline length field is filled and valid. */ + +//----------------------------------------------------------------------// +//- Log GPS velocity enums definitions -// +//----------------------------------------------------------------------// + +/*! + * GPS velocity status definitions. + */ +typedef enum _SbgEComGpsVelStatus +{ + SBG_ECOM_VEL_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_VEL_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_VEL_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_VEL_LIMIT = 3 /*!< Velocity limit exceeded. */ +} SbgEComGpsVelStatus; + +/*! + * GPS velocity types definitions. + */ +typedef enum _SbgEComGpsVelType +{ + SBG_ECOM_VEL_NO_SOLUTION = 0, /*!< No valid velocity solution available. */ + SBG_ECOM_VEL_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ + SBG_ECOM_VEL_DOPPLER = 2, /*!< A Doppler velocity has been computed. */ + SBG_ECOM_VEL_DIFFERENTIAL = 3 /*!< A differential velocity has been computed between two positions. */ +} SbgEComGpsVelType; + +//----------------------------------------------------------------------// +//- Log GPS position enums definitions -// +//----------------------------------------------------------------------// + +/*! + * GPS position status definitions. + */ +typedef enum _SbgEComGpsPosStatus +{ + SBG_ECOM_POS_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_POS_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_POS_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_POS_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ +} SbgEComGpsPosStatus; + +/*! + * GPS position types definitions. + */ +typedef enum _SbgEComGpsPosType +{ + SBG_ECOM_POS_NO_SOLUTION = 0, /*!< No valid solution available. */ + SBG_ECOM_POS_UNKNOWN_TYPE = 1, /*!< An unknown solution type has been computed. */ + SBG_ECOM_POS_SINGLE = 2, /*!< Single point solution position. */ + SBG_ECOM_POS_PSRDIFF = 3, /*!< Standard Pseudorange Differential Solution (DGPS). */ + SBG_ECOM_POS_SBAS = 4, /*!< SBAS satellite used for differential corrections. */ + SBG_ECOM_POS_OMNISTAR = 5, /*!< Omnistar VBS Position (L1 sub-meter). */ + SBG_ECOM_POS_RTK_FLOAT = 6, /*!< Floating RTK ambiguity solution (20 cms RTK). */ + SBG_ECOM_POS_RTK_INT = 7, /*!< Integer RTK ambiguity solution (2 cms RTK). */ + SBG_ECOM_POS_PPP_FLOAT = 8, /*!< Precise Point Positioning with float ambiguities. */ + SBG_ECOM_POS_PPP_INT = 9, /*!< Precise Point Positioning with fixed ambiguities. */ + SBG_ECOM_POS_FIXED = 10 /*!< Fixed location solution position. */ +} SbgEComGpsPosType; + +//----------------------------------------------------------------------// +//- Log GPS HDT enums definitions -// +//----------------------------------------------------------------------// + +/*! + * GPS HDT status definitions. + */ +typedef enum _SbgEComGpsHdtStatus +{ + SBG_ECOM_HDT_SOL_COMPUTED = 0, /*!< A valid solution has been computed. */ + SBG_ECOM_HDT_INSUFFICIENT_OBS = 1, /*!< Not enough valid SV to compute a solution. */ + SBG_ECOM_HDT_INTERNAL_ERROR = 2, /*!< An internal error has occurred. */ + SBG_ECOM_HDT_HEIGHT_LIMIT = 3 /*!< The height limit has been exceeded. */ +} SbgEComGpsHdtStatus; + +//----------------------------------------------------------------------// +//- Helpers methods for velocity status access -// +//----------------------------------------------------------------------// + +/*! + * Method used to read GPS velocity status from a status field. + * + * \param[in] status Status uint32_t value to extract the velocity status from it. + * \return The extracted velocity status. + */ +SBG_INLINE SbgEComGpsVelStatus sbgEComLogGpsVelGetStatus(uint32_t status) +{ + return (SbgEComGpsVelStatus)((status >> SBG_ECOM_GPS_VEL_STATUS_SHIFT) & SBG_ECOM_GPS_VEL_STATUS_MASK); +} + +/*! + * Method used to read GPS velocity type from a status field. + * + * \param[in] status Status uint32_t value to extract the velocity type from it. + * \return The extracted velocity type. + */ +SBG_INLINE SbgEComGpsVelType sbgEComLogGpsVelGetType(uint32_t status) +{ + return (SbgEComGpsVelType)((status >> SBG_ECOM_GPS_VEL_TYPE_SHIFT) & SBG_ECOM_GPS_VEL_TYPE_MASK); +} + +/*! + * Method used to write the GPS velocity status to a status field. + * + * \param[in] status The velocity status to set. + * \param[in] type The velocity type to set. + * \return The build GpsVelData status field. + */ +SBG_INLINE uint32_t sbgEComLogGpsVelBuildStatus(SbgEComGpsVelStatus status, SbgEComGpsVelType type) +{ + // + // Create the combined status field + // + return ((((uint32_t)status)&SBG_ECOM_GPS_VEL_STATUS_MASK) << SBG_ECOM_GPS_VEL_STATUS_SHIFT) | + ((((uint32_t)type)&SBG_ECOM_GPS_VEL_TYPE_MASK) << SBG_ECOM_GPS_VEL_TYPE_SHIFT); +} + +//----------------------------------------------------------------------// +//- Helpers methods for position status access -// +//----------------------------------------------------------------------// + +/*! + * Method used to read GPS position status from a status field. + * + * \param[in] status Status uint32_t value to extract the position status from it. + * \return The extracted position status. + */ +SBG_INLINE SbgEComGpsPosStatus sbgEComLogGpsPosGetStatus(uint32_t status) +{ + return (SbgEComGpsPosStatus)((status >> SBG_ECOM_GPS_POS_STATUS_SHIFT) & SBG_ECOM_GPS_POS_STATUS_MASK); +} + +/*! + * Method used to read GPS position type from a status field. + * + * \param[in] status Status uint32_t value to extract the position type from it. + * \return The extracted position type. + */ +SBG_INLINE SbgEComGpsPosType sbgEComLogGpsPosGetType(uint32_t status) +{ + return (SbgEComGpsPosType)((status >> SBG_ECOM_GPS_POS_TYPE_SHIFT) & SBG_ECOM_GPS_POS_TYPE_MASK); +} + +/*! + * Method used to write the GPS position status to a status field. + * + * \param[in] status The position status to set. + * \param[in] type The position type to set. + * \param[in] masks Bit mask to set. + * \return The build GpsPosData status field. + */ +SBG_INLINE uint32_t sbgEComLogGpsPosBuildStatus(SbgEComGpsPosStatus status, SbgEComGpsPosType type, uint32_t masks) +{ + // + // Create the combined status field + // + return ((((uint32_t)status)&SBG_ECOM_GPS_POS_STATUS_MASK) << SBG_ECOM_GPS_POS_STATUS_SHIFT) | + ((((uint32_t)type)&SBG_ECOM_GPS_POS_TYPE_MASK) << SBG_ECOM_GPS_POS_TYPE_SHIFT) | masks; +} + +//----------------------------------------------------------------------// +//- Helpers methods for HDT status access -// +//----------------------------------------------------------------------// + +/*! + * Method used to read GPS HDT status from a status field. + * + * \param[in] status Status uint32_t value to extract the HDT status from it. + * \return The extracted position status. + */ +SBG_INLINE SbgEComGpsHdtStatus sbgEComLogGpsHdtGetStatus(uint32_t status) +{ + return (SbgEComGpsHdtStatus)((status >> SBG_ECOM_GPS_HDT_STATUS_SHIFT) & SBG_ECOM_GPS_HDT_STATUS_MASK); +} + +/*! + * Method used to write the GPS HDT status to a status field. + * + * \param[in] status The HDT status to set. + * \param[in] masks Bit mask to set. + * \return The build GpsPosData status field. + */ +SBG_INLINE uint32_t sbgEComLogGpsHdtBuildStatus(SbgEComGpsHdtStatus status, uint32_t masks) +{ + // + // Create the combined status field + // + return ((((uint32_t)status)&SBG_ECOM_GPS_HDT_STATUS_MASK) << SBG_ECOM_GPS_HDT_STATUS_SHIFT) | masks; +} + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_GPS#_VEL message. + */ +typedef struct _SbgLogGpsVel +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint32_t status; /*!< GPS velocity status, type and bitmask. */ + uint32_t timeOfWeek; /*!< GPS time of week in ms. */ + float velocity[3]; /*!< GPS North, East, Down velocity in m.s^-1. */ + float velocityAcc[3]; /*!< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1. */ + float course; /*!< Track ground course in degrees. */ + float courseAcc; /*!< Course accuracy in degrees. */ +} SbgLogGpsVel; + +/*! + * Structure that stores data for the SBG_ECOM_LOG_GPS#_POS message. + */ +typedef struct _SbgLogGpsPos +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint32_t status; /*!< GPS position status, type and bitmask. */ + uint32_t timeOfWeek; /*!< GPS time of week in ms. */ + double latitude; /*!< Latitude in degrees, positive north. */ + double longitude; /*!< Longitude in degrees, positive east. */ + double altitude; /*!< Altitude above Mean Sea Level in meters. */ + float undulation; /*!< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation). */ + float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ + float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ + float altitudeAccuracy; /*!< 1 sigma altitude accuracy in meters. */ + uint8_t numSvUsed; /*!< Number of space vehicles used to compute the solution (since version 1.4). */ + uint16_t baseStationId; /*!< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4). */ + uint16_t differentialAge; /*!< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4). */ +} SbgLogGpsPos; + +/*! + * Structure that stores data for the SBG_ECOM_LOG_GPS#_HDT message. + */ +typedef struct _SbgLogGpsHdt +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< GPS HDT status, type and bitmask. */ + uint32_t timeOfWeek; /*!< GPS time of week in ms. */ + float heading; /*!< GPS true heading in degrees. */ + float headingAccuracy; /*!< 1 sigma GPS true heading accuracy in degrees. */ + float pitch; /*!< GPS pitch angle measured from the master to the rover in degrees. */ + float pitchAccuracy; /*!< 1 signa GPS pitch angle accuarcy in degrees. */ + float baseline; /*!< The distance between the main and aux antenna in meters. */ +} SbgLogGpsHdt; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_GPS#_VEL message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseGpsVelData(SbgStreamBuffer *pInputStream, SbgLogGpsVel *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_GPS#_VEL message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteGpsVelData(SbgStreamBuffer *pOutputStream, const SbgLogGpsVel *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_GPS#_POS message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseGpsPosData(SbgStreamBuffer *pInputStream, SbgLogGpsPos *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_GPS#_POS message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteGpsPosData(SbgStreamBuffer *pOutputStream, const SbgLogGpsPos *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_GPS#_HDT message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseGpsHdtData(SbgStreamBuffer *pInputStream, SbgLogGpsHdt *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_GPS#_HDT message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteGpsHdtData(SbgStreamBuffer *pOutputStream, const SbgLogGpsHdt *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_GPS#_RAW message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseGpsRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_GPS#_RAW message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteGpsRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_GPS_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c new file mode 100644 index 0000000..279daf6 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.c @@ -0,0 +1,206 @@ +#include "sbgEComBinaryLogImu.h" + +//----------------------------------------------------------------------// +//- Public getters -// +//----------------------------------------------------------------------// + +float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx) +{ + assert(pImuShort); + assert(idx < 3); + + return pImuShort->deltaAngle[idx] / 67108864.0f; +} + +float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx) +{ + assert(pImuShort); + assert(idx < 3); + + return pImuShort->deltaVelocity[idx] / 1048576.0f; +} + +float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort) +{ + assert(pImuShort); + + return pImuShort->temperature / 256.0f; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->gyroscopes[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->gyroscopes[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->gyroscopes[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->temperature = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->deltaVelocity[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->deltaVelocity[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->deltaVelocity[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->deltaAngle[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->deltaAngle[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->deltaAngle[2] = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->gyroscopes[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->temperature); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaVelocity[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->deltaAngle[2]); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->deltaVelocity[0] = sbgStreamBufferReadInt32LE(pInputStream); + pOutputData->deltaVelocity[1] = sbgStreamBufferReadInt32LE(pInputStream); + pOutputData->deltaVelocity[2] = sbgStreamBufferReadInt32LE(pInputStream); + + pOutputData->deltaAngle[0] = sbgStreamBufferReadInt32LE(pInputStream); + pOutputData->deltaAngle[1] = sbgStreamBufferReadInt32LE(pInputStream); + pOutputData->deltaAngle[2] = sbgStreamBufferReadInt32LE(pInputStream); + + pOutputData->temperature = sbgStreamBufferReadInt16LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[0]); + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[1]); + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaVelocity[2]); + + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[0]); + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[1]); + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->deltaAngle[2]); + + sbgStreamBufferWriteInt16LE(pOutputStream, pInputData->temperature); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->accelerometers[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; + pOutputData->accelerometers[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; + pOutputData->accelerometers[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.01f; + + pOutputData->gyroscopes[0] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; + pOutputData->gyroscopes[1] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; + pOutputData->gyroscopes[2] = (float)sbgStreamBufferReadInt16LE(pInputStream) * 0.001f; + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[0] * 100.0f)); + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[1] * 100.0f)); + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->accelerometers[2] * 100.0f)); + + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[0] * 1000.0f)); + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[1] * 1000.0f)); + sbgStreamBufferWriteInt16LE(pOutputStream, (int16_t)(pInputData->gyroscopes[2] * 1000.0f)); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h new file mode 100644 index 0000000..4ad17a8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogImu.h @@ -0,0 +1,199 @@ +/*! + * \file sbgEComBinaryLogImu.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 + * + * \brief Parse IMU (Inertial Measurement Unit) measurement logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_IMU_H +#define SBG_ECOM_BINARY_LOG_IMU_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log Inertial Data definitions -// +//----------------------------------------------------------------------// + +/*! + * Log inertial data status mask definitions + */ +#define SBG_ECOM_IMU_COM_OK (0x00000001u << 0) /*!< Set to 1 if the communication with the IMU is ok. */ +#define SBG_ECOM_IMU_STATUS_BIT (0x00000001u << 1) /*!< Set to 1 if the IMU passes general Built in Tests (calibration, CPU, ...). */ + +#define SBG_ECOM_IMU_ACCEL_X_BIT (0x00000001u << 2) /*!< Set to 1 if the accelerometer X passes Built In Test. */ +#define SBG_ECOM_IMU_ACCEL_Y_BIT (0x00000001u << 3) /*!< Set to 1 if the accelerometer Y passes Built In Test. */ +#define SBG_ECOM_IMU_ACCEL_Z_BIT (0x00000001u << 4) /*!< Set to 1 if the accelerometer Z passes Built In Test. */ + +#define SBG_ECOM_IMU_GYRO_X_BIT (0x00000001u << 5) /*!< Set to 1 if the gyroscope X passes Built In Test. */ +#define SBG_ECOM_IMU_GYRO_Y_BIT (0x00000001u << 6) /*!< Set to 1 if the gyroscope Y passes Built In Test. */ +#define SBG_ECOM_IMU_GYRO_Z_BIT (0x00000001u << 7) /*!< Set to 1 if the gyroscope Z passes Built In Test. */ + +#define SBG_ECOM_IMU_ACCELS_IN_RANGE (0x00000001u << 8) /*!< Set to 1 if all accelerometers are within operating range. */ +#define SBG_ECOM_IMU_GYROS_IN_RANGE (0x00000001u << 9) /*!< Set to 1 if all gyroscopes are within operating range. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_IMU_DATA message. + */ +typedef struct _SbgLogImuData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< IMU status bitmask. */ + float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ + float gyroscopes[3]; /*!< X, Y, Z gyroscopes in rad.s^-1. */ + float temperature; /*!< Internal temperature in °C. */ + float deltaVelocity[3]; /*!< X, Y, Z delta velocity in m.s^-2. */ + float deltaAngle[3]; /*!< X, Y, Z delta angle in rad.s^-1. */ +} SbgLogImuData; + +/*! + * Structure that stores data for the SBG_ECOM_LOG_IMU_SHORT message. + * This message is only sent asynchronously and is the preferred log for post processing. + */ +typedef struct _SbgLogImuShort +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< IMU status bitmask. */ + int32_t deltaVelocity[3]; /*!< X, Y, Z delta velocity. Unit is 1048576 LSB for 1 m.s^-2. */ + int32_t deltaAngle[3]; /*!< X, Y, Z delta angle. Unit is 67108864 LSB for 1 rad.s^-1. */ + int16_t temperature; /*!< IMU average temperature. Unit is 256 LSB for 1°C. */ +} SbgLogImuShort; + +/*! + * Structure that stores the data for SBG_ECOM_LOG_FAST_IMU_DATA message + */ +typedef struct _SbgLogFastImuData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< IMU status bitmask. */ + float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ + float gyroscopes[3]; /*!< X, Y, Z gyroscopes in rad.s^-1. */ +} SbgLogFastImuData; + +//----------------------------------------------------------------------// +//- Public getters -// +//----------------------------------------------------------------------// + +/*! + * Return from an IMU Short log, the X, Y or Z delta angle value in rad.s^-1 + * + * \param[in] pImuShort Input IMU short message instance. + * \param[in] idx The component to return from 0 to 2. + * \return The delta angle value converted in rad.s^-1. + */ +float sbgLogImuShortGetDeltaAngle(const SbgLogImuShort *pImuShort, size_t idx); + +/*! + * Return from an IMU Short log, the X, Y or Z delta velocity value in m.s^-2 + * + * \param[in] pImuShort Input IMU short message instance. + * \param[in] idx The component to return from 0 to 2. + * \return The delta velocity value converted in m.s^-2. + */ +float sbgLogImuShortGetDeltaVelocity(const SbgLogImuShort *pImuShort, size_t idx); + +/*! + * Return from an IMU Short log, the temperature in °C + * + * \param[in] pImuShort Input IMU short message instance. + * \return The converted temperature in °C + */ +float sbgLogImuShortGetTemperature(const SbgLogImuShort *pImuShort); + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_IMU_DATA message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseImuData(SbgStreamBuffer *pInputStream, SbgLogImuData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_IMU_DATA message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteImuData(SbgStreamBuffer *pOutputStream, const SbgLogImuData *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_IMU_SHORT message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseImuShort(SbgStreamBuffer *pInputStream, SbgLogImuShort *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_IMU_SHORT message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteImuShort(SbgStreamBuffer *pOutputStream, const SbgLogImuShort *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_FAST_IMU_DATA message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseFastImuData(SbgStreamBuffer *pInputStream, SbgLogFastImuData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_FAST_IMU_DATA message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteFastImuData(SbgStreamBuffer *pOutputStream, const SbgLogFastImuData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_IMU_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c new file mode 100644 index 0000000..efa30d5 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.c @@ -0,0 +1,89 @@ +#include "sbgEComBinaryLogMag.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->magnetometers[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->magnetometers[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->magnetometers[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->accelerometers[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->accelerometers[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->accelerometers[2] = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->magnetometers[2]); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->accelerometers[2]); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->reserved = sbgStreamBufferReadUint16LE(pInputStream); + + // + // Read the raw magnetic calibration data buffer + // + return sbgStreamBufferReadBuffer(pInputStream, pOutputData->magData, sizeof(pOutputData->magData)); +} + +SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved); + + // + // Write the raw magnetic calibration data buffer + // + return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->magData, sizeof(pInputData->magData)); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h new file mode 100644 index 0000000..4a52bc8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogMag.h @@ -0,0 +1,133 @@ +/*! + * \file sbgEComBinaryLogMag.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 12 March 2013 + * + * \brief Parse magnetic field measurements logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_MAG_H +#define SBG_ECOM_BINARY_LOG_MAG_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log magnetometers status definitions -// +//----------------------------------------------------------------------// + +/*! + * Log magnetometer data status mask definitions + */ +#define SBG_ECOM_MAG_MAG_X_BIT (0x00000001u << 0) /*!< Set to 1 if the magnetometer X passes Built In Test. */ +#define SBG_ECOM_MAG_MAG_Y_BIT (0x00000001u << 1) /*!< Set to 1 if the magnetometer Y passes Built In Test. */ +#define SBG_ECOM_MAG_MAG_Z_BIT (0x00000001u << 2) /*!< Set to 1 if the magnetometer Z passes Built In Test. */ + +#define SBG_ECOM_MAG_ACCEL_X_BIT (0x00000001u << 3) /*!< Set to 1 if the accelerometer X passes Built In Test. */ +#define SBG_ECOM_MAG_ACCEL_Y_BIT (0x00000001u << 4) /*!< Set to 1 if the accelerometer Y passes Built In Test. */ +#define SBG_ECOM_MAG_ACCEL_Z_BIT (0x00000001u << 5) /*!< Set to 1 if the accelerometer Z passes Built In Test. */ + +#define SBG_ECOM_MAG_MAGS_IN_RANGE (0x00000001u << 6) /*!< Set to 1 if all magnetometers are within operating range. */ +#define SBG_ECOM_MAG_ACCELS_IN_RANGE (0x00000001u << 7) /*!< Set to 1 if all accelerometers are within operating range. */ + +#define SBG_ECOM_MAG_CALIBRATION_OK (0x00000001u << 8) /*!< Set to 1 if the magnetometers seems to be calibrated. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_MAG message. + */ +typedef struct _SbgLogMag +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< Magnetometer status bitmask. */ + float magnetometers[3]; /*!< X, Y, Z magnetometer data in A.U. */ + float accelerometers[3]; /*!< X, Y, Z accelerometers in m.s^-2. */ +} SbgLogMag; + +/*! + * Structure that stores data for the SBG_ECOM_LOG_MAG_CALIB message. + */ +typedef struct _SbgLogMagCalib +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t reserved; /*!< Reserved for future use. */ + uint8_t magData[16]; /*!< Magnetometers calibration data. */ +} SbgLogMagCalib; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_MAG message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseMagData(SbgStreamBuffer *pInputStream, SbgLogMag *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_MAG message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteMagData(SbgStreamBuffer *pOutputStream, const SbgLogMag *pInputData); + +/*! + * Parse data for the SBG_ECOM_LOG_MAG_CALIB message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseMagCalibData(SbgStreamBuffer *pInputStream, SbgLogMagCalib *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_MAG_CALIB message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteMagCalibData(SbgStreamBuffer *pOutputStream, const SbgLogMagCalib *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_MAG_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c new file mode 100644 index 0000000..075337c --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.c @@ -0,0 +1,43 @@ +#include "sbgEComBinaryLogOdometer.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->velocity = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->velocity); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h new file mode 100644 index 0000000..a7b3f69 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogOdometer.h @@ -0,0 +1,94 @@ +/*! + * \file sbgEComBinaryLogOdometer.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 25 February 2013 + * + * \brief Parse recevied odometer/DMI velocity measurement logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_ODOMETER_H +#define SBG_ECOM_BINARY_LOG_ODOMETER_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log odometer status definitions -// +//----------------------------------------------------------------------// + +/*! + * Odometer / velocity status mask definitions. + */ +#define SBG_ECOM_ODO_REAL_MEAS (0x0001 << 0) /*!< Set to 1 if this log comes from a real pulse measurement or from a timeout. */ +#define SBG_ECOM_ODO_TIME_SYNC (0x0001 << 1) /*!< Set to 1 if the velocity information is correctly time synchronized. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for odometer data. + */ +typedef struct _SbgLogOdometerData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< Odometer velocity status bitmask. */ + float velocity; /*!< Velocity in m.s^-1 in the odometer direction. */ +} SbgLogOdometerData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_ODO_VEL message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseOdometerData(SbgStreamBuffer *pInputStream, SbgLogOdometerData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_ODO_VEL message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteOdometerData(SbgStreamBuffer *pOutputStream, const SbgLogOdometerData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_ODOMETER_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.c new file mode 100644 index 0000000..71ed37f --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.c @@ -0,0 +1,40 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComBinaryLogRawData.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + size_t payloadSize; + + assert(pInputStream); + assert(pOutputData); + + payloadSize = sbgStreamBufferGetSize(pInputStream); + + if (payloadSize <= SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE) + { + errorCode = sbgStreamBufferReadBuffer(pInputStream, pOutputData->rawBuffer, payloadSize); + pOutputData->bufferSize = payloadSize; + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + + return errorCode; +} + +SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + return sbgStreamBufferWriteBuffer(pOutputStream, pInputData->rawBuffer, pInputData->bufferSize); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.h new file mode 100644 index 0000000..57108c7 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRawData.h @@ -0,0 +1,89 @@ +/*! + * \file sbgEComBinaryLogRawData.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 16 November 2020 + * + * \brief Parse logs used to store a binary stream such as RAW GNSS data. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_RAW_DATA_H +#define SBG_ECOM_BINARY_LOG_RAW_DATA_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log raw Data const definitions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE (4086u) /*!< Maximum buffer size in bytes that can be stored in the raw data log. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores raw data message. + */ +typedef struct _SbgLogRawData +{ + uint8_t rawBuffer[SBG_ECOM_RAW_DATA_MAX_BUFFER_SIZE]; /*!< Buffer that contains raw data. */ + size_t bufferSize; /*!< Raw buffer size in bytes. */ +} SbgLogRawData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse raw data message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); + +/*! + * Write raw data message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_RAW_DATA_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.c new file mode 100644 index 0000000..fcfce45 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.c @@ -0,0 +1,19 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComBinaryLogRtcm.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseRtcmRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData) +{ + return sbgEComBinaryLogParseRawData(pInputStream, pOutputData); +} + +SbgErrorCode sbgEComBinaryLogWriteRtcmRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData) +{ + return sbgEComBinaryLogWriteRawData(pOutputStream, pInputData); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.h new file mode 100644 index 0000000..00e38bf --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogRtcm.h @@ -0,0 +1,73 @@ +/*! + * \file sbgEComBinaryLogRtcm.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 16 November 2020 + * + * \brief Parse RTCM data stream logs as received by the INS. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_RTCM_H +#define SBG_ECOM_BINARY_LOG_RTCM_H + +// sbgCommonLib headers +#include +#include + +// Local headers +#include "sbgEComBinaryLogRawData.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_RTCM_RAW message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseRtcmRawData(SbgStreamBuffer *pInputStream, SbgLogRawData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_RTCM_RAW message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteRtcmRawData(SbgStreamBuffer *pOutputStream, const SbgLogRawData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_RTCM_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.c new file mode 100644 index 0000000..503c0bc --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.c @@ -0,0 +1,865 @@ +/*! + * \file sbgEComBinaryLogSat.h + * \author SBG Systems + * \date 1 March 2022 + * + * \brief Handle binary satellite logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComBinaryLogSat.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET (0) /*!< Offset of the tracking status field, in bits. */ +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH (3) /*!< Width of the tracking status field, in bits. */ +#define SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_TRACKING_STATUS_WIDTH) - 1) /*!< Tracking status field mask. */ + +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET (3) /*!< Offset of the health status field, in bits. */ +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH (2) /*!< Width of the health status field, in bits. */ +#define SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_HEALTH_STATUS_WIDTH) - 1) /*!< Health status field mask. */ + +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET (5) /*!< Offset of the elevation status field, in bits. */ +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH (2) /*!< Width of the elevation status field, in bits. */ +#define SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK ((1u << SBG_ECOM_LOG_SAT_ELEVATION_STATUS_WIDTH) - 1) /*!< Elevation status field mask. */ + +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET (7) /*!< Offset of the constellation ID field, in bits. */ +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH (4) /*!< Width of the constellation ID field, in bits. */ +#define SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK ((1u << SBG_ECOM_LOG_SAT_CONSTELLATION_ID_WIDTH) - 1) /*!< Constellation ID field mask. */ + +#define SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID (1u << 5) /*!< Set if the SNR value is valid. */ + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Get a bit field from a set of flags. + * + * \param[in] flags Flags. + * \param[in] offset Field offset, in bits. + * \param[in] mask Field mask. + * \return Field value. + */ +#define sbgEComBinaryLogSatGetField(flags, offset, mask) (((flags) >> (offset)) & (mask)) + +/*! + * Set a bit field from a set of flags. + * + * \param[in/out] flags Flags. + * \param[in] value Field value. + * \param[in] offset Field offset, in bits. + * \param[in] mask Field mask. + */ +#define sbgEComBinaryLogSatSetField(flags, value, offset, mask) (flags) &= ~((mask) << (offset)); (flags) |= ((value) & (mask)) << (offset) + + + +/*! + * Check the value of a health status. + * + * \param[in] healthStatus Health status. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComBinaryLogSatCheckHealthStatus(uint8_t healthStatus) +{ + SbgErrorCode errorCode = SBG_INVALID_FRAME; + + switch (healthStatus) + { + case SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN: + case SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY: + case SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY: + errorCode = SBG_NO_ERROR; + break; + } + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "invalid health status: %" PRIu8, healthStatus); + } + + return errorCode; +} + +/*! + * Check the value of a tracking status. + * + * \param[in] trackingStatus Tracking status. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComBinaryLogSatCheckTrackingStatus(uint8_t trackingStatus) +{ + SbgErrorCode errorCode = SBG_INVALID_FRAME; + + switch (trackingStatus) + { + case SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN: + case SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING: + case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN: + case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED: + case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED: + case SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED: + errorCode = SBG_NO_ERROR; + break; + } + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "invalid tracking status: %" PRIu8, trackingStatus); + } + + return errorCode; +} + +/*! + * Returns tracking status string from enum value + * + * \param[in] trackingStatus Tracking status enum to convert + * \return Tracking status as a read only C string. + */ +static const char *sbgEComBinaryLogSatTrackingStatusToStr(SbgEComSatTrackingStatus trackingStatus) +{ + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN] = "unkown", + [SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING] = "searching", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN] = "tracking", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED] = "unused", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED] = "rejected", + [SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED] = "used", + }; + + if (trackingStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[trackingStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN]; + } +} + +/*! + * Returns health status string from enum value + * + * \param[in] trackingStatus Tracking status enum to convert + * \return Tracking status as a read only C string. + */ +static const char *sbgEComBinaryLogSatHealthStatusToStr(SbgEComSatHealthStatus healthStatus) +{ + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN] = "unkown", + [SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY] = "healthy", + [SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY] = "unhealthy", + }; + + if (healthStatus < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[healthStatus]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN]; + } +} + +/*! + * Parse signal data from a stream buffer. + * + * \param[in] pStreamBuffer Stream buffer. + * \param[out] pSignalData Signal data. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComBinaryLogParseSignalData(SbgStreamBuffer *pStreamBuffer, SbgLogSatSignalData *pSignalData) +{ + SbgErrorCode errorCode; + uint8_t signalId; + + assert(pSignalData); + + signalId = sbgStreamBufferReadUint8(pStreamBuffer); + pSignalData->flags = sbgStreamBufferReadUint8(pStreamBuffer); + pSignalData->snr = sbgStreamBufferReadUint8(pStreamBuffer); + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (sbgEComSignalIdIsValid(signalId)) + { + uint8_t healthStatus; + + pSignalData->id = signalId; + + healthStatus = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + + errorCode = sbgEComBinaryLogSatCheckHealthStatus(healthStatus); + + if (errorCode == SBG_NO_ERROR) + { + uint8_t trackingStatus; + + trackingStatus = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + errorCode = sbgEComBinaryLogSatCheckTrackingStatus(trackingStatus); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid signal ID: %" PRIu8, signalId); + } + } + + return errorCode; +} + +/*! + * Check the value of an elevation status. + * + * \param[in] elevationStatus Elevation status value. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComBinaryLogSatCheckElevationStatus(uint8_t elevationStatus) +{ + SbgErrorCode errorCode = SBG_INVALID_FRAME; + + switch (elevationStatus) + { + case SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN: + case SBG_ECOM_SAT_ELEVATION_STATUS_SETTING: + case SBG_ECOM_SAT_ELEVATION_STATUS_RISING: + errorCode = SBG_NO_ERROR; + break; + } + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "invalid elevation status: %" PRIu8, elevationStatus); + } + + return errorCode; +} + +/*! + * Parse satellite data from a stream buffer. + * + * \param[in] pStreamBuffer Stream buffer. + * \param[out] pSatGroupData Satellite group data. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComBinaryLogParseSatData(SbgStreamBuffer *pStreamBuffer, SbgLogSatData *pSatData) +{ + SbgErrorCode errorCode; + + assert(pSatData); + + pSatData->id = sbgStreamBufferReadUint8(pStreamBuffer); + pSatData->elevation = sbgStreamBufferReadInt8(pStreamBuffer); + pSatData->azimuth = sbgStreamBufferReadUint16(pStreamBuffer); + pSatData->flags = sbgStreamBufferReadUint16(pStreamBuffer); + pSatData->nrSignals = sbgStreamBufferReadUint8(pStreamBuffer); + pSatData->signalDataArraySize = pSatData->nrSignals; + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (pSatData->nrSignals <= SBG_ECOM_SAT_MAX_NR_SIGNALS) + { + pSatData->pSignalData = malloc(sizeof(*pSatData->pSignalData) * pSatData->signalDataArraySize); + + if (pSatData->pSignalData) + { + uint8_t constellationId; + + constellationId = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); + + if (sbgEComConstellationIdIsValid(constellationId)) + { + uint8_t elevationStatus; + + elevationStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); + + errorCode = sbgEComBinaryLogSatCheckElevationStatus(elevationStatus); + + if (errorCode == SBG_NO_ERROR) + { + uint8_t healthStatus; + + healthStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + + errorCode = sbgEComBinaryLogSatCheckHealthStatus(healthStatus); + + if (errorCode == SBG_NO_ERROR) + { + uint8_t trackingStatus; + + trackingStatus = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + errorCode = sbgEComBinaryLogSatCheckTrackingStatus(trackingStatus); + + if (errorCode == SBG_NO_ERROR) + { + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + errorCode = sbgEComBinaryLogParseSignalData(pStreamBuffer, &pSatData->pSignalData[i]); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + } + } + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid constellation id: %" PRIu8, constellationId); + } + + if (errorCode != SBG_NO_ERROR) + { + free(pSatData->pSignalData); + pSatData->pSignalData = NULL; + } + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate signal data array"); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid number of signals: %zu", pSatData->nrSignals); + } + } + + return errorCode; +} + +/*! + * Satellite data constructor. + * + * \param[in] pSatData Satellite data. + * \param[in] nrSignals Number of signals. + * \param[in] id Satellite ID. + * \param[in] elevation Elevation, in degrees. + * \param[in] azimuth Azimuth, in degrees. + * \param[in] constellationId Constellation ID. + * \param[in] elevationStatus Elevation status. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgLogSatDataConstruct(SbgLogSatData *pSatData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint16_t flags = 0; + + assert(pSatData); + assert(nrSignals <= SBG_ECOM_SAT_MAX_NR_SIGNALS); + + sbgEComBinaryLogSatSetField(flags, constellationId, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); + sbgEComBinaryLogSatSetField(flags, elevationStatus, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); + sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComBinaryLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + pSatData->id = id; + pSatData->elevation = elevation; + pSatData->azimuth = azimuth; + pSatData->flags = flags; + pSatData->signalDataArraySize = nrSignals; + pSatData->nrSignals = 0; + + pSatData->pSignalData = malloc(sizeof(*pSatData->pSignalData) * pSatData->signalDataArraySize); + + if (!pSatData->pSignalData) + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate signal data array"); + } + + return errorCode; +} + +/*! + * Satellite data destructor. + * + * \param[in] pSatData Satellite data. + */ +static void sbgLogSatDataDestroy(SbgLogSatData *pSatData) +{ + assert(pSatData); + + free(pSatData->pSignalData); + pSatData->pSignalData = NULL; +} + +/*! + * Select one of two health statuses according to their priority rules. + * + * \param[in] healthStatus1 First health status. + * \param[in] healthStatus2 Second health status. + */ +static SbgEComSatHealthStatus sbgEComBinaryLogSatSelectHealthStatus(SbgEComSatHealthStatus healthStatus1, SbgEComSatHealthStatus healthStatus2) +{ + SbgEComSatHealthStatus healthStatus; + + if (healthStatus2 > healthStatus1) + { + healthStatus = healthStatus2; + } + else + { + healthStatus = healthStatus1; + } + + return healthStatus; +} + +/*! + * Select one of two tracking statuses according to their priority rules. + * + * \param[in] trackingStatus1 First tracking status. + * \param[in] trackingStatus2 Second tracking status. + */ +static SbgEComSatTrackingStatus sbgEComBinaryLogSatSelectTrackingStatus(SbgEComSatTrackingStatus trackingStatus1, SbgEComSatTrackingStatus trackingStatus2) +{ + SbgEComSatTrackingStatus trackingStatus; + + if (trackingStatus2 > trackingStatus1) + { + trackingStatus = trackingStatus2; + } + else + { + trackingStatus = trackingStatus1; + } + + return trackingStatus; +} + +/*! + * Update the health and tracking statuses of satellite data. + * + * \param[in] pSatData Satellite data. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + */ +static void sbgLogSatDataUpdateStatus(SbgLogSatData *pSatData, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) +{ + uint16_t flags; + + assert(pSatData); + + flags = pSatData->flags; + + healthStatus = sbgEComBinaryLogSatSelectHealthStatus(sbgLogSatDataGetHealthStatus(pSatData), healthStatus); + trackingStatus = sbgEComBinaryLogSatSelectTrackingStatus(sbgLogSatDataGetTrackingStatus(pSatData), trackingStatus); + + sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComBinaryLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + pSatData->flags = flags; +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgLogSatGroupData *pSatGroupData) +{ + SbgErrorCode errorCode; + + assert(pSatGroupData); + assert(pStreamBuffer); + + pSatGroupData->timeStamp = sbgStreamBufferReadUint32LE(pStreamBuffer); + pSatGroupData->reserved = sbgStreamBufferReadUint32LE(pStreamBuffer); + pSatGroupData->nrSatellites = sbgStreamBufferReadUint8(pStreamBuffer); + pSatGroupData->satDataArraySize = pSatGroupData->nrSatellites; + + errorCode = sbgStreamBufferGetLastError(pStreamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (pSatGroupData->nrSatellites <= SBG_ECOM_SAT_MAX_NR_SATELLITES) + { + pSatGroupData->pSatData = malloc(sizeof(*pSatGroupData->pSatData) * pSatGroupData->satDataArraySize); + + if (pSatGroupData->pSatData) + { + for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) + { + errorCode = sbgEComBinaryLogParseSatData(pStreamBuffer, &pSatGroupData->pSatData[i]); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + } + + if (errorCode != SBG_NO_ERROR) + { + free(pSatGroupData->pSatData); + pSatGroupData->pSatData = NULL; + } + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate satellite data array"); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid number of satellites: %zu", pSatGroupData->nrSatellites); + } + } + + return errorCode; +} + +SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgLogSatGroupData *pSatGroupData) +{ + assert(pStreamBuffer); + assert(pSatGroupData); + assert(pSatGroupData->nrSatellites <= UINT8_MAX); + + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatGroupData->timeStamp); + sbgStreamBufferWriteUint32LE(pStreamBuffer, pSatGroupData->reserved); + sbgStreamBufferWriteUint8(pStreamBuffer, (uint8_t)pSatGroupData->nrSatellites); + + for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) + { + const SbgLogSatData *pSatData = &pSatGroupData->pSatData[i]; + + assert(pSatData->nrSignals <= UINT8_MAX); + + sbgStreamBufferWriteUint8(pStreamBuffer, pSatData->id); + sbgStreamBufferWriteInt8(pStreamBuffer, pSatData->elevation); + sbgStreamBufferWriteUint16(pStreamBuffer, pSatData->azimuth); + sbgStreamBufferWriteUint16(pStreamBuffer, pSatData->flags); + sbgStreamBufferWriteUint8(pStreamBuffer, (uint8_t)pSatData->nrSignals); + + for (size_t j = 0; j < pSatData->nrSignals; j++) + { + const SbgLogSatSignalData *pSignalData = &pSatData->pSignalData[j]; + + sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->id); + sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->flags); + sbgStreamBufferWriteUint8(pStreamBuffer, pSignalData->snr); + } + } + + return sbgStreamBufferGetLastError(pStreamBuffer); +} + +SbgErrorCode sbgLogSatGroupDataConstruct(SbgLogSatGroupData *pSatGroupData, size_t nrSatellites, uint32_t timeStamp) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pSatGroupData); + assert(nrSatellites <= SBG_ECOM_SAT_MAX_NR_SATELLITES); + + pSatGroupData->timeStamp = timeStamp; + pSatGroupData->reserved = 0; + pSatGroupData->nrSatellites = 0; + pSatGroupData->satDataArraySize = nrSatellites; + + pSatGroupData->pSatData = malloc(sizeof(*pSatGroupData->pSatData) * pSatGroupData->satDataArraySize); + + if (!pSatGroupData->pSatData) + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable to allocate satellite data array"); + } + + return SBG_NO_ERROR; +} + +void sbgLogSatGroupDataDestroy(SbgLogSatGroupData *pSatGroupData) +{ + assert(pSatGroupData); + + for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) + { + sbgLogSatDataDestroy(&pSatGroupData->pSatData[i]); + } + + free(pSatGroupData->pSatData); + pSatGroupData->pSatData = NULL; +} + +SbgLogSatData *sbgLogSatGroupDataAdd(SbgLogSatGroupData *pSatGroupData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus) +{ + SbgLogSatData *pSatData = NULL; + + assert(pSatGroupData); + + if (pSatGroupData->nrSatellites < pSatGroupData->satDataArraySize) + { + SbgErrorCode errorCode; + SbgLogSatData *pTmpSatData; + + pTmpSatData = &pSatGroupData->pSatData[pSatGroupData->nrSatellites]; + + errorCode = sbgLogSatDataConstruct(pTmpSatData, nrSignals, id, elevation, azimuth, constellationId, elevationStatus, healthStatus, trackingStatus); + + if (errorCode == SBG_NO_ERROR) + { + pSatData = pTmpSatData; + pSatGroupData->nrSatellites++; + } + } + + return pSatData; +} + +SbgLogSatData *sbgLogSatGroupGet(SbgLogSatGroupData *pSatGroupData, uint8_t id) +{ + SbgLogSatData *pSatData = NULL; + + assert(pSatGroupData); + + for (size_t i = 0; i < pSatGroupData->nrSatellites; i++) + { + if (pSatGroupData->pSatData[i].id == id) + { + pSatData = &pSatGroupData->pSatData[i]; + break; + } + } + + return pSatData; +} + +SbgEComConstellationId sbgLogSatDataGetConstellationId(const SbgLogSatData *pSatData) +{ + uint8_t value; + + assert(pSatData); + + value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_OFFSET, SBG_ECOM_LOG_SAT_CONSTELLATION_ID_MASK); + + return (SbgEComConstellationId)value; +} + +const char *sbgLogSatDataGetConstellationIdAsStr(const SbgLogSatData *pSatData) +{ + return sbgEComConstellationToStr(sbgLogSatDataGetConstellationId(pSatData)); +} + +SbgEComSatElevationStatus sbgLogSatDataGetElevationStatus(const SbgLogSatData *pSatData) +{ + uint8_t value; + + assert(pSatData); + + value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_OFFSET, SBG_ECOM_LOG_SAT_ELEVATION_STATUS_MASK); + + return (SbgEComSatElevationStatus)value; +} + +const char *sbgLogSatDataGetElevationStatusAsStr(const SbgLogSatData *pSatData) +{ + SbgEComSatElevationStatus enumIdx; + static const char *enumToStrLut[] = + { + [SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN] = "unkown", + [SBG_ECOM_SAT_ELEVATION_STATUS_SETTING] = "setting", + [SBG_ECOM_SAT_ELEVATION_STATUS_RISING] = "rising", + }; + + assert(pSatData); + + enumIdx = sbgLogSatDataGetElevationStatus(pSatData); + + if (enumIdx < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[enumIdx]; + } + else + { + return enumToStrLut[SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN]; + } +} + +SbgEComSatHealthStatus sbgLogSatDataGetHealthStatus(const SbgLogSatData *pSatData) +{ + uint8_t value; + + assert(pSatData); + + value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + + return (SbgEComSatHealthStatus)value; +} + +const char *sbgLogSatDataGetHealthStatusAsStr(const SbgLogSatData *pSatData) +{ + assert(pSatData); + + return sbgEComBinaryLogSatHealthStatusToStr(sbgLogSatDataGetHealthStatus(pSatData)); +} + +SbgEComSatTrackingStatus sbgLogSatDataGetTrackingStatus(const SbgLogSatData *pSatData) +{ + uint8_t value; + + assert(pSatData); + + value = sbgEComBinaryLogSatGetField(pSatData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + return (SbgEComSatTrackingStatus)value; +} + +const char *sbgLogSatDataGetTrackingStatusAsStr(const SbgLogSatData *pSatData) +{ + assert(pSatData); + + return sbgEComBinaryLogSatTrackingStatusToStr(sbgLogSatDataGetTrackingStatus(pSatData)); +} + +SbgLogSatSignalData *sbgLogSatDataAdd(SbgLogSatData *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr) +{ + SbgLogSatSignalData *pSignalData = NULL; + + assert(pSatData); + + if (pSatData->nrSignals < pSatData->signalDataArraySize) + { + uint8_t flags = 0; + + sbgEComBinaryLogSatSetField(flags, healthStatus, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + sbgEComBinaryLogSatSetField(flags, trackingStatus, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + if (snrValid) + { + flags |= SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID; + } + + pSignalData = &pSatData->pSignalData[pSatData->nrSignals]; + + pSignalData->id = id; + pSignalData->flags = flags; + pSignalData->snr = snr; + + pSatData->nrSignals++; + + sbgLogSatDataUpdateStatus(pSatData, healthStatus, trackingStatus); + } + + return pSignalData; +} + +SbgLogSatSignalData *sbgLogSatDataGet(SbgLogSatData *pSatData, SbgEComSignalId id) +{ + SbgLogSatSignalData *pSignalData = NULL; + + assert(pSatData); + + for (size_t i = 0; i < pSatData->nrSignals; i++) + { + if (pSatData->pSignalData[i].id == id) + { + pSignalData = &pSatData->pSignalData[i]; + break; + } + } + + return pSignalData; +} + +const char *sbgLogSatSignalDataGetSignalIdAsStr(const SbgLogSatSignalData *pSignalData) +{ + return sbgEComSignalToStr(pSignalData->id); +} + +bool sbgLogSatSignalDataSnrIsValid(const SbgLogSatSignalData *pSignalData) +{ + if (pSignalData->flags&SBG_ECOM_LOG_SAT_SIGNAL_SNR_VALID) + { + return true; + } + else + { + return false; + } +} + +SbgEComSatHealthStatus sbgLogSatSignalDataGetHealthStatus(const SbgLogSatSignalData *pSignalData) +{ + uint8_t value; + + assert(pSignalData); + + value = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_HEALTH_STATUS_OFFSET, SBG_ECOM_LOG_SAT_HEALTH_STATUS_MASK); + + return (SbgEComSatHealthStatus)value; +} + +const char *sbgLogSatSignalDataGetHealthStatusAsStr(const SbgLogSatSignalData *pSignalData) +{ + assert(pSignalData); + + return sbgEComBinaryLogSatHealthStatusToStr(sbgLogSatSignalDataGetHealthStatus(pSignalData)); +} + +SbgEComSatTrackingStatus sbgLogSatSignalDataGetTrackingStatus(const SbgLogSatSignalData *pSignalData) +{ + uint8_t value; + + assert(pSignalData); + + value = sbgEComBinaryLogSatGetField(pSignalData->flags, SBG_ECOM_LOG_SAT_TRACKING_STATUS_OFFSET, SBG_ECOM_LOG_SAT_TRACKING_STATUS_MASK); + + return (SbgEComSatTrackingStatus)value; +} + +const char *sbgLogSatSignalDataGetTrackingStatusAsStr(const SbgLogSatSignalData *pSignalData) +{ + assert(pSignalData); + + return sbgEComBinaryLogSatTrackingStatusToStr(sbgLogSatSignalDataGetTrackingStatus(pSignalData)); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.h new file mode 100644 index 0000000..bdb5f8e --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogSat.h @@ -0,0 +1,377 @@ +/*! + * \file sbgEComBinaryLogSat.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 1 March 2022 + * + * \brief Parse space vehicles in view information log. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_SAT_H +#define SBG_ECOM_BINARY_LOG_SAT_H + +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Maximum number of satellites in a satellite group. + */ +#define SBG_ECOM_SAT_MAX_NR_SATELLITES (64) + +/*! + * Maximum number of signals per satellite. + */ +#define SBG_ECOM_SAT_MAX_NR_SIGNALS (8) + +//----------------------------------------------------------------------// +//- Enumeration definitions -// +//----------------------------------------------------------------------// + +/*! + * Tracking status. + * + * The tracking status embeds the solution status when the latter is known. + * + * All values must be strictly lower than 8. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComSatTrackingStatus +{ + SBG_ECOM_SAT_TRACKING_STATUS_UNKNOWN = 0, /*!< Unknown tracking status such as no signal / idle. */ + SBG_ECOM_SAT_TRACKING_STATUS_SEARCHING = 1, /*!< Signal is beeing searched and can't be used yet. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_UNKNOWN = 2, /*!< Signal is tracked but don't know if used or not in the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_NOT_USED = 3, /*!< Signal is tracked and is not used in the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_REJECTED = 4, /*!< Signal is tracjed and is rejected from the solution. */ + SBG_ECOM_SAT_TRACKING_STATUS_TRACKING_USED = 5, /*!< Signal is tracked and used in the solution. */ +} SbgEComSatTrackingStatus; + +/*! + * Health status. + * + * All values must be strictly lower than 4. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComSatHealthStatus +{ + SBG_ECOM_SAT_HEALTH_STATUS_UNKNOWN = 0, /*!< Don't know the satellite or the signal health status. */ + SBG_ECOM_SAT_HEALTH_STATUS_HEALTHY = 1, /*!< The satellite or the signal is healthy and can be used. */ + SBG_ECOM_SAT_HEALTH_STATUS_UNHEALTHY = 2, /*!< The satellite or the signal is not healthy and can't be used. */ +} SbgEComSatHealthStatus; + +/*! + * Elevation status. + * + * All values must be strictly lower than 4. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComSatElevationStatus +{ + SBG_ECOM_SAT_ELEVATION_STATUS_UNKNOWN = 0, /*!< Don't know if the satellite elevation is setting or rising. */ + SBG_ECOM_SAT_ELEVATION_STATUS_SETTING = 1, /*!< The satellite elevation is setting. */ + SBG_ECOM_SAT_ELEVATION_STATUS_RISING = 2, /*!< The satellite elevation is rising */ +} SbgEComSatElevationStatus; + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Satellite signal data. + * + * The flags include the snr available, health and tracking statuses. + */ +typedef struct _SbgLogSatSignalData +{ + SbgEComSignalId id; /*!< Signal ID. */ + uint8_t flags; /*!< Flags. */ + uint8_t snr; /*!< Signal-to-noise ratio, in dB. */ +} SbgLogSatSignalData; + +/*! + * Satellite data. + * + * The flags include the constellation ID, the elevation status, the health status, and the tracking status. + * + * Satellite data and signal data each have their own health and tracking statuses. The statuses of satellite + * data may be a priority-based summary of the statuses of signal data, or they may reflect information that + * is limited to a satellite and unavailable for its signals. + * + * The priority rules are : + * - health status : unhealthy -> healthy -> unknown + * - tracking status : tracking and used -> tracking and not used -> tracking (solution status unknown) -> searching -> unknown + * + * For example, if satellite data have an unknown tracking status, and have three signals, one with the + * searching status, another with the tracking and not used status, and the last with the tracking and used status, + * the satellite data tracking status will be tracking and used. + * But if those satellite data are initially set with a healthy health status, and all three signals added have the + * unknown health status, the satellite data health status remains healthy. + */ +typedef struct _SbgLogSatData +{ + uint8_t id; /*!< Satellite ID. */ + int8_t elevation; /*!< Elevation, in degrees [-90; +90], valid if and only if the elevation is known. */ + uint16_t azimuth; /*!< Azimuth, in degrees [0; 359], valid if and only if the elevation is known. */ + uint16_t flags; /*!< Flags. */ + size_t nrSignals; /*!< Number of signals. */ + size_t signalDataArraySize; /*!< Size of the signal data array. */ + SbgLogSatSignalData *pSignalData; /*!< Signal data array. */ +} SbgLogSatData; + +/*! + * Satellite group data. + */ +typedef struct _SbgLogSatGroupData +{ + uint32_t timeStamp; /*!< Time since the sensor power up, in us. */ + uint32_t reserved; /*!< Reserved for future use. */ + size_t nrSatellites; /*!< Number of satellites. */ + size_t satDataArraySize; /*!< Size of the satellite data array. */ + SbgLogSatData *pSatData; /*!< Satellite data array. */ +} SbgLogSatGroupData; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Parse satellite group data from a stream buffer. + * + * This function constructs the satellite group data, which must be destroyed once not needed any more. + * + * \param[in] pStreamBuffer Stream buffer. + * \param[out] pSatGroupData Satellite group data. + * \return SBG_NO_ERROR if successful. + */ +SbgErrorCode sbgEComBinaryLogParseSatGroupData(SbgStreamBuffer *pStreamBuffer, SbgLogSatGroupData *pSatGroupData); + +/*! + * Write satellite group data to a stream buffer. + * + * \param[out] pStreamBuffer Stream buffer. + * \param[in] pSatGroupData Satellite group data. + * \return SBG_NO_ERROR if successful. + */ +SbgErrorCode sbgEComBinaryLogWriteSatGroupData(SbgStreamBuffer *pStreamBuffer, const SbgLogSatGroupData *pSatGroupData); + +/*! + * Satellite group data constructor. + * + * \param[in] pSatGroupData Satellite group data. + * \param[in] nrSatellites Number of satellites. + * \param[in] timeStamp Time stamp, in us. + * \return SBG_NO_ERROR if successful. + */ +SbgErrorCode sbgLogSatGroupDataConstruct(SbgLogSatGroupData *pSatGroupData, size_t nrSatellites, uint32_t timeStamp); + +/*! + * Satellite group data destructor. + * + * \param[in] pSatGroupData Satellite group data. + */ +void sbgLogSatGroupDataDestroy(SbgLogSatGroupData *pSatGroupData); + +/*! + * Add satellite data to satellite group data. + * + * \param[in] pSatGroupData Satellite group data. + * \param[in] nrSignals Number of signals. + * \param[in] id Satellite ID. + * \param[in] elevation Elevation, in degrees. + * \param[in] azimuth Azimuth, in degrees. + * \param[in] constellationId Constellation ID. + * \param[in] elevationStatus Elevation status. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + * \return Satellite data, NULL if an error occurs. + */ +SbgLogSatData *sbgLogSatGroupDataAdd(SbgLogSatGroupData *pSatGroupData, size_t nrSignals, uint8_t id, int8_t elevation, uint16_t azimuth, SbgEComConstellationId constellationId, SbgEComSatElevationStatus elevationStatus, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus); + +/*! + * Get satellite data from satellite group data. + * + * \param[in] pSatGroupData Satellite group data. + * \param[in] id Satellite ID. + * \return Satellite data, NULL if not found. + */ +SbgLogSatData *sbgLogSatGroupGet(SbgLogSatGroupData *pSatGroupData, uint8_t id); + +/*! + * Get the constellation ID of satellite data. + * + * \param[in] pSatData Satellite data. + * \return Constellation ID. + */ +SbgEComConstellationId sbgLogSatDataGetConstellationId(const SbgLogSatData *pSatData); + +/*! + * Get the constellation ID of satellite data as a read only C string. + * + * \param[in] pSatData Satellite data. + * \return Constellation ID as a read only C string. + */ +const char *sbgLogSatDataGetConstellationIdAsStr(const SbgLogSatData *pSatData); + +/*! + * Get the elevation status of satellite data. + * + * \param[in] pSatData Satellite data. + * \return Elevation status. + */ +SbgEComSatElevationStatus sbgLogSatDataGetElevationStatus(const SbgLogSatData *pSatData); + +/*! + * Get the elevation status of satellite data as a read only C string. + * + * \param[in] pSatData Satellite data. + * \return Elevation status as a read only C string. + */ +const char *sbgLogSatDataGetElevationStatusAsStr(const SbgLogSatData *pSatData); + +/*! + * Get the health status of satellite data. + * + * \param[in] pSatData Satellite data. + * \return Health status. + */ +SbgEComSatHealthStatus sbgLogSatDataGetHealthStatus(const SbgLogSatData *pSatData); + +/*! + * Get the health status of satellite data as a read only C string. + * + * \param[in] pSatData Satellite data. + * \return Health status as a read only C string. + */ +const char *sbgLogSatDataGetHealthStatusAsStr(const SbgLogSatData *pSatData); + +/*! + * Get the tracking status of satellite data. + * + * \param[in] pSatData Satellite data. + * \return Tracking status. + */ +SbgEComSatTrackingStatus sbgLogSatDataGetTrackingStatus(const SbgLogSatData *pSatData); + +/*! + * Get the tracking status of satellite data as a read only C string. + * + * \param[in] pSatData Satellite data. + * \return Tracking status as a read only C string. + */ +const char *sbgLogSatDataGetTrackingStatusAsStr(const SbgLogSatData *pSatData); + +/*! + * Add signal data to satellite data. + * + * The health and tracking statuses of the satellite data are updated according to their respective + * priority rules. + * + * \param[in] pSatData Satellite data. + * \param[in] id Signal ID. + * \param[in] healthStatus Health status. + * \param[in] trackingStatus Tracking status. + * \param[in] snrValid Set to true if the SNR value is valid. + * \param[in] snr Signal-to-noise ratio, in dB. + * \return Signal data, NULL if an error occurs. + */ +SbgLogSatSignalData *sbgLogSatDataAdd(SbgLogSatData *pSatData, SbgEComSignalId id, SbgEComSatHealthStatus healthStatus, SbgEComSatTrackingStatus trackingStatus, bool snrValid, uint8_t snr); + +/*! + * Get signal data from satellite data. + * + * \param[in] pSatData Satellite data. + * \param[in] id Signal ID. + * \return Signal data, NULL if not found. + */ +SbgLogSatSignalData *sbgLogSatDataGet(SbgLogSatData *pSatData, SbgEComSignalId id); + +/*! + * Get a signal id as a read only C string. + * + * \param[in] pSignalData Signal data. + * \return Signal id as a read only C string. + */ +const char *sbgLogSatSignalDataGetSignalIdAsStr(const SbgLogSatSignalData *pSignalData); + +/*! + * Returns true if the SNR value is valid. + * + * \param[in] pSignalData Signal data. + * \return true if the SNR value is valid. + */ +bool sbgLogSatSignalDataSnrIsValid(const SbgLogSatSignalData *pSignalData); + +/*! + * Get the health status of signal data. + * + * \param[in] pSignalData Signal data. + * \return Health status. + */ +SbgEComSatHealthStatus sbgLogSatSignalDataGetHealthStatus(const SbgLogSatSignalData *pSignalData); + +/*! + * Get the health status of signal data as a read only C string. + * + * \param[in] pSignalData Signal data. + * \return Health status as a read only C string. + */ +const char *sbgLogSatSignalDataGetHealthStatusAsStr(const SbgLogSatSignalData *pSignalData); + +/*! + * Get the tracking status of signal data. + * + * \param[in] pSignalData Signal data. + * \return Tracking status. + */ +SbgEComSatTrackingStatus sbgLogSatSignalDataGetTrackingStatus(const SbgLogSatSignalData *pSignalData); + +/*! + * Get the tracking status of signal data as a read only C string. + * + * \param[in] pSignalData Signal data. + * \return Tracking status as a read only C string. + */ +const char *sbgLogSatSignalDataGetTrackingStatusAsStr(const SbgLogSatSignalData *pSignalData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_SAT_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c new file mode 100644 index 0000000..d89f210 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.c @@ -0,0 +1,110 @@ +#include "sbgEComBinaryLogShipMotion.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + + // + // Read the main heave period in seconds + // + pOutputData->mainHeavePeriod = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Read the surge, sway and heave ship motion + // + pOutputData->shipMotion[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipMotion[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipMotion[2] = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Read the ship accelerations + // + pOutputData->shipAccel[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipAccel[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipAccel[2] = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Test if we have a additional information such as ship velocity and status (since version 1.4) + // + if (sbgStreamBufferGetSpace(pInputStream) >= 14) + { + // + // Read new outputs + // + pOutputData->shipVel[0] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipVel[1] = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->shipVel[2] = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + } + else + { + // + // Those outputs are not available in previous versions + // + pOutputData->shipVel[0] = 0.0f; + pOutputData->shipVel[1] = 0.0f; + pOutputData->shipVel[2] = 0.0f; + + pOutputData->status = 0; + } + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + + // + // Write the main heave period in seconds + // + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->mainHeavePeriod); + + // + // Write the surge, sway and heave ship motion + // + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipMotion[2]); + + // + // Write the ship accelerations + // + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipAccel[2]); + + // + // Write additional inforamtion added in version 1.4 + // + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[0]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[1]); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->shipVel[2]); + + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h new file mode 100644 index 0000000..81b9651 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogShipMotion.h @@ -0,0 +1,102 @@ +/*! + * \file sbgEComBinaryLogShipMotion.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 30 March 2013 + * + * \brief Parse logs that returns ship motion values such as heave. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_SHIP_MOTION_H +#define SBG_ECOM_BINARY_LOG_SHIP_MOTION_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Heave status definitions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_HEAVE_VALID (0x0001u << 0) /*!< Set to 1 after heave convergence time. */ +#define SBG_ECOM_HEAVE_VEL_AIDED (0x0001u << 1) /*!< Set to 1 if heave output is compensated for transient accelerations. */ +#define SBG_ECOM_HEAVE_SURGE_SWAY_INCLUDED (0x0001u << 2) /*!< Set to 1 if surge and sway channels are provided in this output. */ +#define SBG_ECOM_HEAVE_PERIOD_INCLUDED (0x0001u << 3) /*!< Set to 1 if the heave period is provided in this output. */ +#define SBG_ECOM_HEAVE_PERIOD_VALID (0x0001u << 4) /*!< Set to 1 if the returned heave period is assumed to be valid. */ +#define SBG_ECOM_HEAVE_SWELL_MODE (0x0001u << 5) /*!< Set to 1 if the real time heave filter is using the swell mode computations. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message.
+ * The data are expressed in the standard NED Ekinox coordiante frame. + * Surge is positive forward, sway is positive right and heave is positive down.
+ * Note that status flag should be read before using the different parameters because it will provide validity information + * about all included outputs. Some frames may not provide the heave period or surge/sway axes for example + */ +typedef struct _SbgLogShipMotionData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< Ship Motion data status bitmask */ + float mainHeavePeriod; /*!< Main heave period in seconds. */ + float shipMotion[3]; /*!< Surge, sway and heave in meters. */ + float shipAccel[3]; /*!< Surge, sway and heave ship Acceleration in m.s^-2. */ + float shipVel[3]; /*!< Surge, sway and heave velocities */ +} SbgLogShipMotionData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseShipMotionData(SbgStreamBuffer *pInputStream, SbgLogShipMotionData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteShipMotionData(SbgStreamBuffer *pOutputStream, const SbgLogShipMotionData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_SHIP_MOTION_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c new file mode 100644 index 0000000..2c83251 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.c @@ -0,0 +1,72 @@ +#include "sbgEComBinaryLogStatus.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->generalStatus = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->comStatus2 = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->comStatus = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->aidingStatus = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->reserved2 = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->reserved3 = sbgStreamBufferReadUint16LE(pInputStream); + + // + // Test if we have a additional information such as uptime (since version 1.7) + // + if (sbgStreamBufferGetSpace(pInputStream) >= sizeof(uint32_t)) + { + // + // Read the additional information + // + pOutputData->uptime = sbgStreamBufferReadUint32LE(pInputStream); + } + else + { + // + // Default the additional information + // + pOutputData->uptime = 0; + } + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->generalStatus); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->comStatus2); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->comStatus); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->aidingStatus); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->reserved2); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->reserved3); + + // + // Write the additional information added in version 1.7 + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->uptime); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h new file mode 100644 index 0000000..601305a --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogStatus.h @@ -0,0 +1,216 @@ +/*! + * \file sbgEComBinaryLogStatus.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 03 April 2013 + * + * \brief Parse logs used to report device status. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_STATUS_H +#define SBG_ECOM_BINARY_LOG_STATUS_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- General status definitions -// +//----------------------------------------------------------------------// +#define SBG_ECOM_GENERAL_MAIN_POWER_OK (0x0001u << 0) /*!< Set to 1 when main power supply is OK. */ +#define SBG_ECOM_GENERAL_IMU_POWER_OK (0x0001u << 1) /*!< Set to 1 when IMU power supply is OK. */ +#define SBG_ECOM_GENERAL_GPS_POWER_OK (0x0001u << 2) /*!< Set to 1 when GPS power supply is OK. */ +#define SBG_ECOM_GENERAL_SETTINGS_OK (0x0001u << 3) /*!< Set to 1 if settings where correctly loaded. */ +#define SBG_ECOM_GENERAL_TEMPERATURE_OK (0x0001u << 4) /*!< Set to 1 when temperature is within specified limits. */ +#define SBG_ECOM_GENERAL_DATALOGGER_OK (0x0001u << 5) /*!< Set to 1 when the datalogger is working correctly. */ +#define SBG_ECOM_GENERAL_CPU_OK (0x0001u << 6) /*!< Set to 1 if the CPU headroom is correct.*/ + +//----------------------------------------------------------------------// +//- Communication status definitions -// +//----------------------------------------------------------------------// + +/*! + * Communication CAN status definitions. + */ +#define SBG_ECOM_CAN_STATUS_SHIFT (28u) /*!< Shift used to access the CAN status part. */ +#define SBG_ECOM_CAN_STATUS_MASK (0x00000007u) /*!< Mask used to keep only the CAN status part. */ + +/*! + * Communication status bit mask definitions. + */ +#define SBG_ECOM_PORTA_VALID (0x00000001u << 0) /*!< Set to 0 in case of low level communication error. */ +#define SBG_ECOM_PORTB_VALID (0x00000001u << 1) /*!< Set to 0 in case of low level communication error. */ +#define SBG_ECOM_PORTC_VALID (0x00000001u << 2) /*!< Set to 0 in case of low level communication error. */ +#define SBG_ECOM_PORTD_VALID (0x00000001u << 3) /*!< Set to 0 in case of low level communication error. */ +#define SBG_ECOM_PORTE_VALID (0x00000001u << 4) /*!< Set to 0 in case of low level communication error. */ + +#define SBG_ECOM_PORTA_RX_OK (0x00000001u << 5) /*!< Set to 0 in case of error on PORT A input. */ +#define SBG_ECOM_PORTA_TX_OK (0x00000001u << 6) /*!< Set to 0 in case of error on PORT A output. */ +#define SBG_ECOM_PORTB_RX_OK (0x00000001u << 7) /*!< Set to 0 in case of error on PORT B input. */ +#define SBG_ECOM_PORTB_TX_OK (0x00000001u << 8) /*!< Set to 0 in case of error on PORT B output. */ +#define SBG_ECOM_PORTC_RX_OK (0x00000001u << 9) /*!< Set to 0 in case of error on PORT C input. */ +#define SBG_ECOM_PORTC_TX_OK (0x00000001u << 10) /*!< Set to 0 in case of error on PORT C output. */ +#define SBG_ECOM_PORTD_RX_OK (0x00000001u << 11) /*!< Set to 0 in case of error on PORT D input. */ +#define SBG_ECOM_PORTD_TX_OK (0x00000001u << 12) /*!< Set to 0 in case of error on PORT D input. */ +#define SBG_ECOM_PORTE_RX_OK (0x00000001u << 13) /*!< Set to 0 in case of error on PORT E input. */ +#define SBG_ECOM_PORTE_TX_OK (0x00000001u << 14) /*!< Set to 0 in case of error on PORT D input. */ + +#define SBG_ECOM_ETH0_VALID (0x00000001u << 15) /*!< Set to 0 in case of error on ETH0. */ +#define SBG_ECOM_ETH1_VALID (0x00000001u << 16) /*!< Set to 0 in case of error on ETH1. */ +#define SBG_ECOM_ETH2_VALID (0x00000001u << 17) /*!< Set to 0 in case of error on ETH2. */ +#define SBG_ECOM_ETH3_VALID (0x00000001u << 18) /*!< Set to 0 in case of error on ETH3. */ +#define SBG_ECOM_ETH4_VALID (0x00000001u << 19) /*!< Set to 0 in case of error on ETH4. */ + +#define SBG_ECOM_CAN_VALID (0x00000001u << 25) /*!< Set to 0 in case of low level communication error. */ +#define SBG_ECOM_CAN_RX_OK (0x00000001u << 26) /*!< Set to 0 in case of error on CAN Bus input buffer. */ +#define SBG_ECOM_CAN_TX_OK (0x00000001u << 27) /*!< Set to 0 in case of error on CAN Bus output buffer. */ + +/*! + * Second communication status bit mask definitions. + */ +#define SBG_ECOM_COM2_ETH0_RX_OK (0x0001u << 0) /*!< Set to 0 in case of error on ETH0 input. */ +#define SBG_ECOM_COM2_ETH0_TX_OK (0x0001u << 1) /*!< Set to 0 in case of error on ETH0 output. */ +#define SBG_ECOM_COM2_ETH1_RX_OK (0x0001u << 2) /*!< Set to 0 in case of error on ETH1 input. */ +#define SBG_ECOM_COM2_ETH1_TX_OK (0x0001u << 3) /*!< Set to 0 in case of error on ETH1 output. */ +#define SBG_ECOM_COM2_ETH2_RX_OK (0x0001u << 4) /*!< Set to 0 in case of error on ETH2 input. */ +#define SBG_ECOM_COM2_ETH2_TX_OK (0x0001u << 5) /*!< Set to 0 in case of error on ETH2 output. */ +#define SBG_ECOM_COM2_ETH3_RX_OK (0x0001u << 6) /*!< Set to 0 in case of error on ETH3 input. */ +#define SBG_ECOM_COM2_ETH3_TX_OK (0x0001u << 7) /*!< Set to 0 in case of error on ETH3 output. */ +#define SBG_ECOM_COM2_ETH4_RX_OK (0x0001u << 8) /*!< Set to 0 in case of error on ETH4 input. */ +#define SBG_ECOM_COM2_ETH4_TX_OK (0x0001u << 9) /*!< Set to 0 in case of error on ETH4 output. */ + +/*! + * Communication status for the CAN Bus. + */ +typedef enum _SbgEComCanBusStatus +{ + SBG_ECOM_CAN_BUS_OFF = 0, /*!< Bus OFF operation due to too much errors. */ + SBG_ECOM_CAN_BUS_TX_RX_ERR = 1, /*!< Errors on Tx or Rx. */ + SBG_ECOM_CAN_BUS_OK = 2, /*!< Bus OK. */ + SBG_ECOM_CAN_BUS_ERROR = 3 /*!< Bus error. */ +} SbgEComCanBusStatus; + +//----------------------------------------------------------------------// +//- Aiding status definitions -// +//----------------------------------------------------------------------// +#define SBG_ECOM_AIDING_GPS1_POS_RECV (0x00000001u << 0) /*!< Set to 1 when valid GPS 1 position data is received. */ +#define SBG_ECOM_AIDING_GPS1_VEL_RECV (0x00000001u << 1) /*!< Set to 1 when valid GPS 1 velocity data is received. */ +#define SBG_ECOM_AIDING_GPS1_HDT_RECV (0x00000001u << 2) /*!< Set to 1 when valid GPS 1 true heading data is received. */ +#define SBG_ECOM_AIDING_GPS1_UTC_RECV (0x00000001u << 3) /*!< Set to 1 when valid GPS 1 UTC time data is received. */ +#define SBG_ECOM_AIDING_GPS2_POS_RECV (0x00000001u << 4) /*!< Set to 1 when valid GPS 2 position data is received. */ +#define SBG_ECOM_AIDING_GPS2_VEL_RECV (0x00000001u << 5) /*!< Set to 1 when valid GPS 2 velocity data is received. */ +#define SBG_ECOM_AIDING_GPS2_HDT_RECV (0x00000001u << 6) /*!< Set to 1 when valid GPS 2 true heading data is received. */ +#define SBG_ECOM_AIDING_GPS2_UTC_RECV (0x00000001u << 7) /*!< Set to 1 when valid GPS 2 UTC time data is received. */ +#define SBG_ECOM_AIDING_MAG_RECV (0x00000001u << 8) /*!< Set to 1 when valid Magnetometer data is received. */ +#define SBG_ECOM_AIDING_ODO_RECV (0x00000001u << 9) /*!< Set to 1 when Odometer pulse is received. */ +#define SBG_ECOM_AIDING_DVL_RECV (0x00000001u << 10) /*!< Set to 1 when valid DVL data is received. */ +#define SBG_ECOM_AIDING_USBL_RECV (0x00000001u << 11) /*!< Set to 1 when valid USBL data is received. */ +#define SBG_ECOM_AIDING_DEPTH_RECV (0x00000001u << 12) /*!< Set to 1 when valid Depth Log data is received. */ +#define SBG_ECOM_AIDING_AIR_DATA_RECV (0x00000001u << 13) /*!< Set to 1 when valid Air Data (altitude and/or true airspeed) is received. */ +#define SBG_ECOM_AIDING_USER_POS_RECV (0x00000001u << 14) /*!< Set to 1 when valid user position data is received. */ +#define SBG_ECOM_AIDING_USER_VEL_RECV (0x00000001u << 15) /*!< Set to 1 when valid user velocity data is received. */ +#define SBG_ECOM_AIDING_USER_HEADING_RECV (0x00000001u << 16) /*!< Set to 1 when valid user heading data is received. */ + +//----------------------------------------------------------------------// +//- Status definitions -// +//----------------------------------------------------------------------// + +/*! + * Stores global status data. + */ +typedef struct _SbgLogStatusData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t generalStatus; /*!< General status bitmask and enums. */ + uint32_t comStatus; /*!< Communication status bitmask and enums. */ + uint16_t comStatus2; /*!< Second communication status bitmask and enums. */ + uint32_t aidingStatus; /*!< Aiding equipments status bitmask and enums. */ + uint32_t reserved2; /*!< Reserved status field for future use. */ + uint16_t reserved3; /*!< Reserved status field for future use. */ + uint32_t uptime; /*!< System uptime in seconds. */ +} SbgLogStatusData; + +//----------------------------------------------------------------------// +//- Public getters & helpers -// +//----------------------------------------------------------------------// + +/*! + * Method used to read the CAN bus status from a communication status field. + * + * \param[in] status Status field to extract the CAN bus status from it. + * \return The extracted CAN bus status. + */ +SBG_INLINE SbgEComCanBusStatus sbgEComLogStatusGetCanStatus(uint32_t status) +{ + return (SbgEComCanBusStatus)((status >> SBG_ECOM_CAN_STATUS_SHIFT) & SBG_ECOM_CAN_STATUS_MASK); +} + +/*! + * Method used to write the CAN bus status field. + * + * \param[in] canStatus The CAN bus status to set. + * \param[in] masks Bit mask to set. + * \return The build communication status field. + */ +SBG_INLINE uint32_t sbgEComLogStatusBuildCommunicationStatus(SbgEComCanBusStatus canStatus, uint32_t masks) +{ + // + // Create the combined status field + // + return ((((uint32_t)canStatus)&SBG_ECOM_CAN_STATUS_MASK) << SBG_ECOM_CAN_STATUS_SHIFT) | masks; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_STATUS message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseStatusData(SbgStreamBuffer *pInputStream, SbgLogStatusData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_STATUS message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteStatusData(SbgStreamBuffer *pOutputStream, const SbgLogStatusData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_STATUS_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c new file mode 100644 index 0000000..67491d0 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.c @@ -0,0 +1,57 @@ +#include "sbgEComBinaryLogUsbl.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + + pOutputData->latitude = sbgStreamBufferReadDoubleLE(pInputStream); + pOutputData->longitude = sbgStreamBufferReadDoubleLE(pInputStream); + + pOutputData->depth = sbgStreamBufferReadFloatLE(pInputStream); + + pOutputData->latitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->longitudeAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + pOutputData->depthAccuracy = sbgStreamBufferReadFloatLE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->latitude); + sbgStreamBufferWriteDoubleLE(pOutputStream, pInputData->longitude); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depth); + + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->latitudeAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->longitudeAccuracy); + sbgStreamBufferWriteFloatLE(pOutputStream, pInputData->depthAccuracy); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h new file mode 100644 index 0000000..d97f60f --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUsbl.h @@ -0,0 +1,105 @@ +/*! + * \file sbgEComBinaryLogUsbl.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 02 June 2014 + * + * \brief Parse received USBL position mesurements logs. + * + * USBL binary logs contains underwater positioning data of a USBL beacon. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_USBL_H +#define SBG_ECOM_BINARY_LOG_USBL_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log USBL status definitions -// +//----------------------------------------------------------------------// + +/*! + * USBL sensor status mask definitions + */ +#define SBG_ECOM_USBL_TIME_SYNC (0x0001u << 0) /*!< Set to 1 if the USBL sensor data is correctly time synchronized. */ +#define SBG_ECOM_USBL_POSITION_VALID (0x0001u << 1) /*!< Set to 1 if the USBL data represents a valid 2D position. */ +#define SBG_ECOM_USBL_DEPTH_VALID (0x0001u << 2) /*!< Set to 1 if the USBL data has a valid depth information. */ + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Log structure for USBL data. + */ +typedef struct _SbgLogUsblData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< USBL system status bitmask. */ + + double latitude; /*!< Latitude in degrees, positive north. */ + double longitude; /*!< Longitude in degrees, positive east. */ + + float depth; /*!< Depth in meters below mean sea level (positive down). */ + + float latitudeAccuracy; /*!< 1 sigma latitude accuracy in meters. */ + float longitudeAccuracy; /*!< 1 sigma longitude accuracy in meters. */ + float depthAccuracy; /*!< 1 sigma depth accuracy in meters. */ +} SbgLogUsblData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse data for the SBG_ECOM_LOG_USBL message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseUsblData(SbgStreamBuffer *pInputStream, SbgLogUsblData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_USBL message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteUsblData(SbgStreamBuffer *pOutputStream, const SbgLogUsblData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_USBL_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c new file mode 100644 index 0000000..d803438 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.c @@ -0,0 +1,100 @@ +#include "sbgEComBinaryLogUtc.h" + +//----------------------------------------------------------------------// +//- Private global definitions -// +//----------------------------------------------------------------------// + +/*!< Lookup table for clock status enum */ +static const char *gClockStatusStr[] = +{ + [SBG_ECOM_CLOCK_ERROR] = "SBG_ECOM_CLOCK_ERROR", + [SBG_ECOM_CLOCK_FREE_RUNNING] = "SBG_ECOM_CLOCK_FREE_RUNNING", + [SBG_ECOM_CLOCK_STEERING] = "SBG_ECOM_CLOCK_STEERING", + [SBG_ECOM_CLOCK_VALID] = "SBG_ECOM_CLOCK_VALID" +}; + +/*!< Lookup table for UTC status enum */ +static const char *gUtcStatusStr[] = +{ + [SBG_ECOM_UTC_INVALID] = "SBG_ECOM_UTC_INVALID", + [SBG_ECOM_UTC_NO_LEAP_SEC] = "SBG_ECOM_UTC_NO_LEAP_SEC", + [SBG_ECOM_UTC_VALID] = "SBG_ECOM_UTC_VALID", +}; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +const char *sbgEcomLogUtcGetClockStatusAsString(const SbgLogUtcData *pLogUtc) +{ + SbgEComClockStatus clockStatus; + + assert(pLogUtc); + + clockStatus = sbgEComLogUtcGetClockStatus(pLogUtc->status); + assert(clockStatus < SBG_ARRAY_SIZE(gClockStatusStr)); + + return gClockStatusStr[clockStatus]; +} + +const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgLogUtcData *pLogUtc) +{ + SbgEComClockUtcStatus utcStatus; + + assert(pLogUtc); + + utcStatus = sbgEComLogUtcGetClockUtcStatus(pLogUtc->status); + assert(utcStatus < SBG_ARRAY_SIZE(gUtcStatusStr)); + + return gUtcStatusStr[utcStatus]; +} + +SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData) +{ + assert(pInputStream); + assert(pOutputData); + + // + // Read the frame payload + // + pOutputData->timeStamp = sbgStreamBufferReadUint32LE(pInputStream); + pOutputData->status = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->year = sbgStreamBufferReadUint16LE(pInputStream); + pOutputData->month = sbgStreamBufferReadInt8LE(pInputStream); + pOutputData->day = sbgStreamBufferReadInt8LE(pInputStream); + pOutputData->hour = sbgStreamBufferReadInt8LE(pInputStream); + pOutputData->minute = sbgStreamBufferReadInt8LE(pInputStream); + pOutputData->second = sbgStreamBufferReadInt8LE(pInputStream); + pOutputData->nanoSecond = sbgStreamBufferReadInt32LE(pInputStream); + pOutputData->gpsTimeOfWeek = sbgStreamBufferReadUint32LE(pInputStream); + + // + // Return if any error has occurred while parsing the frame + // + return sbgStreamBufferGetLastError(pInputStream); +} + +SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData) +{ + assert(pOutputStream); + assert(pInputData); + + // + // Write the frame payload + // + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->timeStamp); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->status); + sbgStreamBufferWriteUint16LE(pOutputStream, pInputData->year); + sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->month); + sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->day); + sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->hour); + sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->minute); + sbgStreamBufferWriteInt8LE(pOutputStream, pInputData->second); + sbgStreamBufferWriteInt32LE(pOutputStream, pInputData->nanoSecond); + sbgStreamBufferWriteUint32LE(pOutputStream, pInputData->gpsTimeOfWeek); + + // + // Return if any error has occurred while writing the frame + // + return sbgStreamBufferGetLastError(pOutputStream); +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h new file mode 100644 index 0000000..39561bb --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogUtc.h @@ -0,0 +1,190 @@ +/*! + * \file sbgEComBinaryLogUtc.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 20 February 2013 + * + * \brief Parse logs used to report device UTC time. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_BINARY_LOG_UTC_H +#define SBG_ECOM_BINARY_LOG_UTC_H + +// sbgCommonLib headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Clock status definitions -// +//----------------------------------------------------------------------// + +/*! + * Clock status and UTC time status definitions. + */ +#define SBG_ECOM_CLOCK_STATUS_SHIFT (1u) /*!< Shift used to extract the clock status part. */ +#define SBG_ECOM_CLOCK_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock status part. */ +#define SBG_ECOM_CLOCK_UTC_STATUS_SHIFT (6u) /*!< Shift used to extract the clock UTC status part. */ +#define SBG_ECOM_CLOCK_UTC_STATUS_MASK (0x000Fu) /*!< Mask used to keep only the clock UTC status part. */ + +/*! + * Clock status mask definitions. + */ +#define SBG_ECOM_CLOCK_STABLE_INPUT (0x0001u << 0) /*!< Set to 1 if a stable input clock could be used to synchronized the internal clock. */ +#define SBG_ECOM_CLOCK_UTC_SYNC (0x0001u << 5) /*!< The UTC time is synchronized with a PPS. */ + +/*! + * Clock status enum. + */ +typedef enum _SbgEComClockStatus +{ + SBG_ECOM_CLOCK_ERROR = 0, /*!< An error has occurred on the clock estimation. */ + SBG_ECOM_CLOCK_FREE_RUNNING = 1, /*!< The clock is only based on the internal crystal. */ + SBG_ECOM_CLOCK_STEERING = 2, /*!< A PPS has been detected and the clock is converging to it. */ + SBG_ECOM_CLOCK_VALID = 3 /*!< The clock has converged to the PPS and is within 500ns. */ +} SbgEComClockStatus; + +/*! + * Status for the UTC time data. + */ +typedef enum _SbgEComClockUtcStatus +{ + SBG_ECOM_UTC_INVALID = 0, /*!< The UTC time is not known, we are just propagating the UTC time internally. */ + SBG_ECOM_UTC_NO_LEAP_SEC = 1, /*!< We have received valid UTC time information but we don't have the leap seconds information. */ + SBG_ECOM_UTC_VALID = 2 /*!< We have received valid UTC time data with valid leap seconds. */ +} SbgEComClockUtcStatus; + +//----------------------------------------------------------------------// +//- Clock status helpers methods -// +//----------------------------------------------------------------------// + +/*! + * Method used to read the clock status from a status field. + * + * \param[in] status Status field to extract the clock status from it. + * \return The extracted clock status. + */ +SBG_INLINE SbgEComClockStatus sbgEComLogUtcGetClockStatus(uint16_t status) +{ + return (SbgEComClockStatus)((status >> SBG_ECOM_CLOCK_STATUS_SHIFT) & SBG_ECOM_CLOCK_STATUS_MASK); +} + +/*! + * Method used to read the UTC time status from a clock status field. + * + * \param[in] status Status field to extract the UTC time status from it. + * \return The extracted UTC time status. + */ +SBG_INLINE SbgEComClockUtcStatus sbgEComLogUtcGetClockUtcStatus(uint16_t status) +{ + return (SbgEComClockUtcStatus)((status >> SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) & SBG_ECOM_CLOCK_UTC_STATUS_MASK); +} + +/*! + * Method used to write the clock status field. + * + * \param[in] clockStatus The clock status to set. + * \param[in] utcStatus The UTC time status to set. + * \param[in] masks Bit mask to set. + * \return The build clock status field. + */ +SBG_INLINE uint16_t sbgEComLogUtcBuildClockStatus(SbgEComClockStatus clockStatus, SbgEComClockUtcStatus utcStatus, uint16_t masks) +{ + // + // Create the combined status field + // + return ((((uint16_t)clockStatus)&SBG_ECOM_CLOCK_STATUS_MASK) << SBG_ECOM_CLOCK_STATUS_SHIFT) | + ((((uint16_t)utcStatus)&SBG_ECOM_CLOCK_UTC_STATUS_MASK) << SBG_ECOM_CLOCK_UTC_STATUS_SHIFT) | masks; +} + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Structure that stores data for the SBG_ECOM_LOG_UTC_TIME message. + */ +typedef struct _SbgLogUtcData +{ + uint32_t timeStamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< UTC time and clock status information */ + uint16_t year; /*!< Year for example: 2013. */ + int8_t month; /*!< Month in year [1 .. 12]. */ + int8_t day; /*!< Day in month [1 .. 31]. */ + int8_t hour; /*!< Hour in day [0 .. 23]. */ + int8_t minute; /*!< Minute in hour [0 .. 59]. */ + int8_t second; /*!< Second in minute [0 .. 60]. (60 is used only when a leap second is added) */ + int32_t nanoSecond; /*!< Nanosecond of current second in ns. */ + uint32_t gpsTimeOfWeek; /*!< GPS time of week in ms. */ +} SbgLogUtcData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Returns the clock status as a NULL terminated C string. + * + * \param[in] pLogUtc UTC log instance. + * \return The clock status as a C string. + */ +const char *sbgEcomLogUtcGetClockStatusAsString(const SbgLogUtcData *pLogUtc); + + +/*! + * Returns the UTC status as a NULL terminated C string. + * + * \param[in] pLogUtc UTC log instance. + * \return The UTC status as a C string. + */ +const char *sbgEcomLogUtcGetUtcStatusAsString(const SbgLogUtcData *pLogUtc); + +/*! + * Parse data for the SBG_ECOM_LOG_UTC_DATA message and fill the corresponding structure. + * + * \param[in] pInputStream Input stream buffer to read the payload from. + * \param[out] pOutputData Pointer on the output structure that stores parsed data. + * \return SBG_NO_ERROR if the payload has been parsed. + */ +SbgErrorCode sbgEComBinaryLogParseUtcData(SbgStreamBuffer *pInputStream, SbgLogUtcData *pOutputData); + +/*! + * Write data for the SBG_ECOM_LOG_UTC_DATA message to the output stream buffer from the provided structure. + * + * \param[out] pOutputStream Output stream buffer to write the payload to. + * \param[in] pInputData Pointer on the input structure that stores data to write. + * \return SBG_NO_ERROR if the message has been generated in the provided buffer. + */ +SbgErrorCode sbgEComBinaryLogWriteUtcData(SbgStreamBuffer *pOutputStream, const SbgLogUtcData *pInputData); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOG_UTC_H diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c new file mode 100644 index 0000000..883e0b2 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.c @@ -0,0 +1,183 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComBinaryLogAirData.h" +#include "sbgEComBinaryLogDepth.h" +#include "sbgEComBinaryLogDiag.h" +#include "sbgEComBinaryLogDvl.h" +#include "sbgEComBinaryLogEkf.h" +#include "sbgEComBinaryLogEvent.h" +#include "sbgEComBinaryLogGps.h" +#include "sbgEComBinaryLogImu.h" +#include "sbgEComBinaryLogMag.h" +#include "sbgEComBinaryLogOdometer.h" +#include "sbgEComBinaryLogRtcm.h" +#include "sbgEComBinaryLogShipMotion.h" +#include "sbgEComBinaryLogStatus.h" +#include "sbgEComBinaryLogUsbl.h" +#include "sbgEComBinaryLogUtc.h" +#include "sbgEComBinaryLogSat.h" +#include "sbgEComBinaryLogs.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer inputStream; + + assert(pPayload); + assert(payloadSize > 0); + assert(pOutputData); + + // + // Create an input stream buffer that points to the frame payload so we can easily parse it's content + // + sbgStreamBufferInitForRead(&inputStream, pPayload, payloadSize); + + // + // Handle the different classes of messages differently + // + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + // + // Parse the incoming log according to its type + // + switch (msg) + { + case SBG_ECOM_LOG_STATUS: + errorCode = sbgEComBinaryLogParseStatusData(&inputStream, &pOutputData->statusData); + break; + case SBG_ECOM_LOG_IMU_DATA: + errorCode = sbgEComBinaryLogParseImuData(&inputStream, &pOutputData->imuData); + break; + case SBG_ECOM_LOG_IMU_SHORT: + errorCode = sbgEComBinaryLogParseImuShort(&inputStream, &pOutputData->imuShort); + break; + case SBG_ECOM_LOG_EKF_EULER: + errorCode = sbgEComBinaryLogParseEkfEulerData(&inputStream, &pOutputData->ekfEulerData); + break; + case SBG_ECOM_LOG_EKF_QUAT: + errorCode = sbgEComBinaryLogParseEkfQuatData(&inputStream, &pOutputData->ekfQuatData); + break; + case SBG_ECOM_LOG_EKF_NAV: + errorCode = sbgEComBinaryLogParseEkfNavData(&inputStream, &pOutputData->ekfNavData); + break; + case SBG_ECOM_LOG_SHIP_MOTION: + case SBG_ECOM_LOG_SHIP_MOTION_HP: + errorCode = sbgEComBinaryLogParseShipMotionData(&inputStream, &pOutputData->shipMotionData); + break; + case SBG_ECOM_LOG_ODO_VEL: + errorCode = sbgEComBinaryLogParseOdometerData(&inputStream, &pOutputData->odometerData); + break; + case SBG_ECOM_LOG_UTC_TIME: + errorCode = sbgEComBinaryLogParseUtcData(&inputStream, &pOutputData->utcData); + break; + case SBG_ECOM_LOG_GPS1_VEL: + case SBG_ECOM_LOG_GPS2_VEL: + errorCode = sbgEComBinaryLogParseGpsVelData(&inputStream, &pOutputData->gpsVelData); + break; + case SBG_ECOM_LOG_GPS1_POS: + case SBG_ECOM_LOG_GPS2_POS: + errorCode = sbgEComBinaryLogParseGpsPosData(&inputStream, &pOutputData->gpsPosData); + break; + case SBG_ECOM_LOG_GPS1_HDT: + case SBG_ECOM_LOG_GPS2_HDT: + errorCode = sbgEComBinaryLogParseGpsHdtData(&inputStream, &pOutputData->gpsHdtData); + break; + case SBG_ECOM_LOG_GPS1_RAW: + case SBG_ECOM_LOG_GPS2_RAW: + errorCode = sbgEComBinaryLogParseGpsRawData(&inputStream, &pOutputData->gpsRawData); + break; + case SBG_ECOM_LOG_GPS1_SAT: + case SBG_ECOM_LOG_GPS2_SAT: + errorCode = sbgEComBinaryLogParseSatGroupData(&inputStream, &pOutputData->satGroupData); + break; + case SBG_ECOM_LOG_RTCM_RAW: + errorCode = sbgEComBinaryLogParseRtcmRawData(&inputStream, &pOutputData->rtcmRawData); + break; + case SBG_ECOM_LOG_MAG: + errorCode = sbgEComBinaryLogParseMagData(&inputStream, &pOutputData->magData); + break; + case SBG_ECOM_LOG_MAG_CALIB: + errorCode = sbgEComBinaryLogParseMagCalibData(&inputStream, &pOutputData->magCalibData); + break; + case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: + errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); + break; + case SBG_ECOM_LOG_DVL_WATER_TRACK: + errorCode = sbgEComBinaryLogParseDvlData(&inputStream, &pOutputData->dvlData); + break; + case SBG_ECOM_LOG_AIR_DATA: + errorCode = sbgEComBinaryLogParseAirData(&inputStream, &pOutputData->airData); + break; + case SBG_ECOM_LOG_USBL: + errorCode = sbgEComBinaryLogParseUsblData(&inputStream, &pOutputData->usblData); + break; + case SBG_ECOM_LOG_DEPTH: + errorCode = sbgEComBinaryLogParseDepth(&inputStream, &pOutputData->depthData); + break; + case SBG_ECOM_LOG_EVENT_A: + case SBG_ECOM_LOG_EVENT_B: + case SBG_ECOM_LOG_EVENT_C: + case SBG_ECOM_LOG_EVENT_D: + case SBG_ECOM_LOG_EVENT_E: + case SBG_ECOM_LOG_EVENT_OUT_A: + case SBG_ECOM_LOG_EVENT_OUT_B: + errorCode = sbgEComBinaryLogParseEvent(&inputStream, &pOutputData->eventMarker); + break; + case SBG_ECOM_LOG_DIAG: + errorCode = sbgEComBinaryLogParseDiagData(&inputStream, &pOutputData->diagData); + break; + + default: + errorCode = SBG_ERROR; + } + } + else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) + { + // + // Parse the message depending on the message ID + // + switch ((SbgEComLog1)msg) + { + case SBG_ECOM_LOG_FAST_IMU_DATA: + errorCode = sbgEComBinaryLogParseFastImuData(&inputStream, &pOutputData->fastImuData); + break; + default: + errorCode = SBG_ERROR; + } + } + else + { + // + // Un-handled message class + // + errorCode = SBG_ERROR; + } + + return errorCode; +} + +void sbgEComBinaryLogCleanup(SbgBinaryLogData *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId) +{ + assert(pLogData); + + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + switch (msgId) + { + case SBG_ECOM_LOG_GPS1_SAT: + case SBG_ECOM_LOG_GPS2_SAT: + sbgLogSatGroupDataDestroy(&pLogData->satGroupData); + break; + } + } +} diff --git a/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h new file mode 100644 index 0000000..9782a16 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/binaryLogs/sbgEComBinaryLogs.h @@ -0,0 +1,136 @@ +/*! + * \file sbgEComBinaryLogs.h + * \ingroup binaryLogs + * \author SBG Systems + * \date 06 February 2013 + * + * \brief Parse incoming sbgECom logs and store result in an union. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +/*! + * \defgroup binaryLogs Binary Logs + * \brief All messages and logs that can be output by the device. + */ + +#ifndef SBG_ECOM_BINARY_LOGS_H +#define SBG_ECOM_BINARY_LOGS_H + + // sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComBinaryLogAirData.h" +#include "sbgEComBinaryLogDepth.h" +#include "sbgEComBinaryLogDiag.h" +#include "sbgEComBinaryLogDvl.h" +#include "sbgEComBinaryLogEkf.h" +#include "sbgEComBinaryLogEvent.h" +#include "sbgEComBinaryLogGps.h" +#include "sbgEComBinaryLogImu.h" +#include "sbgEComBinaryLogMag.h" +#include "sbgEComBinaryLogOdometer.h" +#include "sbgEComBinaryLogRawData.h" +#include "sbgEComBinaryLogRtcm.h" +#include "sbgEComBinaryLogSat.h" +#include "sbgEComBinaryLogShipMotion.h" +#include "sbgEComBinaryLogStatus.h" +#include "sbgEComBinaryLogUsbl.h" +#include "sbgEComBinaryLogUtc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Log structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Union used to store received logs data. + */ +typedef union _SbgBinaryLogData +{ + SbgLogStatusData statusData; /*!< Stores data for the SBG_ECOM_LOG_STATUS message. */ + SbgLogImuData imuData; /*!< Stores data for the SBG_ECOM_LOG_IMU_DATA message. */ + SbgLogImuShort imuShort; /*!< Stores data for the SBG_ECOM_LOG_IMU_SHORT message. */ + SbgLogEkfEulerData ekfEulerData; /*!< Stores data for the SBG_ECOM_LOG_EKF_EULER message. */ + SbgLogEkfQuatData ekfQuatData; /*!< Stores data for the SBG_ECOM_LOG_EKF_QUAT message. */ + SbgLogEkfNavData ekfNavData; /*!< Stores data for the SBG_ECOM_LOG_EKF_NAV message. */ + SbgLogShipMotionData shipMotionData; /*!< Stores data for the SBG_ECOM_LOG_SHIP_MOTION or SBG_ECOM_LOG_SHIP_MOTION_HP message. */ + SbgLogOdometerData odometerData; /*!< Stores data for the SBG_ECOM_LOG_ODO_VEL message. */ + SbgLogUtcData utcData; /*!< Stores data for the SBG_ECOM_LOG_UTC_TIME message. */ + SbgLogGpsPos gpsPosData; /*!< Stores data for the SBG_ECOM_LOG_GPS_POS message. */ + SbgLogGpsVel gpsVelData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_VEL message. */ + SbgLogGpsHdt gpsHdtData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_HDT message. */ + SbgLogRawData gpsRawData; /*!< Stores data for the SBG_ECOM_LOG_GPS#_RAW message. */ + SbgLogRawData rtcmRawData; /*!< Stores data for the SBG_ECOM_LOG_RTCM_RAW message. */ + SbgLogMag magData; /*!< Stores data for the SBG_ECOM_LOG_MAG message. */ + SbgLogMagCalib magCalibData; /*!< Stores data for the SBG_ECOM_LOG_MAG_CALIB message. */ + SbgLogDvlData dvlData; /*!< Stores data for the SBG_ECOM_LOG_DVL_BOTTOM_TRACK message. */ + SbgLogAirData airData; /*!< Stores data for the SBG_ECOM_LOG_AIR_DATA message. */ + SbgLogUsblData usblData; /*!< Stores data for the SBG_ECOM_LOG_USBL message. */ + SbgLogDepth depthData; /*!< Stores data for the SBG_ECOM_LOG_DEPTH message */ + SbgLogEvent eventMarker; /*!< Stores data for the SBG_ECOM_LOG_EVENT_# message. */ + SbgLogDiagData diagData; /*!< Stores data for the SBG_ECOM_LOG_DIAG message. */ + SbgLogSatGroupData satGroupData; /*!< Stores data for the SBG_ECOM_LOG_SAT message. */ + + /* Fast logs */ + SbgLogFastImuData fastImuData; /*!< Stores Fast Imu Data for 1KHz output */ + +} SbgBinaryLogData; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Parse an incoming log and fill the output union. + * + * \param[in] msgClass Received message class + * \param[in] msg Received message ID + * \param[in] pPayload Read only pointer on the payload buffer. + * \param[in] payloadSize Payload size in bytes. + * \param[out] pOutputData Pointer on the output union that stores parsed data. + */ +SbgErrorCode sbgEComBinaryLogParse(SbgEComClass msgClass, SbgEComMsgId msg, const void *pPayload, size_t payloadSize, SbgBinaryLogData *pOutputData); + +/*! + * Clean up resources allocated during parsing, if any. + * + * \param[in] pLogData Log data. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + */ +void sbgEComBinaryLogCleanup(SbgBinaryLogData *pLogData, SbgEComClass msgClass, SbgEComMsgId msgId); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_BINARY_LOGS_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmd.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmd.h new file mode 100644 index 0000000..2620e5e --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmd.h @@ -0,0 +1,62 @@ +/*! + * \file sbgEComCmd.h + * \ingroup commands + * \author SBG Systems + * \date 16 June 2014 + * + * \brief Include all available sbgECom commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +/*! + * \defgroup commands Commands + * \brief Configuration and action commands mainly for ELLIPSE products. + */ + +#ifndef SBG_ECOM_CMD_H +#define SBG_ECOM_CMD_H + +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComCmdAdvanced.h" +#include "sbgEComCmdAirData.h" +#include "sbgEComCmdApi.h" +#include "sbgEComCmdDvl.h" +#include "sbgEComCmdEthernet.h" +#include "sbgEComCmdEvent.h" +#include "sbgEComCmdFeatures.h" +#include "sbgEComCmdGnss.h" +#include "sbgEComCmdInfo.h" +#include "sbgEComCmdInterface.h" +#include "sbgEComCmdLicense.h" +#include "sbgEComCmdMag.h" +#include "sbgEComCmdOdo.h" +#include "sbgEComCmdOutput.h" +#include "sbgEComCmdSensor.h" +#include "sbgEComCmdSettings.h" + +#endif // SBG_ECOM_CMD_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.c new file mode 100644 index 0000000..d0df61c --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.c @@ -0,0 +1,318 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdAdvanced.h" +#include "sbgEComCmdCommon.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdAdvancedGetConf(SbgEComHandle *pHandle, SbgEComAdvancedConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command without payload since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ADVANCED_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read the mandatory time reference parameter + // + pConf->timeReference = (SbgEComTimeReferenceSrc)sbgStreamBufferReadUint8LE(&inputStream); + errorCode = sbgStreamBufferGetLastError(&inputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // The GNSS options parameter has been introduced in ELLIPSE firmware v2.2 + // We shouldn't report it as an error for older firmware + // + pConf->gnssOptions = sbgStreamBufferReadUint32LE(&inputStream); + + if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) + { + // + // The NMEA options parameter has been introduced in ELLIPSE firmware v2.3 + // We shouldn't report it as an error for older firmware + // + pConf->nmeaOptions = sbgStreamBufferReadUint32LE(&inputStream); + + if (sbgStreamBufferGetLastError(&inputStream) == SBG_NO_ERROR) + { + errorCode = SBG_NO_ERROR; + } + else + { + pConf->nmeaOptions = 0; + } + } + else + { + pConf->gnssOptions = 0; + } + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdAdvancedSetConf(SbgEComHandle *pHandle, const SbgEComAdvancedConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[9]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->timeReference); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->gnssOptions); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->nmeaOptions); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ADVANCED_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComValidityThresholds *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command without payload since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_VALIDITY_THRESHOLDS command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters and check payload consistency + // + pConf->positionThreshold = sbgStreamBufferReadFloatLE(&inputStream); + pConf->velocityThreshold = sbgStreamBufferReadFloatLE(&inputStream); + pConf->attitudeThreshold = sbgStreamBufferReadFloatLE(&inputStream); + pConf->headingThreshold = sbgStreamBufferReadFloatLE(&inputStream); + + errorCode = sbgStreamBufferGetLastError(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdAdvancedSetThresholds(SbgEComHandle *pHandle, const SbgEComValidityThresholds *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[4*sizeof(float)]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pConf->positionThreshold); + sbgStreamBufferWriteFloatLE(&outputStream, pConf->velocityThreshold); + sbgStreamBufferWriteFloatLE(&outputStream, pConf->attitudeThreshold); + sbgStreamBufferWriteFloatLE(&outputStream, pConf->headingThreshold); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_VALIDITY_THRESHOLDS, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.h new file mode 100644 index 0000000..23a7415 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAdvanced.h @@ -0,0 +1,142 @@ +/*! + * \file sbgEComCmdAdvanced.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Advanced settings related commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_ADVANCED_H +#define SBG_ECOM_CMD_ADVANCED_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Advanced definitions -// +//----------------------------------------------------------------------// + +/*! + * List of available time reference source. + */ +typedef enum _SbgEComTimeReferenceSrc +{ + SBG_ECOM_TIME_REF_DISABLED = 0, /*!< The device is running it's internal clock without any time reference. */ + SBG_ECOM_TIME_REF_SYNC_IN_A, /*!< The main port sync in A is used as a time reference. */ + SBG_ECOM_TIME_REF_UTC_GPS_1 /*!< The GPS 1 module is used to provide both time reference and UTC data. */ +} SbgEComTimeReferenceSrc; + +/*! + * List all options for the GNSS bitmask options + */ +#define SBG_ECOM_GNSS_OPT_01 (uint32_t)(0x00000001 << 0) /*!< Reserved advanced GNSS option for ELLIPSE-N/D v3 */ + +/*! + * List all options for the NMEA bitmask options + */ +#define SBG_ECOM_NMEA_OPT_MODE_STD (uint32_t)(0x00000001 << 0) /*!< Output NMEA messages that complies with 82 chars limit */ +#define SBG_ECOM_NMEA_OPT_FORCE_UTC (uint32_t)(0x00000001 << 4) /*!< Always output time in NMEA messages even if invalid */ + +//----------------------------------------------------------------------// +//- Advanced configurations -// +//----------------------------------------------------------------------// + +/*! + * Structure containing all the info for advanced configuration. + */ +typedef struct _SbgEComAdvancedConf +{ + SbgEComTimeReferenceSrc timeReference; /*!< Time reference source for clock alignment. */ + uint32_t gnssOptions; /*!< Advanced GNSS options - contact SBG Systems. */ + uint32_t nmeaOptions; /*!< Advanced NMEA output options. */ +} SbgEComAdvancedConf; + +/*! + * Structure containing all validity thresholds (status outputs) + * Setting these thresholds to 0.0 will keep default configuration + */ +typedef struct _SbgEComValidityThresholds +{ + float positionThreshold; /*!< Norm of the position standard deviation threshold to raise position valid flag (m)*/ + float velocityThreshold; /*!< Norm of the velocity standard deviation threshold to raise velocity valid flag (m/s)*/ + float attitudeThreshold; /*!< Max of the roll/pitch standard deviations threshold to raise attitude valid flag (rad) */ + float headingThreshold; /*!< Heading standard deviations threshold to raise heading valid flag (rad) */ +} SbgEComValidityThresholds; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the advanced configurations. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pConf Pointer to a SbgEComAdvancedConf to contain the current configuration. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAdvancedGetConf(SbgEComHandle *pHandle, SbgEComAdvancedConf *pConf); + +/*! + * Set the advanced configurations. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pConf Pointer to a SbgEComAdvancedConf that contains the new configuration. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAdvancedSetConf(SbgEComHandle *pHandle, const SbgEComAdvancedConf *pConf); + +/*! + * Retrieve the current validity thresholds + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pConf Pointer to a SbgEComValidityThresholds to contain the current configuration. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ + +SbgErrorCode sbgEComCmdAdvancedGetThresholds(SbgEComHandle *pHandle, SbgEComValidityThresholds *pConf); + +/*! + * Set the validity thresholds + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pConf Pointer to a SbgEComValidityThresholds that contains the new configuration. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAdvancedSetThresholds(SbgEComHandle *pHandle, const SbgEComValidityThresholds *pConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_ADVANCED_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.c new file mode 100644 index 0000000..5ae9a6c --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.c @@ -0,0 +1,323 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdAirData.h" +#include "sbgEComCmdCommon.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + + +SbgErrorCode sbgEComCmdAirDataSetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds modelId) +{ + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, modelId); +} + +SbgErrorCode sbgEComCmdAirDataGetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds *pModelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; + + assert(pHandle); + assert(pModelId); + + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_MODEL_ID, &modelIdAsUint); + + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComAirDataModelsIds)modelIdAsUint; + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdAirDataSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pLeverArm); + + // + // Create the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[2]); + + // + // Make sure the payload has been build correctly + // + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdAirDataGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse the payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + // + // The command has been executed successfully so return if an error has occurred during payload parsing + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdAirDataSetRejection(SbgEComHandle *pHandle, const SbgEComAirDataRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[2 * sizeof(uint8_t)]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + // + // Create the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->airspeed); + sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->altitude); + + // + // Make sure the payload has been build correctly + // + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdAirDataGetRejection(SbgEComHandle *pHandle, SbgEComAirDataRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIRDATA_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the payload + // + pRejectConf->airspeed = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + pRejectConf->altitude = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return if an error has occurred during payload parsing + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.h new file mode 100644 index 0000000..2d463f4 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdAirData.h @@ -0,0 +1,134 @@ +/*! + * \file sbgEComCmdAirData.h + * \ingroup commands + * \author SBG Systems + * \date 18 February 2019 + * + * \brief AirData aiding module configuration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_AIR_DATA_H +#define SBG_ECOM_CMD_AIR_DATA_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * This enum defines the different AirData model IDs available in standard + */ +typedef enum _SbgEComAirDataModelsIds +{ + SBG_ECOM_AIR_DATA_MODEL_INTERNAL = 1, /*!< Use the internal barometer sensor if available. */ + SBG_ECOM_AIR_DATA_MODEL_GENERIC_ECOM = 2, /*!< Generic AirData model using sbgECom input protocol format. */ + SBG_ECOM_AIR_DATA_MODEL_AHRS_500 = 3, /*!< Crossbow AHRS-500 compatible input for barometric altitude and airspeed. */ +} SbgEComAirDataModelsIds; + +/*! + * Holds all necessary information for AirData module data rejection. + */ +typedef struct _SbgEComAirDataRejectionConf +{ + SbgEComRejectionMode airspeed; /*!< Rejection mode for the true air speed measurement. */ + SbgEComRejectionMode altitude; /*!< Rejection mode for the barometric altitude measurement. */ +} SbgEComAirDataRejectionConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Set the AirData model to use that both defines the protocol as well as the associated error model. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId AirData model ID to set + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataSetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds modelId); + +/*! + * Retrieve the AirData model id currently in use by the device. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Returns the AirData model ID currently in use by the device. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataGetModelId(SbgEComHandle *pHandle, SbgEComAirDataModelsIds *pModelId); + +/*! + * Set the lever arm configuration of the AirData module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pLeverArm The X, Y, Z airspeed sensor lever arm in meters from the pitot sensor to the IMU. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm); + +/*! + * Retrieve the lever arm configuration of the AirData module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pLeverArm Returns the airspeed sensor X,Y,Z lever arm in meters from the pitot sensor to the IMU. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm); + +/*! + * Set the rejection configuration of the AirData module (this command doesn't need a reboot to be applied) + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf The new rejection configuration to set. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataSetRejection(SbgEComHandle *pHandle, const SbgEComAirDataRejectionConf *pRejectConf); + +/*! + * Retrieve the current rejection configuration of the AirData module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Return the rejection configuration currently in use. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdAirDataGetRejection(SbgEComHandle *pHandle, SbgEComAirDataRejectionConf *pRejectConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_AIR_DATA_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.c new file mode 100644 index 0000000..ebb832a --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.c @@ -0,0 +1,243 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdApi.h" +#include "sbgEComCmdCommon.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Status code indicating success. + */ +#define SBG_ECOM_CMD_API_STATUS_CODE_OK (200) + +/*! + * Status code indicating an internal server error. + */ +#define SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR (500) + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Parse the payload of a REST API reply. + * + * \param[in] pReply REST API reply. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComCmdApiReplyParsePayload(SbgEComCmdApiReply *pReply) +{ + SbgErrorCode errorCode; + SbgStreamBuffer streamBuffer; + uint16_t statusCode; + + assert(pReply); + + sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(&pReply->payload), sbgEComProtocolPayloadGetSize(&pReply->payload)); + + statusCode = sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + const char *pContent; + size_t size; + + pContent = sbgStreamBufferGetCursor(&streamBuffer); + size = sbgStreamBufferGetSpace(&streamBuffer); + + if (size != 0) + { + if (pContent[size - 1] == '\0') + { + pReply->statusCode = statusCode; + pReply->pContent = pContent; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid content format"); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid content size"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to read status code"); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply) //TODO: minuscule ds un commit séparé +{ + assert(pReply); + + sbgEComProtocolPayloadConstruct(&pReply->payload); + + pReply->statusCode = SBG_ECOM_CMD_API_STATUS_CODE_INTERNAL_SERVER_ERROR; + pReply->pContent = NULL; +} + +void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply) //TODO: minuscule ds un commit séparé +{ + assert(pReply); + + sbgEComProtocolPayloadDestroy(&pReply->payload); +} + +bool sbgEComCmdApiReplySuccessful(const SbgEComCmdApiReply *pReply) +{ + assert(pReply); + + return pReply->statusCode == SBG_ECOM_CMD_API_STATUS_CODE_OK; +} + +SbgErrorCode sbgEComCmdApiGet(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, SbgEComCmdApiReply *pReply) +{ + SbgErrorCode errorCode; + uint8_t *pSendBuffer; + SbgStreamBuffer streamBuffer; + size_t pathSize; + size_t querySize; + size_t size; + + assert(pPath); + + if (!pQuery) + { + pQuery = ""; + } + + pathSize = strlen(pPath) + 1; + querySize = strlen(pQuery) + 1; + size = pathSize + querySize; + + pSendBuffer = malloc(size); + + if (pSendBuffer) + { + sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); + + sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); + errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); + assert(errorCode == SBG_NO_ERROR); + + for (uint32_t i = 0; i < pHandle->numTrials; i++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_GET, &pReply->payload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComCmdApiReplyParsePayload(pReply); + break; + } + } + else + { + break; + } + } + + free(pSendBuffer); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable allocate buffer"); + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdApiPost(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, const char *pBody, SbgEComCmdApiReply *pReply) +{ + SbgErrorCode errorCode; + uint8_t *pSendBuffer; + SbgStreamBuffer streamBuffer; + size_t pathSize; + size_t querySize; + size_t bodySize; + size_t size; + + assert(pPath); + + if (!pQuery) + { + pQuery = ""; + } + + if (!pBody) + { + pBody = ""; + } + + pathSize = strlen(pPath) + 1; + querySize = strlen(pQuery) + 1; + bodySize = strlen(pBody) + 1; + size = pathSize + querySize + bodySize; + + pSendBuffer = malloc(size); + + if (pSendBuffer) + { + sbgStreamBufferInitForWrite(&streamBuffer, pSendBuffer, size); + + sbgStreamBufferWriteBuffer(&streamBuffer, pPath, pathSize); + sbgStreamBufferWriteBuffer(&streamBuffer, pQuery, querySize); + errorCode = sbgStreamBufferWriteBuffer(&streamBuffer, pBody, bodySize); + assert(errorCode == SBG_NO_ERROR); + + for (uint32_t i = 0; i < pHandle->numTrials; i++) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_API_POST, &pReply->payload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComCmdApiReplyParsePayload(pReply); + break; + } + } + else + { + break; + } + } + + free(pSendBuffer); + } + else + { + errorCode = SBG_MALLOC_FAILED; + SBG_LOG_ERROR(errorCode, "unable allocate buffer"); + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.h new file mode 100644 index 0000000..ddc697d --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdApi.h @@ -0,0 +1,122 @@ +/*! + * \file sbgEComCmdApi.h + * \ingroup commands + * \author SBG Systems + * \date October 14, 2020 + * + * \brief REST API related commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_API_H +#define SBG_ECOM_CMD_API_H + +// sbgCommonLib headers +#include + +// Project headers +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Reply to REST API commands. + * + * The reply content is a null-terminated string, normally in JSON format. + * + * The content directly refers to data inside the payload. + */ +typedef struct _SbgEComCmdApiReply +{ + SbgEComProtocolPayload payload; /*!< Payload. */ + uint16_t statusCode; /*!< Status code. */ + const char *pContent; /*!< Content. */ +} SbgEComCmdApiReply; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * REST API reply constructor. + * + * \param[in] pReply REST API reply. + */ +void sbgEComCmdApiReplyConstruct(SbgEComCmdApiReply *pReply); + +/*! + * REST API reply destructor. + * + * \param[in] pReply REST API reply. + */ +void sbgEComCmdApiReplyDestroy(SbgEComCmdApiReply *pReply); + +/*! + * Check if a reply indicates successful command execution. + * + * \param[in] pReply REST API reply. + * \return True if the reply indicates successful command execution. + */ +bool sbgEComCmdApiReplySuccessful(const SbgEComCmdApiReply *pReply); + +/*! + * Send a GET command. + * + * The reply must be destroyed before the next attempt to receive data, either logs or command replies. + * + * \param[in] pHandle ECom handle. + * \param[in] pPath URI path component. + * \param[in] pQuery Query string, may be NULL. + * \param[out] pReply Reply. + * \return SBG_NO_ERROR if successful. + */ +SbgErrorCode sbgEComCmdApiGet(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, SbgEComCmdApiReply *pReply); + +/*! + * Send a POST command. + * + * The reply must be destroyed before the next attempt to receive data, either logs or command replies. + * + * \param[in] pHandle ECom handle. + * \param[in] pPath URI path component. + * \param[in] pQuery Query string, may be NULL. + * \param[in] pBody Body, may be NULL. + * \param[out] pReply Reply. + * \return SBG_NO_ERROR if successful. + */ +SbgErrorCode sbgEComCmdApiPost(SbgEComHandle *pHandle, const char *pPath, const char *pQuery, const char *pBody, SbgEComCmdApiReply *pReply); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_API_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.c new file mode 100644 index 0000000..99d09c7 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.c @@ -0,0 +1,504 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +//----------------------------------------------------------------------// +//- Common command reception operations -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) +{ + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; + + assert(pHandle); + assert(pMsgClass); + assert(pMsgId); + + sbgEComProtocolPayloadConstruct(&payload); + + errorCode = sbgEComReceiveAnyCmd2(pHandle, pMsgClass, pMsgId, &payload, timeOut); + + if (errorCode == SBG_NO_ERROR) + { + size_t size; + + size = sbgEComProtocolPayloadGetSize(&payload); + + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; + + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + + memcpy(pData, pBuffer, size); + } + + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } + + sbgEComProtocolPayloadDestroy(&payload); + + return errorCode; +} + +SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut) +{ + SbgErrorCode errorCode; + uint32_t start; + + assert(pHandle); + + if (timeOut > 0) + { + start = sbgGetTime(); + } + else + { + // + // Avoid compiler warning + // + start = 0; + } + + for (;;) + { + uint8_t receivedMsgClass; + uint8_t receivedMsgId; + uint32_t now; + + errorCode = sbgEComProtocolReceive2(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsgId, pPayload); + + if (errorCode == SBG_NO_ERROR) + { + if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) + { + if (pHandle->pReceiveLogCallback) + { + SbgBinaryLogData logData; + + errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, receivedMsgId, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload), &logData); + + if (errorCode == SBG_NO_ERROR) + { + pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsgId, &logData, pHandle->pUserArg); + + sbgEComBinaryLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsgId); + } + } + } + else + { + if (pMsgClass) + { + *pMsgClass = receivedMsgClass; + } + + if (pMsgId) + { + *pMsgId = receivedMsgId; + } + + break; + } + } + + if (timeOut > 0) + { + if (errorCode == SBG_NOT_READY) + { + sbgSleep(1); + } + + now = sbgGetTime(); + + if ((now - start) >= timeOut) + { + errorCode = SBG_TIME_OUT; + break; + } + } + else + { + errorCode = SBG_NOT_READY; + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut) +{ + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; + + sbgEComProtocolPayloadConstruct(&payload); + + errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msgId, &payload, timeOut); + + if (errorCode == SBG_NO_ERROR) + { + size_t size; + + size = sbgEComProtocolPayloadGetSize(&payload); + + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; + + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + + memcpy(pData, pBuffer, size); + } + + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } + + sbgEComProtocolPayloadDestroy(&payload); + + return errorCode; +} + +SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut) +{ + SbgErrorCode errorCode; + uint32_t start; + + assert(pHandle); + + start = sbgGetTime(); + + for (;;) + { + uint8_t receivedMsgClass; + uint8_t receivedMsgId; + uint32_t now; + + errorCode = sbgEComReceiveAnyCmd2(pHandle, &receivedMsgClass, &receivedMsgId, pPayload, 0); + + if (errorCode == SBG_NO_ERROR) + { + if ((receivedMsgClass == msgClass) && (receivedMsgId == msgId)) + { + break; + } + else if ((receivedMsgClass == SBG_ECOM_CLASS_LOG_CMD_0) && (receivedMsgId == SBG_ECOM_CMD_ACK)) + { + SbgStreamBuffer streamBuffer; + uint8_t ackMsgClass; + uint8_t ackMsgId; + SbgErrorCode ackErrorCode; + + sbgStreamBufferInitForRead(&streamBuffer, sbgEComProtocolPayloadGetBuffer(pPayload), sbgEComProtocolPayloadGetSize(pPayload)); + + ackMsgId = sbgStreamBufferReadUint8(&streamBuffer); + ackMsgClass = sbgStreamBufferReadUint8(&streamBuffer); + ackErrorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if ((errorCode == SBG_NO_ERROR) && (ackMsgClass == msgClass) && (ackMsgId == msgId)) + { + // + // If a successful ACK is expected, the caller should instead explicitely wait for + // it. As a result, consider receiving a "successful ACK" instead of an actual message + // with the requested class/ID an error. + // + if (ackErrorCode != SBG_NO_ERROR) + { + errorCode = ackErrorCode; + } + else + { + errorCode = SBG_ERROR; + } + + break; + } + } + } + else if (errorCode == SBG_NOT_READY) + { + sbgSleep(1); + } + else + { + break; + } + + now = sbgGetTime(); + + if ((now - start) >= timeOut) + { + errorCode = SBG_TIME_OUT; + break; + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- ACK related commands operations -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint8_t ackClass; + uint8_t ackMsg; + + assert(pHandle); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Try to receive the ACK + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, &receivedPayload, timeOut); + + // + // Test if an ACK frame has been received + // + if (errorCode == SBG_NO_ERROR) + { + // + // Validate the received ACK frame + // + if (sbgEComProtocolPayloadGetSize(&receivedPayload) == 2*sizeof(uint16_t)) + { + SbgStreamBuffer inputStream; + + // + // Initialize a stream buffer to parse the received payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // The ACK frame contains the ack message ID and class, and a uint16_t for the return error code + // We make sure that the ACK is for the correct command + // + ackMsg = sbgStreamBufferReadUint8LE(&inputStream); + ackClass = sbgStreamBufferReadUint8LE(&inputStream); + + if ((ackMsg == msg) && (ackClass == msgClass)) + { + // + // Parse the error code and return it + // + errorCode = (SbgErrorCode)sbgStreamBufferReadUint16LE(&inputStream); + } + else + { + // + // We have received an ACK but not for this frame! + // + errorCode = SBG_INVALID_FRAME; + } + } + else + { + // + // The ACK is invalid + // + errorCode = SBG_INVALID_FRAME; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgErrorCode cmdError) +{ + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2*sizeof(uint8_t)+sizeof(uint16_t)]; + + assert(pHandle); + + // + // Initialize a stream buffer to write the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Write the message ID and class and then the error code + // + sbgStreamBufferWriteUint8LE(&outputStream, msg); + sbgStreamBufferWriteUint8LE(&outputStream, msgClass); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)cmdError); + + // + // Send the ACK command + // + return sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ACK, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); +} + +//----------------------------------------------------------------------// +//- Generic command definitions -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t modelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[sizeof(uint32_t)]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Init stream buffer for output and Build payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint32LE(&outputStream, modelId); + + // + // Make sure the payload has been build correctly + // + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdGenericGetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t *pModelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pModelId); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a the specified command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + *pModelId = sbgStreamBufferReadUint32LE(&inputStream); + + // + // The command has been executed successfully so return + // We return the stream buffer error code to catch any overflow error on the payload + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.h new file mode 100644 index 0000000..422265b --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdCommon.h @@ -0,0 +1,229 @@ +/*! + * \file sbgEComCmdCommon.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Definitions and methods common to all commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_COMMON_H +#define SBG_ECOM_CMD_COMMON_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Defintions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_DEFAULT_CMD_TIME_OUT (500) /*!< Default time out in ms for commands reception. */ + +/*! + * List of all rejection modes for aiding inputs. + */ +typedef enum _SbgEComRejectionMode +{ + SBG_ECOM_NEVER_ACCEPT_MODE = 0, /*!< Measurement is not taken into account. */ + SBG_ECOM_AUTOMATIC_MODE = 1, /*!< Measurement is accepted and rejected automatically depending on consistency checks */ + SBG_ECOM_ALWAYS_ACCEPT_MODE = 2 /*!< Measurement is always accepted. Should be used with caution */ +} SbgEComRejectionMode; + +/*! + * List of all axis directions for modules/sensor alignment. + */ +typedef enum _SbgEComAxisDirection +{ + SBG_ECOM_ALIGNMENT_FORWARD = 0, /*!< IMU/module Axis is turned in vehicle's forward direction. */ + SBG_ECOM_ALIGNMENT_BACKWARD = 1, /*!< IMU/module Axis is turned in vehicle's backward direction. */ + SBG_ECOM_ALIGNMENT_LEFT = 2, /*!< IMU/module Axis is turned in vehicle's left direction. */ + SBG_ECOM_ALIGNMENT_RIGHT = 3, /*!< IMU/module Axis is turned in vehicle's right direction. */ + SBG_ECOM_ALIGNMENT_UP = 4, /*!< IMU/module Axis is turned in vehicle's up direction. */ + SBG_ECOM_ALIGNMENT_DOWN = 5 /*!< IMU/module Axis is turned in vehicle's down direction. */ +} SbgEComAxisDirection; + +//----------------------------------------------------------------------// +//- Common command reception operations -// +//----------------------------------------------------------------------// + +/*! + * Receive a command message. + * + * All binary logs received are handled trough the standard callback system. + * + * \param[in] pHandle SbgECom handle. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pData Data buffer, may be NULL. + * \param[out] pSize Number of bytes received, in bytes, may be NULL. + * \param[in] maxSize Data buffer size, in bytes. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received, + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, + * SBG_TIME_OUT if no command message was received within the specified time out. + */ +SbgErrorCode sbgEComReceiveAnyCmd(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); + +/*! + * Receive a command message. + * + * All binary logs received are handled trough the standard callback system. + * + * This function is equivalent to sbgEComReceiveAnyCmd() with two exceptions : + * - the use of a payload object allows handling payloads not limited by the size of a user-provided buffer + * - the payload object allows direct access to the protocol work buffer to avoid an extra copy per call + * + * Any allocated resource associated with the given payload is released when calling this function. + * + * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until + * the next attempt to receive a frame, with any of the receive functions. + * + * \param[in] pHandle SbgECom handle. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pPayload Payload. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received, + * SBG_TIME_OUT if no command message was received within the specified time out. + */ +SbgErrorCode sbgEComReceiveAnyCmd2(SbgEComHandle *pHandle, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut); + +/*! + * Receive a specific command message. + * + * This function also processes ACK messages for the given class and ID. + * + * All binary logs received during this time are handled trough the standard callback system. + * + * \param[in] pHandle SbgECom handle. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[out] pData Data buffer. + * \param[out] pSize Number of bytes received, in bytes. + * \param[in] maxSize Data buffer size, in bytes. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received, + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the buffer, + * SBG_TIME_OUT if no command message was received within the specified time out, + * any error code reported by an ACK message for the given class and ID. + */ +SbgErrorCode sbgEComReceiveCmd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, void *pData, size_t *pSize, size_t maxSize, uint32_t timeOut); + +/*! + * Receive a specific command message. + * + * This function also processes ACK messages for the given class and ID. + * + * All binary logs received during this time are handled trough the standard callback system. + * + * This function is equivalent to sbgEComReceiveCmd() with two exceptions : + * - the use of a payload object allows handling payloads not limited by the size of a user-provided buffer + * - the payload object allows direct access to the protocol work buffer to avoid an extra copy per call + * + * Any allocated resource associated with the given payload is released when calling this function. + * + * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until + * the next attempt to receive a frame, with any of the receive functions. + * + * \param[in] pHandle SbgECom handle. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[out] pPayload Payload. + * \param[in] timeOut Time-out, in ms. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no command message has been received, + * SBG_TIME_OUT if no command message was received within the specified time out, + * any error code reported by an ACK message for the given class and ID. + */ +SbgErrorCode sbgEComReceiveCmd2(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msgId, SbgEComProtocolPayload *pPayload, uint32_t timeOut); + +//----------------------------------------------------------------------// +//- ACK related commands operations -// +//----------------------------------------------------------------------// + +/*! + * Wait for an ACK for a specified amount of time. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] msgClass The message class that we want to check + * \param[in] msg The message ID that we want to check + * \param[in] timeOut Time out in ms during which we can receive the ACK. + * \return SBG_NO_ERROR if the ACK has been received. + */ +SbgErrorCode sbgEComWaitForAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t timeOut); + +/*! + * Send an ACK for a specific command with an associated error code. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] msgClass The message class that we want to send + * \param[in] msg The message ID that we want to send. + * \param[in] cmdError The associated error code. + * \return SBG_NO_ERROR if the ACK has been sent. + */ +SbgErrorCode sbgEComSendAck(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, SbgErrorCode cmdError); + +//----------------------------------------------------------------------// +//- Generic command definitions -// +//----------------------------------------------------------------------// + +/*! + * Generic function to set an error model ID + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] msgClass Original message class + * \param[in] msg Original message ID + * \param[in] modelId Model ID to set + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGenericSetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t modelId); + +/*! + * Generic function to get an error model ID + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] msgClass Original message class + * \param[in] msg Original message ID + * \param[out] pModelId Returns the currently used model ID. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGenericGetModelId(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, uint32_t *pModelId); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_COMMON_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.c new file mode 100644 index 0000000..2972dda --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.c @@ -0,0 +1,334 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdDvl.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdDvlSetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds modelId) +{ + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, modelId); +} + +SbgErrorCode sbgEComCmdDvlGetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds *pModelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; + + assert(pHandle); + assert(pModelId); + + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_MODEL_ID, &modelIdAsUint); + + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComDvlModelsIds)modelIdAsUint; + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdDvlInstallationSet(SbgEComHandle *pHandle, const SbgEComDvlInstallation *pDvlInstallation) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pDvlInstallation); + + // + // Create the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->leverArm[2]); + + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pDvlInstallation->alignment[2]); + + sbgStreamBufferWriteBooleanLE(&outputStream, pDvlInstallation->preciseInstallation); + + // + // Make sure the payload has been build correctly + // + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdDvlInstallationGet(SbgEComHandle *pHandle, SbgEComDvlInstallation *pDvlInstallation) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pDvlInstallation); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_INSTALLATION, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse the payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + pDvlInstallation->leverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pDvlInstallation->leverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pDvlInstallation->leverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + pDvlInstallation->alignment[0] = sbgStreamBufferReadFloatLE(&inputStream); + pDvlInstallation->alignment[1] = sbgStreamBufferReadFloatLE(&inputStream); + pDvlInstallation->alignment[2] = sbgStreamBufferReadFloatLE(&inputStream); + + pDvlInstallation->preciseInstallation = sbgStreamBufferReadBooleanLE(&inputStream); + + // + // The command has been executed successfully so return if an error has occurred during payload parsing + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdDvlSetRejection(SbgEComHandle *pHandle, const SbgEComDvlRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[2*sizeof(uint8_t)]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + // + // Create the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->bottomLayer); + sbgStreamBufferWriteUint8LE(&outputStream, pRejectConf->waterLayer); + + // + // Make sure the payload has been build correctly + // + errorCode = sbgStreamBufferGetLastError(&outputStream); + + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdDvlGetRejection(SbgEComHandle *pHandle, SbgEComDvlRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_DVL_REJECT_MODES, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to parse payload + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the payload + // + pRejectConf->bottomLayer = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + pRejectConf->waterLayer = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return if an error has occurred during payload parsing + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.h new file mode 100644 index 0000000..1283404 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdDvl.h @@ -0,0 +1,143 @@ +/*! + * \file sbgEComCmdDvl.h + * \ingroup commands + * \author SBG Systems + * \date 13 December 2018 + * + * \brief DVL (Doppler Velocity Logger) aiding module configuration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_DVL_H +#define SBG_ECOM_CMD_DVL_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * This enum defines the different DVL model IDs available in standard + */ +typedef enum _SbgEComDvlModelsIds +{ + SBG_ECOM_DVL_MODEL_GENERIC_PD6 = 202, /*!< Generic DVL using PD6 protocol format. */ + SBG_ECOM_DVL_MODEL_WAYFINDER = 203 /*!< Teledyne Wayfinder DVL using proprietary protocol. */ +} SbgEComDvlModelsIds; + +/*! + * DVL mechanical installation parameters such as lever arm and alignment + */ +typedef struct _SbgEComDvlInstallation +{ + float leverArm[3]; /*!< X, Y, Z DVL lever arm in meters expressed from the DVL to the IMU. */ + float alignment[3]; /*!< Roll, pitch, yaw DVL alignment expressed in radians. */ + bool preciseInstallation; /*!< Set to true if both the DVL lever arm and DVL alignment are precise and don't require in-run estimation. */ +} SbgEComDvlInstallation; + +/*! + * Holds all necessary information for DVL module data rejection. + */ +typedef struct _SbgEComDvlRejectionConf +{ + SbgEComRejectionMode bottomLayer; /*!< Rejection mode for the bottom tracking (ie when the velocity measurement is in respect to the seabed). */ + SbgEComRejectionMode waterLayer; /*!< Rejection mode for the water tracking (ie when the velocity measurement is relative to a water layer). */ +} SbgEComDvlRejectionConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Set the DVL model to use that both defines the protocol as well as the associated error model. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId DVL model ID to set + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlSetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds modelId); + +/*! + * Retrieve the DVL model id currently in use by the device. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Returns the DVL model ID currently in use by the device. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlGetModelId(SbgEComHandle *pHandle, SbgEComDvlModelsIds *pModelId); + +/*! + * Set the lever arm and alignment configuration of the DVL module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pDvlInstallation The DVL lever arm and alignment configuration to apply. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlInstallationSet(SbgEComHandle *pHandle, const SbgEComDvlInstallation *pDvlInstallation); + +/*! + * Retrieve the lever arm and alignment configuration of the DVL module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pDvlInstallation Returns the DVL lever arm and alignment configuration currently in use. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlInstallationGet(SbgEComHandle *pHandle, SbgEComDvlInstallation *pDvlInstallation); + +/*! + * Set the rejection configuration of the DVL module (this command doesn't need a reboot to be applied) + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf The new DVL rejection configuration to set. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlSetRejection(SbgEComHandle *pHandle, const SbgEComDvlRejectionConf *pRejectConf); + +/*! + * Retrieve the current rejection configuration of the DVL module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Return the DVL rejection configuration currently in use. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdDvlGetRejection(SbgEComHandle *pHandle, SbgEComDvlRejectionConf *pRejectConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_DVL_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.c new file mode 100644 index 0000000..5fd9f40 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.c @@ -0,0 +1,276 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdEthernet.h" + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Write in the output stream buffer, the provided Ethernet configuration. + * + * \param[out] pOutputStream Pointer on the output stream buffer to write to. + * \param[in] pEthernetConf Structure used to hold the parameters to write to the payload buffer. + * \return SBG_NO_ERROR if the structure has been written correctly. + */ +static SbgErrorCode sbgEComEthernetConfWrite(SbgStreamBuffer *pOutputStream, const SbgEComEthernetConf *pEthernetConf) +{ + assert(pOutputStream); + assert(pEthernetConf); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(pOutputStream, (uint8_t)pEthernetConf->mode); + sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->ipAddress); + sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->netmask); + sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->gateway); + sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->dns1); + sbgStreamBufferWriteUint32LE(pOutputStream, pEthernetConf->dns2); + + // + // Return if an error has occurred during the parse + // + return sbgStreamBufferGetLastError(pOutputStream); +} + +/*! + * Parse the input stream buffer to extract all parameters and fill the corresponding structure. + * + * \param[in] pInputStream Pointer on the input stream buffer to read from. + * \param[out] pEthernetConf Structure used to store the parsed parameters. + * \return SBG_NO_ERROR if the structure has been parsed correctly. + */ +static SbgErrorCode sbgEComEthernetConfParse(SbgStreamBuffer *pInputStream, SbgEComEthernetConf *pEthernetConf) +{ + assert(pInputStream); + assert(pEthernetConf); + + // + // Read all parameters from the payload + // + pEthernetConf->mode = (SbgEComEthernetMode)sbgStreamBufferReadUint8LE(pInputStream); + pEthernetConf->ipAddress = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); + pEthernetConf->netmask = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); + pEthernetConf->gateway = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); + pEthernetConf->dns1 = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); + pEthernetConf->dns2 = (sbgIpAddress)sbgStreamBufferReadUint32LE(pInputStream); + + // + // Return if an error has occurred during the parse + // + return sbgStreamBufferGetLastError(pInputStream); +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pEthernetConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with no payload to retreive the network configuration + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read all parameters from the payload and return any error during the parse + // + errorCode = sbgEComEthernetConfParse(&inputStream, pEthernetConf); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pEthernetConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + errorCode = sbgEComEthernetConfWrite(&outputStream, pEthernetConf); + + // + // Send the payload if no error has occurred + // + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + } + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pEthernetConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with no payload to retreive the network configuration + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ETHERNET_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read all parameters from the payload and return any error during the parse + // + errorCode = sbgEComEthernetConfParse(&inputStream, pEthernetConf); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.h new file mode 100644 index 0000000..b0b98cc --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEthernet.h @@ -0,0 +1,113 @@ +/*! + * \file sbgEComCmdEthernet.h + * \ingroup commands + * \author SBG Systems + * \date 14 November 2016 + * + * \brief Ethernet configuration related commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_ETHERNET_H +#define SBG_ECOM_CMD_ETHERNET_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Ethernet interface configuration -// +//----------------------------------------------------------------------// + +/*! + * Enum that defines the different type of IP acquisition method. + */ +typedef enum _SbgEComEthernetMode +{ + SBG_ECOM_ETHERNET_DHCP = 0, /*!< The TCP/IP configuration should be acquired from a DHCP server. */ + SBG_ECOM_ETHERNET_STATIC = 1 /*!< The TCP/IP configuration is manually defined. */ +} SbgEComEthernetMode; + +/*! + * Structure that contains all Ethernet configuration or settings. + */ +typedef struct _SbgEComEthernetConf +{ + SbgEComEthernetMode mode; /*!< Define how the device will acquiere its IP address, either DHCP or Static. */ + sbgIpAddress ipAddress; /*!< For static mode, defines the device IP address. */ + sbgIpAddress netmask; /*!< For static mode, defines the device net mask. */ + sbgIpAddress gateway; /*!< For static mode, defines the gateway to use. */ + sbgIpAddress dns1; /*!< For static mode, defines the primary DNS to use. */ + sbgIpAddress dns2; /*!< For static mode, defines the secondary DNS to use. */ +} SbgEComEthernetConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Get the configuration for the Ethernet interface. + * + * Warning: this method only returns the Ethernet configuration and NOT the ip address currently used by the device. + * You should rather use sbgEComEthernetInfo to retreive the current assigned IP. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read configuration from the device. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComEthernetGetConf(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf); + +/*! + * Set the configuration for the Ethernet interface. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the new configuration to apply. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComEthernetSetConf(SbgEComHandle *pHandle, const SbgEComEthernetConf *pEthernetConf); + +/*! + * Get the current assigned and used IP address as well as network inforamtion. + * + * In opposition to sbgEComEthernetGetConf, this method will not return the Ethernet configuration. + * It will rather return the IP address currently used by the device. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pEthernetConf Poiner to a SbgEComEthernetConf struct that holds the read IP settings from the device. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComEthernetInfo(SbgEComHandle *pHandle, SbgEComEthernetConf *pEthernetConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_ETHERNET_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.c new file mode 100644 index 0000000..0766cdb --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.c @@ -0,0 +1,294 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdEvent.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdSyncInGetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, SbgEComSyncInConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t syncInIdParam = syncInId; + + // + // Send the command with syncInId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &syncInIdParam, sizeof(syncInIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_SYNC_IN_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // First is returned the id of the sync, then the sensitivity and the delay at last. + // + syncInId = (SbgEComSyncInId)sbgStreamBufferReadUint8LE(&inputStream); + pConf->sensitivity = (SbgEComSyncInSensitivity)sbgStreamBufferReadUint8LE(&inputStream); + pConf->delay = sbgStreamBufferReadInt32LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, const SbgEComSyncInConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)syncInId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->sensitivity); + sbgStreamBufferWriteInt32LE(&outputStream, pConf->delay); + + // + // Send the message over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_IN_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdSyncOutGetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, SbgEComSyncOutConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t syncOutIdParam = syncOutId; + + // + // Send the command with syncOutId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &syncOutIdParam, sizeof(syncOutIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_SYNC_OUT_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // First is returned the id of the sync, then a reserved field, the output function, polarity and the duration at last. + // + syncOutId = (SbgEComSyncOutId)sbgStreamBufferReadUint8LE(&inputStream); + sbgStreamBufferReadUint8LE(&inputStream); + pConf->outputFunction = (SbgEComSyncOutFunction)sbgStreamBufferReadUint16LE(&inputStream); + pConf->polarity = (SbgEComSyncOutPolarity)sbgStreamBufferReadUint8LE(&inputStream); + pConf->duration = sbgStreamBufferReadUint32LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, const SbgEComSyncOutConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)syncOutId); + sbgStreamBufferWriteUint8LE(&outputStream, 0); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)pConf->outputFunction); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->polarity); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->duration); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SYNC_OUT_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.h new file mode 100644 index 0000000..240ff47 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdEvent.h @@ -0,0 +1,188 @@ +/*! + * \file sbgEComCmdEvent.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Input/output event markers configuration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_EVENT_H +#define SBG_ECOM_CMD_EVENT_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Event definitions -// +//----------------------------------------------------------------------// + +/*! + * List of sync in signals available. + */ +typedef enum _SbgEComSyncInId +{ + SBG_ECOM_SYNC_IN_A = 0, /*!< Sync IN A */ + SBG_ECOM_SYNC_IN_B = 1, /*!< Sync IN B */ + SBG_ECOM_SYNC_IN_C = 2, /*!< Sync IN C */ + SBG_ECOM_SYNC_IN_D = 3 /*!< Sync IN D */ +} SbgEComSyncInId; + +/*! + * List of available sensitivities for sync in signals. + */ +typedef enum _SbgEComSyncInSensitivity +{ + SBG_ECOM_SYNC_IN_DISABLED = 0, /*!< This trigger is turned OFF. */ + SBG_ECOM_SYNC_IN_FALLING_EDGE = 1, /*!< The trigger will be activated by a falling edge. */ + SBG_ECOM_SYNC_IN_RISING_EDGE = 2, /*!< The trigger will be activated by a rising edge. */ + SBG_ECOM_SYNC_IN_BOTH_EDGES = 3 /*!< The trigger is activated by a level change (rising or falling edge). */ +} SbgEComSyncInSensitivity; + +/*! + * List of sync out signals available. + */ +typedef enum _SbgEComSyncOutId +{ + SBG_ECOM_SYNC_OUT_A = 0, /*!< Synchronization output A */ + SBG_ECOM_SYNC_OUT_B = 1 /*!< Synchronization output B */ +} SbgEComSyncOutId; + +/*! + * Logic and synchronization output types + */ +typedef enum _SbgEComSyncOutFunction +{ + SBG_ECOM_SYNC_OUT_MODE_DISABLED = 0, /*!< Output is disabled. */ + SBG_ECOM_SYNC_OUT_MODE_MAIN_LOOP = 1, /*!< Output is generated at 200Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_2 = 2, /*!< Output is generated at 100Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_4 = 4, /*!< Output is generated at 50Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_8 = 8, /*!< Output is generated at 25Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_10 = 10, /*!< Output is generated at 20Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_20 = 20, /*!< Output is generated at 10Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_40 = 40, /*!< Output is generated at 5Hz. */ + SBG_ECOM_SYNC_OUT_MODE_DIV_200 = 200, /*!< Output is generated at 1Hz. */ + SBG_ECOM_SYNC_OUT_MODE_PPS = 10000, /*!< Pulse Per Second. Same mode as above. */ + SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_A = 10003, /*!< Output is generated on a Sync In A event. */ + SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_B = 10004, /*!< Output is generated on a Sync In B event. */ + SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_C = 10005, /*!< Output is generated on a Sync In C event. */ + SBG_ECOM_SYNC_OUT_MODE_EVENT_IN_D = 10006, /*!< Output is generated on a Sync In D event. */ + + SBG_ECOM_SYNC_OUT_MODE_DIRECT_PPS = 10100, /*!< The internal GNSS PPS signal is directly routed to the Sync Out. + This mode is only valid for ELLIPSE-N with hardware revisions above 1.2.1.0. + Polarity and duration parameters are ignored with this specific mode. */ + +} SbgEComSyncOutFunction; + +/*! + * Logic output polarity + */ +typedef enum _SbgEComSyncOutPolarity +{ + SBG_ECOM_SYNC_OUT_FALLING_EDGE = 0, /*!< The output pin will generate a falling edge*/ + SBG_ECOM_SYNC_OUT_RISING_EDGE = 1, /*!< The output pin will generate a rising edge */ + SBG_ECOM_SYNC_OUT_TOGGLE = 2, /*!< The pulse is a level change */ +} SbgEComSyncOutPolarity; + +//----------------------------------------------------------------------// +//- Event configurations -// +//----------------------------------------------------------------------// + +/*! + * Helper structure for sync in configuration. + */ +typedef struct _SbgEComSyncInConf +{ + SbgEComSyncInSensitivity sensitivity; /*!< Sensitivity of the sync in. */ + int32_t delay; /*!< Delay to take into account for the sync in. (in us)*/ +} SbgEComSyncInConf; + +/*! + * Helper structure for sync out configuration. + */ +typedef struct _SbgEComSyncOutConf +{ + SbgEComSyncOutFunction outputFunction; /*!< Output function of the sync out pin */ + SbgEComSyncOutPolarity polarity; /*!< Polarity of the sync out. */ + uint32_t duration; /*!< Pulse width for the sync out (in ns). */ +} SbgEComSyncOutConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the configuration of a Sync In. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] syncInId The id of the sync whose configuration is to be retrieved. + * \param[out] pConf Pointer to a SbgEComSyncInConf to contain the current configuration of the sync in. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSyncInGetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, SbgEComSyncInConf *pConf); + +/*! + * Set the configuration of a Sync In. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] syncInId The id of the sync whose configuration is to be set. + * \param[in] pConf Pointer to a SbgEComSyncInConf that contains the new configuration for the sync in. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSyncInSetConf(SbgEComHandle *pHandle, SbgEComSyncInId syncInId, const SbgEComSyncInConf *pConf); + +/*! + * Retrieve the configuration of a Sync Out. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] syncOutId The id of the sync whose configuration is to be retrieved. + * \param[out] pConf Pointer to a SbgEComSyncOutConf to contain the current configuration of the sync out. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSyncOutGetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, SbgEComSyncOutConf *pConf); + +/*! + * Set the configuration of a Sync Out. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] syncOutId The id of the sync whose configuration is to be set. + * \param[in] pConf Pointer to a SbgEComSyncOutConf that contains the new configuration for the sync Out. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSyncOutSetConf(SbgEComHandle *pHandle, SbgEComSyncOutId syncOutId, const SbgEComSyncOutConf *pConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_EVENT_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.c new file mode 100644 index 0000000..efec8d3 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.c @@ -0,0 +1,100 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdFeatures.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdGetFeatures(SbgEComHandle *pHandle, SbgEComFeatures *pFeatures) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pFeatures); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_FEATURES, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GPS_FEATURES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pFeatures->sensorFeaturesMask = sbgStreamBufferReadUint32LE(&inputStream); + pFeatures->gnssType = (SbgEComGnssType)sbgStreamBufferReadUint8LE(&inputStream); + pFeatures->gnssUpdateRate = sbgStreamBufferReadUint8LE(&inputStream); + pFeatures->gnssSignalsMask = sbgStreamBufferReadUint32LE(&inputStream); + pFeatures->gnssFeaturesMask = sbgStreamBufferReadUint32LE(&inputStream); + sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssProductCode, 32*sizeof(char)); + sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssSerialNumber, 32*sizeof(char)); + + // + // Only parse the GNSS firmware version if available + // + if (sbgStreamBufferGetSpace(&inputStream) > 0) + { + sbgStreamBufferReadBuffer(&inputStream, pFeatures->gnssFirmwareVersion, 32 * sizeof(char)); + } + else + { + strcpy(pFeatures->gnssFirmwareVersion, ""); + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.h new file mode 100644 index 0000000..080370e --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdFeatures.h @@ -0,0 +1,140 @@ +/*! + * \file sbgEComCmdFeatures.h + * \ingroup commands + * \author SBG Systems + * \date 19 March 2015 + * + * \brief Commands used to query supported device features. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_FEATURES_H +#define SBG_ECOM_CMD_FEATURES_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Feature commands definitions -// +//----------------------------------------------------------------------// + +// +// Main sensor features +// +#define SBG_ECOM_SENSOR_FEATURE_IMU (0x00000001 << 0) /*!< This unit can provide IMU data */ +#define SBG_ECOM_SENSOR_FEATURE_AHRS (0x00000001 << 1) /*!< This unit can provide orientation data */ +#define SBG_ECOM_SENSOR_FEATURE_NAVIGATION (0x00000001 << 2) /*!< This unit can provide position and velocity data */ +#define SBG_ECOM_SENSOR_FEATURE_SHIP_MOTION (0x00000001 << 3) /*!< This unit can provide ship motion data output (heave) */ + +// +// GPS Signals bitmask defining every signal +// +#define SBG_ECOM_GNSS_SIGNAL_GPS_L1 (0x00000001 << 0) /*!< This GNSS receiver tracks GPS L1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GPS_L2 (0x00000001 << 1) /*!< This GNSS receiver tracks GPS L2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GPS_L5 (0x00000001 << 2) /*!< This GNSS receiver tracks GPS L5 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L1 (0x00000001 << 3) /*!< This GNSS receiver tracks GLONASS L1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GLONASS_L2 (0x00000001 << 4) /*!< This GNSS receiver tracks GLONASS L2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B1 (0x00000001 << 5) /*!< This GNSS receiver tracks BEIDOU B1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B2 (0x00000001 << 6) /*!< This GNSS receiver tracks BEIDOU B2 band. */ +#define SBG_ECOM_GNSS_SIGNAL_BEIDOU_B3 (0x00000001 << 7) /*!< This GNSS receiver tracks BEIDOU B3 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E1 (0x00000001 << 8) /*!< This GNSS receiver tracks GALILEO E1 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E5 (0x00000001 << 9) /*!< This GNSS receiver tracks GALILEO E5 band. */ +#define SBG_ECOM_GNSS_SIGNAL_GALILEO_E6 (0x00000001 << 10) /*!< This GNSS receiver tracks GALILEO E6 band. */ +#define SBG_ECOM_GNSS_SIGNAL_QZSS (0x00000001 << 11) /*!< This GNSS receiver tracks QZSS signals */ +#define SBG_ECOM_GNSS_SIGNAL_SBAS (0x00000001 << 12) /*!< This GNSS receiver tracks SBAS signals */ +#define SBG_ECOM_GNSS_SIGNAL_L_BAND (0x00000001 << 13) /*!< This GNSS receiver tracks L-Band (for PPP services) */ + +// +// GPS capabilities +// +#define SBG_ECOM_GNSS_FEATURE_DUAL_ANT (0x00000001 << 0) /*!< This GNSS receiver provides a dual antenna heading */ +#define SBG_ECOM_GNSS_FEATURE_RTK_LIMITED (0x00000001 << 1) /*!< This GNSS receiver has limited RTK accuracy (eg. Trimble RTK 30/30) */ +#define SBG_ECOM_GNSS_FEATURE_RTK (0x00000001 << 2) /*!< This GNSS receiver provides full RTK accuracy */ +#define SBG_ECOM_GNSS_FEATURE_PPP (0x00000001 << 3) /*!< This GNSS receiver provides PPP computations */ +#define SBG_ECOM_GNSS_FEATURE_RAW_DATA (0x00000001 << 4) /*!< This GNSS receiver provides RAW data output */ +#define SBG_ECOM_GNSS_FEATURE_RAIM (0x00000001 << 5) /*!< This GNSS receiver provides Receiver Autonomous Integrity Monitoring */ +#define SBG_ECOM_GNSS_FEATURE_HIGH_SPEED (0x00000001 << 6) /*!< This GNSS receiver has no high speed limitation (> 515m/s) */ + +//----------------------------------------------------------------------// +//- Feature commands types definition -// +//----------------------------------------------------------------------// + +/*! + * This enum defines the different types of internal GNSS receiver that can provide specific features. + * Note External type is considered as not handled by the feature system + */ +typedef enum _SbgEComGnssType +{ + SBG_ECOM_GNSS_TYPE_DISABLED = 0, /*!< GNSS module disabled */ + SBG_ECOM_GNSS_TYPE_EXTERNAL = 1, /*!< External GNSS module (all features are unknown) */ + SBG_ECOM_GNSS_TYPE_UBX_MAX_M8 = 2, /*!< Ublox MAX-M8 module */ + SBG_ECOM_GNSS_TYPE_NOV_OEM615 = 3, /*!< Novatel OEM615 device */ + SBG_ECOM_GNSS_TYPE_NOV_OEM615_DUAL = 4, /*!< Two Novatel OEM615 devices for dual antenna */ + SBG_ECOM_GNSS_TYPE_NOV_OEM617D = 5, /*!< Novatel OEM617D device */ + SBG_ECOM_GNSS_TYPE_SEP_AX4 = 6, /*!< Septentrio Asterx m4 */ + SBG_ECOM_GNSS_TYPE_SEP_AXM2A = 7, /*!< Septentrio Asterx m2a */ + SBG_ECOM_GNSS_TYPE_UBX_F9P = 8, /*!< Ublox ZED-F9P module */ +} SbgEComGnssType; + +/*! + * This structure contains all the information provided by the SBG_ECOM_CMD_GET_FEATURES command + */ +typedef struct _SbgEComFeatures +{ + uint32_t sensorFeaturesMask; /*!< The different measurement capabilities of this unit */ + SbgEComGnssType gnssType; /*!< The type of GNSS receiver used (brand and model) */ + uint8_t gnssUpdateRate; /*!< The actual GNSS update rate */ + uint32_t gnssSignalsMask; /*!< GNSS receiver signals tracking */ + uint32_t gnssFeaturesMask; /*!< GNSS receiver computation and output features */ + char gnssProductCode[32]; /*!< String containing the GNSS receiver product code ("\0" if unknown) */ + char gnssSerialNumber[32]; /*!< String containing the GNSS receiver serial number ("\0" if unknown) */ + char gnssFirmwareVersion[32]; /*!< String containing the GNSS receiver firmware version ("\0" if unknown) */ +} SbgEComFeatures; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the device and embedded GPS receiver features. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pFeatures A pointer to a structure to hold features. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGetFeatures(SbgEComHandle *pHandle, SbgEComFeatures *pFeatures); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_FEATURES_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.c new file mode 100644 index 0000000..9dd1c8b --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.c @@ -0,0 +1,422 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdGnss.h" + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// + +/*! + * Set GNSS error model id. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] id Model ID to set + * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssSetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds modelId, SbgEComCmd cmdId) +{ + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, modelId); +} + +/*! + * Retrieve GNSS error model id. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retreived model id. + * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssGetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId, SbgEComCmd cmdId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; + + assert(pHandle); + assert(pModelId); + + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &modelIdAsUint); + + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComGnssModelsStdIds)modelIdAsUint; + } + + return errorCode; +} + +/*! + * Retrieve the mechanical installation parameters for the GNSS # module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pGnssInstallation Used to store the retrieved the GNSS installation parameters. + * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssInstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pGnssInstallation); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pGnssInstallation->leverArmPrimary[0] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmPrimary[1] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmPrimary[2] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmPrimaryPrecise = sbgStreamBufferReadBooleanLE(&inputStream); + + pGnssInstallation->leverArmSecondary[0] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmSecondary[1] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmSecondary[2] = sbgStreamBufferReadFloatLE(&inputStream); + pGnssInstallation->leverArmSecondaryMode = (SbgEComGnssInstallationMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +/*! + * Set the mechanical installation parameters for the GNSS # module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pGnssInstallation The GNSS installation parameters to set. + * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssInstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation, SbgEComCmd cmdId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pGnssInstallation); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Initialize stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmPrimary[2]); + sbgStreamBufferWriteBooleanLE(&outputStream, pGnssInstallation->leverArmPrimaryPrecise); + + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pGnssInstallation->leverArmSecondary[2]); + sbgStreamBufferWriteUint8LE(&outputStream, pGnssInstallation->leverArmSecondaryMode); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +/*! + * Retrieve the rejection configuration of the gnss module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct to hold rejection configuration of the gnss module. + * \param[in] cmdId The command identifier to get parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssGetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_GNSS_1_REJECT_MODES command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pRejectConf->position = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + sbgStreamBufferReadUint8LE(&inputStream); // Skipped for backward compatibility + pRejectConf->hdt = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +/*! + * Set the rejection configuration of the gnss module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pAlignConf Pointer to a SbgEComGnssRejectionConf struct holding rejection configuration for the gnss module. + * \param[in] cmdId The command identifier to set parameters for a specific GNSS module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +static SbgErrorCode sbgEComCmdGnssSetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf, SbgEComCmd cmdId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->position); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)SBG_ECOM_NEVER_ACCEPT_MODE); // Reserved parameter + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->hdt); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, cmdId, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds modelId) +{ + assert(pHandle); + + return sbgEComCmdGnssSetModelId(pHandle, modelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); +} + +SbgErrorCode sbgEComCmdGnss1GetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId) +{ + assert(pHandle); + assert(pModelId); + + return sbgEComCmdGnssGetModelId(pHandle, pModelId, SBG_ECOM_CMD_GNSS_1_MODEL_ID); +} + +SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation) +{ + assert(pHandle); + assert(pGnssInstallation); + + return sbgEComCmdGnssInstallationGet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); +} + +SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation) +{ + assert(pHandle); + assert(pGnssInstallation); + + return sbgEComCmdGnssInstallationSet(pHandle, pGnssInstallation, SBG_ECOM_CMD_GNSS_1_INSTALLATION); +} + +SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf) +{ + assert(pHandle); + assert(pRejectConf); + + return sbgEComCmdGnssGetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); +} + +SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf) +{ + assert(pHandle); + assert(pRejectConf); + + return sbgEComCmdGnssSetRejection(pHandle, pRejectConf, SBG_ECOM_CMD_GNSS_1_REJECT_MODES); +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.h new file mode 100644 index 0000000..e4bab66 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdGnss.h @@ -0,0 +1,165 @@ +/*! + * \file sbgEComCmdGnss.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief GNSS aiding module configuration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_GNSS_H +#define SBG_ECOM_CMD_GNSS_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * This enum defines the different GNSS model IDs available in standard + */ +typedef enum _SbgEComGnssModelsStdIds +{ + SBG_ECOM_GNSS_MODEL_INTERNAL = 101, /*!< Default internal GNSS for ELLIPSE-N and ELLIPSE-D */ + SBG_ECOM_GNSS_MODEL_NMEA = 102, /*!< ELLIPSE-E to accept an external GNSS using NMEA protocol */ + SBG_ECOM_GNSS_MODEL_UBLOX_GPS_BEIDOU = 103, /*!< Only for ELLIPSE-N hardware 1 & 2 to select GPS+BEIDOU instead of the default GPS+GLONASS */ + SBG_ECOM_GNSS_MODEL_UBLOX_EXTERNAL = 104, /*!< ELLIPSE-E to accept an external Ublox GNSS (receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_01 = 105, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_NOVATEL_EXTERNAL = 106, /*!< ELLIPSE-E to accept an external Novatel GNSS (receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_02 = 107, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_RESERVED_03 = 108, /*!< Reserved, do not use */ + SBG_ECOM_GNSS_MODEL_SEPTENTRIO_EXTERNAL = 109, /*!< ELLIPSE-E to accept an external Septentrio GNSS(receive only - passive) */ + SBG_ECOM_GNSS_MODEL_RESERVED_04 = 110 /*!< Reserved, do not use */ +} SbgEComGnssModelsStdIds; + +/*! + * GNSS mechanical installation modes for the dual antenna mode. + */ +typedef enum _SbgEComGnssInstallationMode +{ + SBG_ECOM_GNSS_INSTALLATION_MODE_SINGLE = 1, /*!< The GNSS will be used in single antenna mode only and the secondary lever arm is not used. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_AUTO = 2, /*!< [Reserved] The GNSS dual antenna information will be used but the secondary lever arm is not known. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_ROUGH = 3, /*!< The GNSS dual antenna information will be used and we have a rough guess for the secondary lever arm. */ + SBG_ECOM_GNSS_INSTALLATION_MODE_DUAL_PRECISE = 4 /*!< The GNSS dual antenna information will be used and the secondary lever arm is accurately entered and doesn't need online re-estimation. */ +} SbgEComGnssInstallationMode; + +/*! + * GNSS mechanical installation parameters to be used with command SBG_ECOM_CMD_GNSS_#_INSTALLATION + */ +typedef struct _SbgEComGnssInstallation +{ + float leverArmPrimary[3]; /*!< GNSS primary antenna lever arm in IMU X, Y, Z axis in meters */ + bool leverArmPrimaryPrecise; /*!< If set to true, the primary lever arm has been accurately entered and doesn't need online re-estimation. */ + + float leverArmSecondary[3]; /*!< GNSS secondary antenna lever arm in IMU X, Y, Z axis in meters */ + SbgEComGnssInstallationMode leverArmSecondaryMode; /*!< Define the secondary antenna (dual antenna) operating mode. */ +} SbgEComGnssInstallation; + +/*! + * Holds all necessary information for GNSS module data rejection. + */ +typedef struct _SbgEComGnssRejectionConf +{ + SbgEComRejectionMode position; /*!< Rejection mode for position. */ + SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ + SbgEComRejectionMode hdt; /*!< Rejection mode for true heading. */ +} SbgEComGnssRejectionConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Set GNSS error model id. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId Model ID to set + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds modelId); + +/*! + * Retrieve GNSS error model id. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retrieved model id. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1GetModelId(SbgEComHandle *pHandle, SbgEComGnssModelsStdIds *pModelId); + +/*! + * Retrieve the mechanical installation parameters for the GNSS 1 module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pGnssInstallation Used to store the retrieved the GNSS installation parameters. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation); + +/*! + * Set the mechanical installation parameters for the GNSS 1 module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pGnssInstallation The GNSS installation parameters to set. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation); + +/*! + * Retrieve the rejection configuration of the gnss module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Pointer to a SbgEComGnssRejectionConf struct to hold rejection configuration of the gnss module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf); + +/*! + * Set the rejection configuration of the gnss module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Pointer to a SbgEComGnssRejectionConf struct holding rejection configuration for the gnss module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_GNSS_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.c new file mode 100644 index 0000000..fb4598a --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.c @@ -0,0 +1,105 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdInfo.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + + assert(pHandle); + assert(pInfo); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INFO, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have correctly received a message + // + if (errorCode == SBG_NO_ERROR) + { + // + // Make sure we have received a payload + // + if (sbgEComProtocolPayloadGetSize(&receivedPayload) > 0) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + sbgStreamBufferReadBuffer(&inputStream, pInfo->productCode, SBG_ECOM_INFO_PRODUCT_CODE_LENGTH); + pInfo->serialNumber = sbgStreamBufferReadUint32LE(&inputStream); + pInfo->calibationRev = sbgStreamBufferReadUint32LE(&inputStream); + pInfo->calibrationYear = sbgStreamBufferReadUint16LE(&inputStream); + pInfo->calibrationMonth = sbgStreamBufferReadUint8LE(&inputStream); + pInfo->calibrationDay = sbgStreamBufferReadUint8LE(&inputStream); + pInfo->hardwareRev = sbgStreamBufferReadUint32LE(&inputStream); + pInfo->firmwareRev = sbgStreamBufferReadUint32LE(&inputStream); + + // + // We have parsed a message so return immediately but report any error during payload parsing + // + errorCode = sbgStreamBufferGetLastError(&inputStream); + + break; + } + else + { + // + // We should have received a non empty payload so we have received an invalid frame + // + errorCode = SBG_INVALID_FRAME; + } + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.h new file mode 100644 index 0000000..26abc4e --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInfo.h @@ -0,0 +1,89 @@ +/*! + * \file sbgEComCmdInfo.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Commands used to query the device information. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_INFO_H +#define SBG_ECOM_CMD_INFO_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Device info definitions -// +//----------------------------------------------------------------------// + +/* Misc */ +#define SBG_ECOM_INFO_PRODUCT_CODE_LENGTH (32) + +//----------------------------------------------------------------------// +//- Device Info structure -// +//----------------------------------------------------------------------// + +/*! + * Helper structure to retrieve device info. + */ +typedef struct _SbgEComDeviceInfo +{ + uint8_t productCode[SBG_ECOM_INFO_PRODUCT_CODE_LENGTH]; /*!< Human readable Product Code. */ + uint32_t serialNumber; /*!< Device serial number */ + uint32_t calibationRev; /*!< Calibration data revision */ + uint16_t calibrationYear; /*!< Device Calibration Year */ + uint8_t calibrationMonth; /*!< Device Calibration Month */ + uint8_t calibrationDay; /*!< Device Calibration Day */ + uint32_t hardwareRev; /*!< Device hardware revision */ + uint32_t firmwareRev; /*!< Firmware revision */ +} SbgEComDeviceInfo; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the device information. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pInfo A pointer to a structure to hold device information. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_INFO_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.c new file mode 100644 index 0000000..e0f10b9 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.c @@ -0,0 +1,301 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdInterface.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdInterfaceGetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, SbgEComInterfaceConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + uint8_t interfaceIdParam = interfaceId; + + // + // Send the command and the interfaceId as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &interfaceIdParam, sizeof(interfaceIdParam)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received correctly the answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // First is returned interfaceId, then baud rate and the mode at last. + // + interfaceId = (SbgEComPortId)sbgStreamBufferReadUint8LE(&inputStream); + pConf->baudRate = sbgStreamBufferReadUint32LE(&inputStream); + pConf->mode = (SbgEComPortMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdInterfaceSetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, const SbgEComInterfaceConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)interfaceId); + sbgStreamBufferWriteUint32LE(&outputStream, pConf->baudRate); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->mode); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_UART_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdInterfaceGetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate *pBitrate, SbgEComCanMode *pMode) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pBitrate); + assert(pMode); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with no payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_CAN_BUS_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read bit rate returned by the device + // + *pBitrate = (SbgEComCanBitRate)sbgStreamBufferReadUint16LE(&inputStream); + + // + // Check if we can parse the CAN mode that has been introduced in sbgECom version 2.0 + // + if (sbgStreamBufferGetSpace(&inputStream) > 0) + { + // + // Read mode returned by the device + // + *pMode = (SbgEComCanMode)sbgStreamBufferReadUint8(&inputStream); + } + else + { + // + // Default the mode to the behavior prior to CAN mode setting introduction + // + *pMode = SBG_ECOM_CAN_MODE_NORMAL; + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdInterfaceSetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate bitrate, SbgEComCanMode mode) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(bitrate <= UINT16_MAX); + assert(mode <= UINT8_MAX); + + // + // Build the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)bitrate); + sbgStreamBufferWriteUint8(&outputStream, (uint8_t)mode); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_BUS_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.h new file mode 100644 index 0000000..c2a69b5 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdInterface.h @@ -0,0 +1,174 @@ +/*! + * \file sbgEComCmdInterface.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Commands used to configure device serial, CAN and Ethernet interfaces. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_INTERFACE_H +#define SBG_ECOM_CMD_INTERFACE_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Serial interface definitions -// +//----------------------------------------------------------------------// + +/*! + * List of serial interfaces available. + */ +typedef enum _SbgEComPortId +{ + SBG_ECOM_IF_COM_A = 0, /*!< Main communication interface. Full duplex. */ + SBG_ECOM_IF_COM_B = 1, /*!< Auxiliary input interface for RTCM. */ + SBG_ECOM_IF_COM_C = 2, /*!< Auxiliary communication interface. Full duplex. */ + SBG_ECOM_IF_COM_D = 3, /*!< Auxiliary input interface. */ + SBG_ECOM_IF_COM_E = 4, /*!< Auxiliary output interface. */ + + SBG_ECOM_IF_ETH_0 = 10, /*!< Ethernet interface 0. */ + SBG_ECOM_IF_ETH_1 = 11, /*!< Ethernet interface 1. */ + SBG_ECOM_IF_ETH_2 = 12, /*!< Ethernet interface 2. */ + SBG_ECOM_IF_ETH_3 = 13, /*!< Ethernet interface 3. */ + SBG_ECOM_IF_ETH_4 = 14, /*!< Ethernet interface 4. */ + + SBG_ECOM_IF_DATA_LOGGER = 20, /*!< Data logger interface. */ +} SbgEComPortId; + +/*! + * List of serial modes available. + */ +typedef enum _SbgEComPortMode +{ + SBG_ECOM_UART_MODE_OFF = 0, /*!< This interface is turned OFF. */ + SBG_ECOM_UART_MODE_232 = 1, /*!< This interface is using RS-232 communications. */ + SBG_ECOM_UART_MODE_422 = 2, /*!< This interface is using RS-422 communications. */ +} SbgEComPortMode; + +//----------------------------------------------------------------------// +//- Serial interface configuration -// +//----------------------------------------------------------------------// + +/*! + * Helper structure to configure a serial interface + */ +typedef struct _SbgEComInterfaceConf +{ + uint32_t baudRate; /*!< The baud rate of the interface. */ + SbgEComPortMode mode; /*!< The mode of the interface. */ +} SbgEComInterfaceConf; + +//----------------------------------------------------------------------// +//- CAN interface definitions -// +//----------------------------------------------------------------------// + +/*! + * Enum containing the list of all available bit rates (in KB/s). + */ +typedef enum _SbgEComCanBitRate +{ + SBG_ECOM_CAN_BITRATE_DISABLED = 0, /*!< The CAN interface is disabled. */ + SBG_ECOM_CAN_BITRATE_10 = 10, /*!< 10Kb/s. */ + SBG_ECOM_CAN_BITRATE_20 = 20, /*!< 20Kb/s. */ + SBG_ECOM_CAN_BITRATE_25 = 25, /*!< 25Kb/s. */ + SBG_ECOM_CAN_BITRATE_50 = 50, /*!< 50Kb/s. */ + SBG_ECOM_CAN_BITRATE_100 = 100, /*!< 100Kb/s. */ + SBG_ECOM_CAN_BITRATE_125 = 125, /*!< 125Kb/s. */ + SBG_ECOM_CAN_BITRATE_250 = 250, /*!< 250Kb/s. */ + SBG_ECOM_CAN_BITRATE_500 = 500, /*!< 500Kb/s. */ + SBG_ECOM_CAN_BITRATE_750 = 750, /*!< 750Kb/s. */ + SBG_ECOM_CAN_BITRATE_1000 = 1000, /*!< 1Mb/s. */ +} SbgEComCanBitRate; + +/*! + * Enum containing the list of different CAN modes + */ +typedef enum _SbgEComCanMode +{ + SBG_ECOM_CAN_MODE_UNDEFINED = 0, /*!< CAN Mode undefined. */ + SBG_ECOM_CAN_MODE_SPY = 1, /*!< Only listening on the CAN bus and doesn't sent anything (even RX ACK bit). */ + SBG_ECOM_CAN_MODE_NORMAL = 2, /*!< The device is allowed to both send and receive over the CAN bus. */ + SBG_ECOM_CAN_NR_MODE +} SbgEComCanMode; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the configuration of one of the interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] interfaceId The interface from which the configuration is to be retrieved. + * \param[out] pConf Pointer to a SbgEComInterfaceConf struct to hold configuration of the interface. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdInterfaceGetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, SbgEComInterfaceConf *pConf); + +/*! + * Set the configuration of one of the interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] interfaceId The interface from which the configuration is to be retrieved. + * \param[in] pConf Pointer to a SbgEComInterfaceConf struct that holds the new configuration for the interface. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdInterfaceSetUartConf(SbgEComHandle *pHandle, SbgEComPortId interfaceId, const SbgEComInterfaceConf *pConf); + +/*! + * Retrieve the configuration of the CAN interface. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pBitrate The bitrate of the CAN interface. + * \param[out] pMode Mode of the CAN interface. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdInterfaceGetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate *pBitrate, SbgEComCanMode *pMode); + +/*! + * Set the configuration of the CAN interface. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] bitRate The bitrate of the CAN interface. + * \param[in] mode Mode of the CAN interface. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdInterfaceSetCanConf(SbgEComHandle *pHandle, SbgEComCanBitRate bitRate, SbgEComCanMode mode); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_INTERFACE_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.c new file mode 100644 index 0000000..ce5a4e8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.c @@ -0,0 +1,44 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdLicense.h" + + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdLicenseApply(SbgEComHandle *pHandle, const void *pBuffer, size_t size) +{ + SbgErrorCode errorCode; + uint32_t currentTimeOut; + + assert(pHandle); + assert(pBuffer); + assert(size > 0); + + // + // Define a time out of 10s to let enough time for the GNSS receiver to apply the license + // + currentTimeOut = pHandle->cmdDefaultTimeOut; + pHandle->cmdDefaultTimeOut = 10000; + + // + // Call function that handle data transfer + // + errorCode = sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_LICENSE_APPLY, pBuffer, size); + + // + // Restore the default time out + // + pHandle->cmdDefaultTimeOut = currentTimeOut; + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.h new file mode 100644 index 0000000..7fb5ea6 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdLicense.h @@ -0,0 +1,66 @@ +/*! + * \file sbgEComCmdLicense.h + * \ingroup commands + * \author SBG Systems + * \date 25 February 2015 + * + * \brief Command used to upload and apply an activation license. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_LICENSE_H +#define SBG_ECOM_CMD_LICENSE_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Upload and apply a new license to a device. + * + * The device will reboot automatically to use the new license. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pBuffer Read only buffer containing the license. + * \param[in] size Size of the buffer. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdLicenseApply(SbgEComHandle *pHandle, const void *pBuffer, size_t size); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_LICENSE_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.c new file mode 100644 index 0000000..1bf5470 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.c @@ -0,0 +1,422 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdMag.h" + +//----------------------------------------------------------------------// +//- Magnetometer commands -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId modelId) +{ + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, modelId); +} + +SbgErrorCode sbgEComCmdMagGetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId *pModelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; + + assert(pHandle); + assert(pModelId); + + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID, &modelIdAsUint); + + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComMagModelsStdId)modelIdAsUint; + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[12 * sizeof(float)]; + uint32_t trial; + uint32_t i; + + assert(pHandle); + assert(pOffset); + assert(pMatrix); + + // + // Initialize a stream buffer to write the command payload + // + errorCode = sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Write the offset vector + // + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pOffset[2]); + + // + // Write the matrix + // + for (i = 0; i < 9; i++) + { + sbgStreamBufferWriteFloatLE(&outputStream, pMatrix[i]); + } + + // + // Make sure that the stream buffer has been initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SET_MAG_CALIB, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received frame was OK + // + if (errorCode == SBG_NO_ERROR) + { + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pRejectConf->magneticField = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // Build payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->magneticField); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Magnetometer onboard calibration commands -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + // + // Initialize a stream buffer to write the command payload + // + errorCode = sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Write the calibration mode and bandwith + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)mode); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)bandwidth); + + // + // Make sure that the stream buffer has been initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_START_MAG_CALIB, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pCalibResults); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 5 s because the onboard magnetic computation can take some time + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_COMPUTE_MAG_CALIB, &receivedPayload, 5000); + + // + // Test if we have received the correct command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + size_t i; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read quality and status parameters + // + pCalibResults->quality = (SbgEComMagCalibQuality)sbgStreamBufferReadUint8LE(&inputStream); + pCalibResults->confidence = (SbgEComMagCalibConfidence)sbgStreamBufferReadUint8LE(&inputStream); + pCalibResults->advancedStatus = sbgStreamBufferReadUint16LE(&inputStream); + + pCalibResults->beforeMeanError = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->beforeStdError = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->beforeMaxError = sbgStreamBufferReadFloatLE(&inputStream); + + pCalibResults->afterMeanError = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->afterStdError = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->afterMaxError = sbgStreamBufferReadFloatLE(&inputStream); + + pCalibResults->meanAccuracy = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->stdAccuracy = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->maxAccuracy = sbgStreamBufferReadFloatLE(&inputStream); + + pCalibResults->numPoints = sbgStreamBufferReadUint16LE(&inputStream); + pCalibResults->maxNumPoints = sbgStreamBufferReadUint16LE(&inputStream); + + // + // Read the computed hard iron offset vector + // + pCalibResults->offset[0] = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->offset[1] = sbgStreamBufferReadFloatLE(&inputStream); + pCalibResults->offset[2] = sbgStreamBufferReadFloatLE(&inputStream); + + // + // Read the computed soft iron matrix + // + for (i = 0; i < 9; i++) + { + pCalibResults->matrix[i] = sbgStreamBufferReadFloatLE(&inputStream); + } + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.h new file mode 100644 index 0000000..bb708b9 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdMag.h @@ -0,0 +1,239 @@ +/*! + * \file sbgEComCmdMag.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Magnetometer aiding module configuration & onboard magnetic calibration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_MAG_H +#define SBG_ECOM_CMD_MAG_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Magnetometer definitions -// +//----------------------------------------------------------------------// + +/*! + * Define if the onboard magnetic calibration should acquiere points for a 3D or 2D calibration. + */ +typedef enum _SbgEComMagCalibMode +{ + SBG_ECOM_MAG_CALIB_MODE_2D = 1, /*!< Tell the device that the magnetic calibration will be performed with limited motions. + This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. + To work correctly, the device should be rotated through at least a full circle. */ + SBG_ECOM_MAG_CALIB_MODE_3D = 2 /*!< Tell the device to start a full 3D magnetic calibration procedure. + The 3D magnetic calibration offers the best accuracy but needs at least motion of ± 30° on the roll and pitch angles. */ +} SbgEComMagCalibMode; + + +/*! + * Used to select the expected dynamics during the magnetic calibration. + */ +typedef enum _SbgEComMagCalibBandwidth +{ + SBG_ECOM_MAG_CALIB_LOW_BW = 0, /*!< Tell the device that low dynamics will be observed during the magnetic calibration process. */ + SBG_ECOM_MAG_CALIB_MEDIUM_BW = 1, /*!< Tell the device that normal dynamics will be observed during the magnetic calibration process. */ + SBG_ECOM_MAG_CALIB_HIGH_BW = 2 /*!< Tell the device that high dynamics will be observed during the magnetic calibration process. */ +} SbgEComMagCalibBandwidth; + +/*! + * General quality indicator of an onboard magnetic calibration. + */ +typedef enum _SbgEComMagCalibQuality +{ + SBG_ECOM_MAG_CALIB_QUAL_OPTIMAL = 0, /*!< All acquired points fit very well on a unit sphere after the calibration. */ + SBG_ECOM_MAG_CALIB_QUAL_GOOD = 1, /*!< Small deviations of the magnetic field norm have been detected. The magnetic calibration should although provide accurate heading. */ + SBG_ECOM_MAG_CALIB_QUAL_POOR = 2, /*!< Large deviations of the magnetic field norm have been detected. It may come from external magnetic distortions during the calibration. */ + SBG_ECOM_MAG_CALIB_QUAL_INVALID = 3 /*!< No valid magnetic calibration has been computed. It could comes from too much magnetic disturbances, insufficient or invalid motions. */ +} SbgEComMagCalibQuality; + +/*! + * Confidence indicator on results of an onbard magnetic calibration. + */ +typedef enum _SbgEComMagCalibConfidence +{ + SBG_ECOM_MAG_CALIB_TRUST_HIGH = 0, /*!< Reported quality indicator can be trusted as enough remarkable magnetic field points have been acquired. */ + SBG_ECOM_MAG_CALIB_TRUST_MEDIUM = 1, /*!< Few remarkable magnetic field points have been used to compute the magnetic calibration leading to a medium confidence in reported quality indicators. */ + SBG_ECOM_MAG_CALIB_TRUST_LOW = 2 /*!< Even if the quality indicator could report an excellent calibration, + The data set used to compute the magnetic calibration was not meaningful enough to compute meaningful quality indicators. + This calibration should be used carefully. */ +} SbgEComMagCalibConfidence; + +/*! + * Status bit masks used to report advanced inforamtion on the onboard magnetic calibration. + */ +#define SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS (0x0001u) /*!< Not enough valid magnetic points have been acquired. */ +#define SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS (0x0002u) /*!< Unable to compute a magnetic calibration due to magnetic interferences or incorrect data set distribution. */ +#define SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE (0x0004u) /*!< For a 3D calibration: not enough motion on X axis. For a 2D calibration; too much motion on X axis. */ +#define SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE (0x0008u) /*!< For a 3D calibration: not enough motion on Y axis. For a 2D calibration; too much motion on Y axis. */ +#define SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE (0x0010u) /*!< For a 3D or 2D calibration: not enough motion on Z axis. */ +#define SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE (0x0020u) /*!< For a 3D calibration: the alignment between the magnetometers and the inertial frame seems to be invalid. */ + +/*! + * This enum defines the different magnetometer model IDs available in standard + */ +typedef enum _SbgEComMagModelsStdIds +{ + SBG_ECOM_MAG_MODEL_NORMAL = 201, /*!< Should be used in most applications */ + SBG_ECOM_MAG_MODEL_NOISY_MAG_TOLERANT = 202, /*!< Should be used in disturbed magnetic environment */ +} SbgEComMagModelsStdId; + +//----------------------------------------------------------------------// +//- Magnetometer configuration -// +//----------------------------------------------------------------------// + +/*! + * Holds all necessary information for Magnetometer module data rejection. + */ +typedef struct _SbgEComMagRejectionConf +{ + SbgEComRejectionMode magneticField; /*!< Rejection mode for magnetic field. */ +} SbgEComMagRejectionConf; + +/*! + * Helper structure to retrieve onboard magnetic calibration results. + */ +typedef struct _SbgEComMagCalibResults +{ + SbgEComMagCalibQuality quality; /*!< General magnetic calibration quality indicator. */ + SbgEComMagCalibConfidence confidence; /*!< Confidence indicator that should be read to interpret the quality indicator. */ + uint16_t advancedStatus; /*!< Set of bit masks used to report advanced information on the magnetic calibration status.*/ + + float beforeMeanError; /*!< Mean magnetic field norm error observed before calibration. */ + float beforeStdError; /*!< Standard deviation of the magnetic field norm error observed before calibration. */ + float beforeMaxError; /*!< Maximum magnetic field norm error observed before calibration. */ + + float afterMeanError; /*!< Mean magnetic field norm error observed after calibration. */ + float afterStdError; /*!< Standard deviation of the magnetic field norm error observed after calibration. */ + float afterMaxError; /*!< Maximum magnetic field norm error observed after calibration. */ + + float meanAccuracy; /*!< Mean expected heading accuracy in radians. */ + float stdAccuracy; /*!< Standard deviation of the expected heading accuracy in radians. */ + float maxAccuracy; /*!< Maximum expected heading accuracy in radians. */ + + uint16_t numPoints; /*!< Number of magnetic field points stored internally and used to compute the magnetic calibration. */ + uint16_t maxNumPoints; /*!< Maximum number of magnetic field points that can be stored internally. */ + float offset[3]; /*!< Computed Hard Iron correction vector offset. */ + float matrix[9]; /*!< Computed Hard & Soft Iron correction matrix. */ +} SbgEComMagCalibResults; + +//----------------------------------------------------------------------// +//- Magnetometer commands -// +//----------------------------------------------------------------------// + +/*! + * Set magnetometer error model id. + * + * \param[in] pHandle A valid sbgECom handle + * \param[in] modelId Magnetometer model id to set + * \return SBG_NO_ERROR if the command has been executed successfully + */ +SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId modelId); + +/*! + * Retrieve magnetometer error model id + * + * \param[in] pHandle A valid sbgECom handle + * \param[out] pModelId Retrieved magnetometer model id + * \return SBG_NO_ERROR if the command has been executed successfully + */ +SbgErrorCode sbgEComCmdMagGetModelId(SbgEComHandle *pHandle, SbgEComMagModelsStdId *pModelId); + +/*! + * Retrieve the rejection configuration of the magnetometer module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Pointer to a SbgEComMagRejectionConf struct to hold rejection configuration of the magnetometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf); + +/*! + * Set the rejection configuration of the magnetometer module. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pRejectConf Pointer to a SbgEComMagRejectionConf struct holding rejection configuration for the magnetometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf); + +/*! + * Send a command that set the magnetometers calibration parameters. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pOffset Magnetometers calibration offset vector. + * \param[in] pMatrix Magnetometers calibration 3x3 matrix. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float *pOffset, const float *pMatrix); + +//----------------------------------------------------------------------// +//- Magnetometer onboard calibration commands -// +//----------------------------------------------------------------------// + +/*! + * Start the magnetic calibration process. + * + * As soon as this command is sent, the device will start logging magnetic field data internally. + * This set of data will be used later by the magnetic calibration algorithms to map the surrounding magnetic field. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] mode Define which magnetic calibration type to perform. It could be 3D or 2D. + * \param[in] bandwidth Tell the device that we should have low, medium or high dynamics during the magnetic calibration process. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth); + +/*! + * This command computes a magnetic calibration solution based on the magnetic field logged since the last call to the command SBG_ECOM_CMD_START_MAG_CALIB (15). + * + * As soon as the computations are done, the device will answer with quality indicators, status flags and if possible a valid magnetic calibration matrix and offset. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pCalibResults Pointer on a SbgEComMagCalibResults structure that can hold onboard magnetic calibration results and status. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_MAG_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.c new file mode 100644 index 0000000..5f07558 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.c @@ -0,0 +1,582 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdOdo.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pOdometerConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ODO_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pOdometerConf->gain = sbgStreamBufferReadFloatLE(&inputStream); + pOdometerConf->gainError = sbgStreamBufferReadUint8LE(&inputStream); + pOdometerConf->reverseMode = sbgStreamBufferReadBooleanLE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pOdometerConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pOdometerConf->gain); + sbgStreamBufferWriteUint8LE(&outputStream, pOdometerConf->gainError); + sbgStreamBufferWriteBooleanLE(&outputStream, pOdometerConf->reverseMode); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pLeverArm); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[2]); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_LEVER_ARM, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pRejectConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pRejectConf->velocity = (SbgEComRejectionMode)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pRejectConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pRejectConf->velocity); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_REJECT_MODE, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoCanGetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, SbgEComCmdOdoCanConf *pOdoCanConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[16]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pOdoCanConf); + assert(canChannel <= UCHAR_MAX); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the command payload used to ask the CAN odometer configuration for a specific channel + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8LE(&outputStream, canChannel); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_ODO_CAN_SPEED_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read fields from payload + // + canChannel = sbgStreamBufferReadUint8LE(&inputStream); + + pOdoCanConf->options = sbgStreamBufferReadUint16LE(&inputStream); + pOdoCanConf->canId = sbgStreamBufferReadUint32LE(&inputStream); + + pOdoCanConf->startBit = sbgStreamBufferReadUint8LE(&inputStream); + pOdoCanConf->dataSize = sbgStreamBufferReadUint8LE(&inputStream); + + pOdoCanConf->scale = sbgStreamBufferReadFloatLE(&inputStream); + pOdoCanConf->offset = sbgStreamBufferReadFloatLE(&inputStream); + pOdoCanConf->minValue = sbgStreamBufferReadFloatLE(&inputStream); + pOdoCanConf->maxValue = sbgStreamBufferReadFloatLE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pOdoCanConf); + assert(canChannel <= UCHAR_MAX); + + // + // A CAN message has a payload of up to 64 bits so the offset can range from 0 to 63 and size from 1 to 64 + // + assert(pOdoCanConf->startBit < 64); + assert(pOdoCanConf->dataSize > 0); + assert(pOdoCanConf->dataSize <= 64); + + // + // Build the command payload + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)canChannel); + + sbgStreamBufferWriteUint16LE(&outputStream, pOdoCanConf->options); + sbgStreamBufferWriteUint32LE(&outputStream, pOdoCanConf->canId); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->startBit); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pOdoCanConf->dataSize); + + sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->scale); + sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->offset); + sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->minValue); + sbgStreamBufferWriteFloatLE(&outputStream, pOdoCanConf->maxValue); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_ODO_CAN_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.h new file mode 100644 index 0000000..ae9e005 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOdo.h @@ -0,0 +1,189 @@ +/*! + * \file sbgEComCmdOdo.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Odometer / DMI aiding module configuration commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_ODO_H +#define SBG_ECOM_CMD_ODO_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * Holds all necessary information for Odometer module parameter configuration. + */ +typedef struct _SbgEComOdoConf +{ + float gain; /*!< Odometer's gain in pulses / meter. */ + uint8_t gainError; /*!< User gain average error in % */ + bool reverseMode; /*!< Whether the odometer is in reverse mode or not. */ +} SbgEComOdoConf; + +/*! + * Holds all necessary information for Odometer module data rejection. + */ +typedef struct _SbgEComOdoRejectionConf +{ + SbgEComRejectionMode velocity; /*!< Rejection mode for velocity. */ +} SbgEComOdoRejectionConf; + +/*! + * CAN odometer channels definition + * A channel is an inforamtion that can be decoded / used by the device. + */ +typedef enum _SbgEComCmdOdoCanChannel +{ + SBG_ECOM_CMD_ODO_CAN_CH_VELOCITY = 0, /*!< Channel used to decode the vehicle velocity information */ + SBG_ECOM_CMD_ODO_CAN_CH_REVERSE = 1 /*!< Channel used to decode the vehicle velocity reverse info (if available). */ +} SbgEComCmdOdoCanChannel; + +/* + * Define CAN odometer options bitmask + */ +#define SBG_ECOM_CMD_ODO_CAN_ENABLE (uint16_t)(0x0001 << 0) /*!< Set to enable CAN odometer information decoding. */ +#define SBG_ECOM_CMD_ODO_CAN_ID_EXTENDED (uint16_t)(0x0001 << 1) /*!< Set for a 29 bit extended CAN message, otherwise standard 11 bit */ +#define SBG_ECOM_CMD_ODO_CAN_BIG_ENDIAN (uint16_t)(0x0001 << 2) /*!< Set if the velocity is encoded in big endian, otherwise little endian */ +#define SBG_ECOM_CMD_ODO_CAN_SIGNED (uint16_t)(0x0001 << 3) /*!< Set to interpret the parsed value as signed, otherwise unsigned. */ + +/*! + * Holds all necessary information for CAN Odometer parameter configuration. + * This format is very similar to info contained in a DBC file. + */ +typedef struct _SbgEComCmdOdoCanConf +{ + uint16_t options; /*!< Set of options bit masks such as CAN extended. */ + uint32_t canId; /*!< CAN message ID from which the odometer velocity will be parsed. */ + + size_t startBit; /*!< Index of field MSB in big endian or LSB in little endian within the payload (any value from 0 to 63). */ + size_t dataSize; /*!< Length in bits of the odometer velocity field (any value from 1 to 64 minus dataOffset). */ + + float scale; /*!< Value to multiply the parsed field with to get physical unit^in m.s-1. */ + float offset; /*!< Offset to add on the scaled velocity information in m.s-1 (after applying scale factor). */ + float minValue; /*!< The minimum velocity to consider the message valid in m.s-1 */ + float maxValue; /*!< The maximum velocity to consider the message valid in m.s-1 */ +} SbgEComCmdOdoCanConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * For quadrature and/or pulse based odometer, retrieve the configuration. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pOdometerConf Pointer to a SbgEComOdoConf struct to hold configuration of the odometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf); + +/*! + * For quadrature and/or pulse base odometer, define the configuration. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pOdometerConf Pointer to a SbgEComOdoConf struct holding configuration for the odometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf); + +/*! + * Retrieve the lever arm applicable for both quadrature or CAN based odometer. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pLeverArm Array of three values, one for each axis. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float *pLeverArm); + +/*! + * Set the lever arm applicable for both quadrature or CAN based odometer. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pLeverArm Array of three values, one for each axis. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float *pLeverArm); + +/*! + * Retrieve the velocity rejection configuration for both quadrature or CAN based odometer. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pRejectConf Pointer to a SbgEComOdoRejectionConf struct to hold rejection configuration of the odometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf); + +/*! + * Set the velocity rejection configuration for both quadrature or CAN based odometer. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pRejectConf Pointer to a SbgEComOdoRejectionConf struct holding rejection configuration for the odometer module. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf); + +/*! + * Retrieve the CAN odometer configuration for a specific CAN information channel + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] canChannel The CAN channel to retreive associated DBC configuration. + * \param[out] pOdoCanConf Struct to hold configuration of the CAN odometer. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoCanGetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, SbgEComCmdOdoCanConf *pOdoCanConf); + +/*! + * Set the CAN odometer configuration for a specific CAN information channel + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] canChannel The CAN channel to define associated DBC configuration. + * \param[in] pOdoCanConf Struct holding configuration for the CAN odometer. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOdoCanSetConf(SbgEComHandle *pHandle, SbgEComCmdOdoCanChannel canChannel, const SbgEComCmdOdoCanConf *pOdoCanConf); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_ODO_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.c new file mode 100644 index 0000000..ba95b85 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.c @@ -0,0 +1,583 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdOutput.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[5]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command and the prepared payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_OUTPUT_CONF command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the received payload + // + outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); + msgId = sbgStreamBufferReadUint8LE(&inputStream); + classId = (SbgEComClass)sbgStreamBufferReadUint8LE(&inputStream); + *pConf = (SbgEComOutputMode)sbgStreamBufferReadUint16LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode conf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[5]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)msgId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)conf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pEnable); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, &outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command and the prepared payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the received payload + // + outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); + classId = (SbgEComClass)sbgStreamBufferReadUint8LE(&inputStream); + *pEnable = (bool)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)classId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)enable); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[2]; + SbgStreamBuffer outputStream; + + + assert(pHandle); + assert(pMode); + assert(pUserId); + assert(pExtended); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, internalId); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a correct answer + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Parse the payload + // + internalId = (SbgECanMessageId)sbgStreamBufferReadUint16LE(&inputStream); + *pMode = (SbgEComOutputMode)sbgStreamBufferReadUint16LE(&inputStream); + *pUserId = sbgStreamBufferReadUint32LE(&inputStream); + *pExtended = (bool)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[9]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)internalId); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)mode); + sbgStreamBufferWriteUint32LE(&outputStream, userId); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)extended); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_CAN_OUTPUT_CONF, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdOutputGetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, char *pNmeaTalkerId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + uint8_t outputBuffer[1]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pNmeaTalkerId); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8(&outputStream, outputPort); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command with the output port as a 1-byte payload + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_NMEA_TALKER_ID command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + outputPort = (SbgEComOutputPort)sbgStreamBufferReadUint8LE(&inputStream); + pNmeaTalkerId[0] = (char)sbgStreamBufferReadUint8LE(&inputStream); + pNmeaTalkerId[1] = (char)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char *pNmeaTalkerId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[3]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pNmeaTalkerId); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)outputPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[0])); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)(pNmeaTalkerId[1])); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_NMEA_TALKER_ID, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.h new file mode 100644 index 0000000..6c6bdbe --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdOutput.h @@ -0,0 +1,197 @@ +/*! + * \file sbgEComCmdOutput.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Commands used to setup logs to output over the device interfaces. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_OUTPUT_H +#define SBG_ECOM_CMD_OUTPUT_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public definitions -// +//----------------------------------------------------------------------// + +/*! + * List of ouput ports available. + */ +typedef enum _SbgEComOutputPort +{ + SBG_ECOM_OUTPUT_PORT_A = 0, /*!< Main output port. */ + SBG_ECOM_OUTPUT_PORT_C = 2, /*!< Secondary output port only available on Ellipse-E devices */ + SBG_ECOM_OUTPUT_PORT_E = 4 /*!< Secondary output port only available on B1 devices */ +} SbgEComOutputPort; + +/*! + * List of output modes available. + */ +typedef enum _SbgEComOutputMode +{ + SBG_ECOM_OUTPUT_MODE_DISABLED = 0, /*!< This output is disabled. */ + SBG_ECOM_OUTPUT_MODE_MAIN_LOOP = 1, /*!< Output the message every main loop (ie 200 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_2 = 2, /*!< Output the message every 2 main loops (ie 100 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_4 = 4, /*!< Output the message every 4 main loops (ie 50 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_5 = 5, /*!< Output the message every 4 main loops (ie 40 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_8 = 8, /*!< Output the message every 8 main loops (ie 25 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_10 = 10, /*!< Output the message every 10 main loops (ie 20 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_20 = 20, /*!< Output the message every 20 main loops (ie 10 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_40 = 40, /*!< Output the message every 40 main loops (ie 5 Hz). */ + SBG_ECOM_OUTPUT_MODE_DIV_200 = 200, /*!< Output the message every 200 main loops (ie 1 Hz). */ + SBG_ECOM_OUTPUT_MODE_PPS = 10000, /*!< Output the message on a Pulse Per Second event. */ + SBG_ECOM_OUTPUT_MODE_NEW_DATA = 10001, /*!< Output sent when a new data is available. */ + SBG_ECOM_OUTPUT_MODE_EVENT_IN_A = 10003, /*!< Output the message when a Sync A is received. */ + SBG_ECOM_OUTPUT_MODE_EVENT_IN_B = 10004, /*!< Output the message when a Sync B is received. */ + SBG_ECOM_OUTPUT_MODE_EVENT_IN_C = 10005, /*!< Output the message when a Sync C is received. */ + SBG_ECOM_OUTPUT_MODE_EVENT_IN_D = 10006, /*!< Output the message when a Sync D is received. */ + SBG_ECOM_OUTPUT_MODE_EVENT_IN_E = 10007, /*!< Output the message when a Sync E is received. */ + SBG_ECOM_OUTPUT_MODE_HIGH_FREQ_LOOP = 20001 /*!< Output the message in the 1KHz IMU loop */ +} SbgEComOutputMode; + +/*! + * Defines which monitoring point to use for an output port. + * This feature enabled deporting measurements at a specific monitoring point. + */ +typedef enum _SbgEComOutputMonitoringPoint +{ + SBG_ECOM_OUTPUT_MONITORING_POINT_IMU = 0, /*!< Output measurements at the IMU location. */ + SBG_ECOM_OUTPUT_MONITORING_POINT_COG = 1, /*!< Output measurements at the center of rotation. */ + SBG_ECOM_OUTPUT_MONITORING_POINT_1 = 2, /*!< Output measurements at the user deported location 1 (only for Ekinox and Apogee). */ + SBG_ECOM_OUTPUT_MONITORING_POINT_2 = 3, /*!< Output measurements at the user deported location 2 (only for Ekinox and Apogee). */ + SBG_ECOM_OUTPUT_MONITORING_POINT_3 = 4, /*!< Output measurements at the user deported location 3 (only for Ekinox and Apogee). */ + SBG_ECOM_OUTPUT_MONITORING_NUM /*!< Number of output monitoring points. */ +} SbgEComOutputMonitoringPoint; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Retrieve the configuration of one the message on one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[in] classId The class of the concerned log. + * \param[in] msgId The id of the concerned log. + * \param[out] pMode Pointer to a SbgEComOutputMode to contain the current output mode of the message. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pMode); + +/*! + * Set the configuration of one the message on one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[in] classId The class of the concerned log. + * \param[in] msgId The id of the concerned log. + * \param[in] mode New output mode to set. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode mode); + +/*! + * Retrieve the enable of one of the output class message on one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port. + * \param[in] classId The class to enable or disable. + * \param[out] pEnable TRUE to enable message output of this class, FALSE to disable it. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputClassGetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool *pEnable); + +/*! + * Set the enable of one of the output class message on one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port. + * \param[in] classId The class to enable or disable. + * \param[in] enable TRUE to enable message output of this class, FALSE to disable it. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputClassSetEnable(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, bool enable); + +/*! + * Retrieve the configuration of one the message on the CAN interface. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] internalId The internal message id. + * \param[out] pMode Pointer to a SbgEComOutputMode to contain the current output mode of the message. + * \param[out] pUserId The user defined message id. + * \param[out] pExtended TRUE if the user id uses the extended format. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdCanOutputGetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode *pMode, uint32_t *pUserId, bool *pExtended); + +/*! + * Set the configuration of one the message on the CAN interface + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] internalId The internal message id. + * \param[in] mode Pointer to a SbgEComOutputMode containing the new output mode of the message. + * \param[in] userId The user defined message id. + * \param[in] extended TRUE if the user id uses the extended format. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdCanOutputSetConf(SbgEComHandle *pHandle, SbgECanMessageId internalId, SbgEComOutputMode mode, uint32_t userId, bool extended); + +/*! + * Retrieve the NMEA talker id of one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[out] pNmeaTalkerId A 2-char array to contain the nmea talker id. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputGetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, char *pNmeaTalkerId); + +/*! + * Set the NMEA talker id of one of the output interfaces. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] outputPort The output port of the device for the log concerned. + * \param[out] pNmeaTalkerId A 2-char array containint the new nmea talker id. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdOutputSetNmeaTalkerId(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, const char *pNmeaTalkerId); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_OUTPUT_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.c new file mode 100644 index 0000000..d6fbcf8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.c @@ -0,0 +1,484 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdSensor.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds modelId) +{ + assert(pHandle); + + return sbgEComCmdGenericSetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, modelId); +} + + +SbgErrorCode sbgEComCmdSensorGetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds *pModelId) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t modelIdAsUint; + + assert(pHandle); + assert(pModelId); + + errorCode = sbgEComCmdGenericGetModelId(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_MOTION_PROFILE_ID, &modelIdAsUint); + + if (errorCode == SBG_NO_ERROR) + { + *pModelId = (SbgEComMotionProfileStdIds)modelIdAsUint; + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_INIT_PARAMETERS command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pConf->latitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->longitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->altitude = sbgStreamBufferReadDoubleLE(&inputStream); + pConf->year = sbgStreamBufferReadUint16LE(&inputStream); + pConf->month = sbgStreamBufferReadUint8LE(&inputStream); + pConf->day = sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[64]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteDoubleLE(&outputStream, pConf->latitude); + sbgStreamBufferWriteDoubleLE(&outputStream, pConf->longitude); + sbgStreamBufferWriteDoubleLE(&outputStream, pConf->altitude); + sbgStreamBufferWriteUint16LE(&outputStream, (uint16_t)pConf->year); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->month); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->day); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_INIT_PARAMETERS, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pConf); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_AIDING_ASSIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pConf->gps1Port = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); + pConf->gps1Sync = (SbgEComModuleSyncAssignment)sbgStreamBufferReadUint8LE(&inputStream); + + sbgStreamBufferSeek(&inputStream, 4*sizeof(uint8_t), SB_SEEK_CUR_INC); /*!< Reserved fields to ignore */ + + pConf->dvlPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); + pConf->dvlSync = (SbgEComModuleSyncAssignment)sbgStreamBufferReadUint8LE(&inputStream); + + pConf->rtcmPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); + pConf->airDataPort = (SbgEComModulePortAssignment)sbgStreamBufferReadUint8LE(&inputStream); + pConf->odometerPinsConf = (SbgEComOdometerPinAssignment)sbgStreamBufferReadUint8LE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[16]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pConf); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Port); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->gps1Sync); + + // + // Skip the 4 reserved bytes + // + sbgStreamBufferWriteUint8LE(&outputStream, 0); + sbgStreamBufferWriteUint8LE(&outputStream, 0); + sbgStreamBufferWriteUint8LE(&outputStream, 0); + sbgStreamBufferWriteUint8LE(&outputStream, 0); + + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->dvlPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->dvlSync); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->rtcmPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->airDataPort); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pConf->odometerPinsConf); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_AIDING_ASSIGNMENT, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + uint32_t trial; + + assert(pHandle); + assert(pAlignConf); + assert(pLeverArm); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command only since this is a no-payload command + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, NULL, 0); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComReceiveCmd2(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, &receivedPayload, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a SBG_ECOM_CMD_IMU_ALIGNMENT command + // + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Initialize stream buffer to read parameters + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read parameters + // + pAlignConf->axisDirectionX = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); + pAlignConf->axisDirectionY = (SbgEComAxisDirection)sbgStreamBufferReadUint8LE(&inputStream); + pAlignConf->misRoll = sbgStreamBufferReadFloatLE(&inputStream); + pAlignConf->misPitch = sbgStreamBufferReadFloatLE(&inputStream); + pAlignConf->misYaw = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[0] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[1] = sbgStreamBufferReadFloatLE(&inputStream); + pLeverArm[2] = sbgStreamBufferReadFloatLE(&inputStream); + + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float *pLeverArm) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[32]; + SbgStreamBuffer outputStream; + + assert(pHandle); + assert(pAlignConf); + assert(pLeverArm); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Init stream buffer for output + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload + // + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pAlignConf->axisDirectionX); + sbgStreamBufferWriteUint8LE(&outputStream, (uint8_t)pAlignConf->axisDirectionY); + sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misRoll); + sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misPitch); + sbgStreamBufferWriteFloatLE(&outputStream, pAlignConf->misYaw); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[0]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[1]); + sbgStreamBufferWriteFloatLE(&outputStream, pLeverArm[2]); + + // + // Send the payload over ECom + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.h new file mode 100644 index 0000000..218c671 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSensor.h @@ -0,0 +1,236 @@ +/*! + * \file sbgEComCmdSensor.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Motion profile, aiding assignement & sensor installation commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_SENSOR_H +#define SBG_ECOM_CMD_SENSOR_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +// Local headers +#include "sbgEComCmdCommon.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Sensor definitions -// +//----------------------------------------------------------------------// + +/*! + * Ports available for the modules. + */ +typedef enum _SbgEComModulePortAssignment +{ + SBG_ECOM_MODULE_PORT_A = 0, /*!< Module connected on PORT_A. */ + SBG_ECOM_MODULE_PORT_B = 1, /*!< Module connected on PORT_B. */ + SBG_ECOM_MODULE_PORT_C = 2, /*!< Module connected on PORT_C. */ + SBG_ECOM_MODULE_PORT_D = 3, /*!< Module connected on PORT_D. */ + SBG_ECOM_MODULE_PORT_E = 4, /*!< Module connected on PORT_E. */ + SBG_ECOM_MODULE_INTERNAL = 5, /*!< Module is connected internally. */ + SBG_ECOM_MODULE_DISABLED = 0xFF /*!< Module is disabled. */ +} SbgEComModulePortAssignment; + +/*! + * Synchronization signals available for the modules. + */ +typedef enum _SbgEComModuleSyncAssignment +{ + SBG_ECOM_MODULE_SYNC_DISABLED = 0, /*!< Module is disabled. */ + SBG_ECOM_MODULE_SYNC_IN_A = 1, /*!< Synchronization is done using SYNC_IN_A pin. */ + SBG_ECOM_MODULE_SYNC_IN_B = 2, /*!< Synchronization is done using SYNC_IN_B pin. */ + SBG_ECOM_MODULE_SYNC_IN_C = 3, /*!< Synchronization is done using SYNC_IN_C pin. */ + SBG_ECOM_MODULE_SYNC_IN_D = 4, /*!< Synchronization is done using SYNC_IN_D pin. */ + SBG_ECOM_MODULE_SYNC_INTERNAL = 5, /*!< Synchronization is internal. */ + SBG_ECOM_MODULE_SYNC_OUT_A = 6, /*!< Synchronization signal is output on SYNC_OUT_A. */ + SBG_ECOM_MODULE_SYNC_OUT_B = 7, /*!< Synchronization signal is output on SYNC_OUT_B. */ +} SbgEComModuleSyncAssignment; + +/*! + * List of configurations available for the odometer. + */ +typedef enum _SbgEComOdometerPinAssignment +{ + SBG_ECOM_MODULE_ODO_DISABLED = 0, /*!< Odometer is disabled. */ + SBG_ECOM_MODULE_ODO_A = 1, /*!< Odometer connected only to ODO_A (unidirectional).. */ + SBG_ECOM_MODULE_ODO_A_B = 2, /*!< Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer.. */ + SBG_ECOM_MODULE_ODO_CAN = 3, /*!< Vehicle odometer using CAN (OBD-II). */ +} SbgEComOdometerPinAssignment; + +/*! + * This enum defines the different motion profile IDs available in standard + */ +typedef enum _SbgEComMotionProfileStdIds +{ + SBG_ECOM_MOTION_PROFILE_GENERAL_PURPOSE = 1, /*!< Should be used as a default when other profiles do not apply */ + SBG_ECOM_MOTION_PROFILE_AUTOMOTIVE = 2, /*!< Dedicated to car applications with strict lateral velocity constraints. */ + SBG_ECOM_MOTION_PROFILE_MARINE = 3, /*!< Used in marine and underwater applications */ + SBG_ECOM_MOTION_PROFILE_AIRPLANE = 4, /*!< For fixed wings aircraft */ + SBG_ECOM_MOTION_PROFILE_HELICOPTER = 5, /*!< For rotary wing aircraft */ + SBG_ECOM_MOTION_PROFILE_PEDESTRIAN = 6, /*!< Pedestrian applications using foot odometry */ + SBG_ECOM_MOTION_PROFILE_UAV_ROTARY_WING = 7, /*!< For rotary wing UAVs that have low dynamics */ + SBG_ECOM_MOTION_PROFILE_HEAVY_MACHINERY = 8, /*!< For vibrating applications with low dynamics and no specific travel direction */ + SBG_ECOM_MOTION_PROFILE_STATIC = 9, /*!< Static motion profile that delivers stable results for 27/7 operations. */ + SBG_ECOM_MOTION_PROFILE_TRUCK = 10, /*!< Truck applications with medium lateral velocity constraints. */ + SBG_ECOM_MOTION_PROFILE_RAILWAY = 11 /*!< Train applications with relaxed lateral velocity constraints. */ +} SbgEComMotionProfileStdIds; + +//----------------------------------------------------------------------// +//- Event configurations -// +//----------------------------------------------------------------------// + +/*! + * Helper structure for module assignments + */ +typedef struct _SbgEComAidingAssignConf +{ + SbgEComModulePortAssignment gps1Port; /*!< GNSS module port assignment. */ + SbgEComModuleSyncAssignment gps1Sync; /*!< GNSS module sync assignment. */ + SbgEComModulePortAssignment dvlPort; /*!< Port on which the DVL is connected */ + SbgEComModuleSyncAssignment dvlSync; /*!< Optional sync signal that could be used to time stamp the DVL data. */ + SbgEComModulePortAssignment rtcmPort; /*!< RTCM input port assignment for IGNG-N DGPS. */ + SbgEComModulePortAssignment airDataPort; /*!< Port on which Air Data aiding is connected. */ + SbgEComOdometerPinAssignment odometerPinsConf; /*!< Odometer module pin assignment. */ +} SbgEComAidingAssignConf; + +/*! + * Helper structure for sensor alignment details + */ +typedef struct _SbgEComSensorAlignmentInfo +{ + SbgEComAxisDirection axisDirectionX; /*!< Sensor X axis direction in vehicle */ + SbgEComAxisDirection axisDirectionY; /*!< Sensor Y axis direction in vehicle */ + float misRoll; /*!< Roll angle fine misalignment in rad */ + float misPitch; /*!< Pitch angle fine misalignment in rad */ + float misYaw; /*!< Yaw angle fine misalignment in rad */ +} SbgEComSensorAlignmentInfo; + +/*! + * Helper structure for sensor Initial condition details + */ +typedef struct _SbgEComInitConditionConf +{ + double latitude; /*!< Initial latitude in ° */ + double longitude; /*!< Initial longitude in ° */ + double altitude; /*!< Initial altitude above MSL in meters */ + uint16_t year; /*!< Initial Year */ + uint8_t month; /*!< Initial month */ + uint8_t day; /*!< Initial day */ +} SbgEComInitConditionConf; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Set the motion profile id used to tune the Kalman Filter to a specific application + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] modelId Motion profile id to set + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds modelId); + +/*! + * Retrieve the motion profile id. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pModelId Retrieved motion profile id + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorGetMotionProfileId(SbgEComHandle *pHandle, SbgEComMotionProfileStdIds *pModelId); + +/*! + * Retrieve the initial conditions settings. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pConf Pointer to a SbgEComInitConditionConf to contain the current initial conditions settings. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf); + +/*! + * Set the initial condition configuration. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pConf Pointer to a SbgEComInitConditionConf containing the new initial condition configuration. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf); + +/*! + * Retrieve the assignment of the aiding sensors. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pConf Pointer to a SbgEComAidingAssignConf to contain the current assignment of the aiding sensors. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf); + +/*! + * Set the assignment of the aiding sensors. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pConf Pointer to a SbgEComAidingAssignConf containing the new assignment of the aiding sensors. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf); + +/*! + * Retrieve the alignment and lever arm configuration of the sensor. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[out] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct to hold alignment configuration of the sensor. + * \param[out] pLeverArm Pointer to a table to contain lever arm X, Y, Z components in meters. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float *pLeverArm); + +/*! + * Set the alignment and lever arm configuration of the sensor. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pAlignConf Pointer to a SbgEComSensorAlignmentInfo struct holding alignment configuration for the sensor. + * \param[in] pLeverArm Pointer to a table containing lever arm X, Y, Z components in meters. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float *pLeverArm); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_SENSOR_H diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.c b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.c new file mode 100644 index 0000000..17c9f3d --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.c @@ -0,0 +1,89 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include +#include + +// Local headers +#include "sbgEComCmdCommon.h" +#include "sbgEComCmdSettings.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + uint32_t trial; + uint8_t outputBuffer[1]; + SbgStreamBuffer outputStream; + + assert(pHandle); + + // + // Build the payload to send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint8(&outputStream, action); + + // + // Send the command three times + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send the command and the action + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SETTINGS_ACTION, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + // + // Make sure that the command has been sent + // + if (errorCode == SBG_NO_ERROR) + { + // + // Try to read the device answer for 500 ms + // + errorCode = sbgEComWaitForAck(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_SETTINGS_ACTION, pHandle->cmdDefaultTimeOut); + + // + // Test if we have received a valid ACK + // + if (errorCode == SBG_NO_ERROR) + { + // + // The command has been executed successfully so return + // + break; + } + } + else + { + // + // We have a write error so exit the try loop + // + break; + } + } + + return errorCode; +} + +SbgErrorCode sbgEComCmdImportSettings(SbgEComHandle *pHandle, const void *pBuffer, size_t size) +{ + // + // Call function that handle data transfer + // + return sbgEComTransferSend(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_IMPORT_SETTINGS, pBuffer, size); +} + +SbgErrorCode sbgEComCmdExportSettings(SbgEComHandle *pHandle, void *pBuffer, size_t *pSize, size_t maxSize) +{ + // + // Call function that handle data transfer + // + return sbgEComTransferReceive(pHandle, SBG_ECOM_CLASS_LOG_CMD_0, SBG_ECOM_CMD_EXPORT_SETTINGS, pBuffer, pSize, maxSize); +} diff --git a/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.h b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.h new file mode 100644 index 0000000..9f82ca1 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/commands/sbgEComCmdSettings.h @@ -0,0 +1,105 @@ +/*! + * \file sbgEComCmdSettings.h + * \ingroup commands + * \author SBG Systems + * \date 11 June 2014 + * + * \brief Import/export/save settings commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_CMD_SETTINGS_H +#define SBG_ECOM_CMD_SETTINGS_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Settings action definition -// +//----------------------------------------------------------------------// + +/*! + * Defintion of all the settings actions available. + */ +typedef enum _SbgEComSettingsAction +{ + SBG_ECOM_REBOOT_ONLY = 0, /*!< Only reboot the device. */ + SBG_ECOM_SAVE_SETTINGS = 1, /*!< Save the settings to non-volatile memory and then reboot the device. */ + SBG_ECOM_RESTORE_DEFAULT_SETTINGS = 2 /*!< Restore default settings, save them to non-volatile memory and reboot the device. */ +} SbgEComSettingsAction; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Send a command to execute a specific system action to reboot/save/restore default settings. + * + * Execute one of the available settings action: + * - SBG_ECOM_REBOOT_ONLY : Only reboot the device. + * - SBG_ECOM_SAVE_SETTINGS : Save the settings to non-volatile memory and then reboot the device. + * - SBG_ECOM_RESTORE_DEFAULT_SETTINGS : Restore default settings, save them to non-volatile memory and reboot the device. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] action One of the available SbgEComSettingsAction. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action); + +/*! + * Send a complete set of settings to the device and store them into the FLASH memory. + * + * The device will reboot automatically to use the new settings. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pBuffer Read only buffer containing the settings. + * \param[in] size Size of the buffer. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdImportSettings(SbgEComHandle *pHandle, const void *pBuffer, size_t size); + +/*! + * Retrieve a complete set of settings from the device as a buffer. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pBuffer Allocated buffer that can hold the received settings. + * \param[out] pSize The number of bytes that have been stored into pBuffer. + * \param[in] maxSize The maximum buffer size in bytes that can be stored into pBuffer. + * \return SBG_NO_ERROR if the command has been executed successfully. + */ +SbgErrorCode sbgEComCmdExportSettings(SbgEComHandle *pHandle, void *pBuffer, size_t *pSize, size_t maxSize); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_CMD_SETTINGS_H diff --git a/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.c b/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.c new file mode 100644 index 0000000..b49c283 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.c @@ -0,0 +1,389 @@ +// sbgCommonLib headers +#include + +// Local headers +#include "sbgEComDefsGnss.h" + +//----------------------------------------------------------------------// +//- Private definitions -// +//----------------------------------------------------------------------// + +/*! + * GNSS signal descriptor. + */ +typedef struct _SbgEComSignalIdDesc +{ + SbgEComSignalId id; /*!< Signal ID. */ + const char *pName; /*!< Corresponding NULL terminated C string. */ +} SbgEComSignalIdDesc; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +SbgEComConstellationId sbgEComGetConstellationFromSignalId(SbgEComSignalId signalId) +{ + SbgEComConstellationId constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; + + // + // Don't use a default catch to explicitly handle ALL enum values + // + switch (signalId) + { + case SBG_ECOM_SIGNAL_ID_UNKNOWN: + constellationId = SBG_ECOM_CONSTELLATION_ID_UNKNOWN; + break; + + // + // GPS constellation + // + case SBG_ECOM_SIGNAL_ID_GPS_L1C_DP: + case SBG_ECOM_SIGNAL_ID_GPS_L1C_D: + case SBG_ECOM_SIGNAL_ID_GPS_L1C_P: + case SBG_ECOM_SIGNAL_ID_GPS_L1_W: + case SBG_ECOM_SIGNAL_ID_GPS_L1_CA: + case SBG_ECOM_SIGNAL_ID_GPS_L1P: + case SBG_ECOM_SIGNAL_ID_GPS_L1_PY: + case SBG_ECOM_SIGNAL_ID_GPS_L1M: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_ML: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_L: + case SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL: + case SBG_ECOM_SIGNAL_ID_GPS_L2_W: + case SBG_ECOM_SIGNAL_ID_GPS_L2_CA: + case SBG_ECOM_SIGNAL_ID_GPS_L2C_M: + case SBG_ECOM_SIGNAL_ID_GPS_L2_PY: + case SBG_ECOM_SIGNAL_ID_GPS_L2M: + case SBG_ECOM_SIGNAL_ID_GPS_L2P: + case SBG_ECOM_SIGNAL_ID_GPS_L5_IQ: + case SBG_ECOM_SIGNAL_ID_GPS_L5_I: + case SBG_ECOM_SIGNAL_ID_GPS_L5_Q: + constellationId = SBG_ECOM_CONSTELLATION_ID_GPS; + break; + + // + // GLONASS constellation + // + case SBG_ECOM_SIGNAL_ID_GLONASS_G1_P: + case SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA: + case SBG_ECOM_SIGNAL_ID_GLONASS_G2_P: + case SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_I: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q: + case SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ: + constellationId = SBG_ECOM_CONSTELLATION_ID_GLONASS; + break; + + // + // Galileo constellation + // + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_C: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_B: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_A: + case SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_I: + case SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_C: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_B: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC: + case SBG_ECOM_SIGNAL_ID_GALILEO_E6_A: + constellationId = SBG_ECOM_CONSTELLATION_ID_GALILEO; + break; + + // + // BeiDou constellation + // + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3I: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P: + case SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP: + constellationId = SBG_ECOM_CONSTELLATION_ID_BEIDOU; + break; + + // + // QZSS constellation + // + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP: + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_D: + case SBG_ECOM_SIGNAL_ID_QZSS_L1C_P: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_CA: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF: + case SBG_ECOM_SIGNAL_ID_QZSS_L1_SB: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_L: + case SBG_ECOM_SIGNAL_ID_QZSS_L2C_M: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_I: + case SBG_ECOM_SIGNAL_ID_QZSS_L5_Q: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_I: + case SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_P: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_DP: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_D: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_E: + case SBG_ECOM_SIGNAL_ID_QZSS_L6_DE: + constellationId = SBG_ECOM_CONSTELLATION_ID_QZSS; + break; + + // + // SBAS system + // + case SBG_ECOM_SIGNAL_ID_SBAS_L1_CA: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_I: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_Q: + case SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ: + constellationId = SBG_ECOM_CONSTELLATION_ID_SBAS; + break; + + // + // IRNSS constellation + // + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_A: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_B: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_C: + case SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_A: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_B: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_C: + case SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC: + constellationId = SBG_ECOM_CONSTELLATION_ID_IRNSS; + break; + + // + // L-Band system + // + case SBG_ECOM_SIGNAL_ID_LBAND: + constellationId = SBG_ECOM_CONSTELLATION_ID_LBAND; + break; + } + + return constellationId; +} + +bool sbgEComSignalIdIsValid(uint8_t signalId) +{ + if ( (signalId == SBG_ECOM_SIGNAL_ID_UNKNOWN) || + (sbgEComGetConstellationFromSignalId(signalId) != SBG_ECOM_CONSTELLATION_ID_UNKNOWN)) + { + return true; + } + else + { + return false; + } +} + +const char *sbgEComSignalToStr(SbgEComSignalId signalId) +{ + static const SbgEComSignalIdDesc signalIdDesc[] = + { + { SBG_ECOM_SIGNAL_ID_UNKNOWN, "unknown" }, + + { SBG_ECOM_SIGNAL_ID_GPS_L1C_DP, "gpsL1C_DP" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1C_D, "gpsL1C_D" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1C_P, "gpsL1C_P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_W, "gpsL1_W" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_CA, "gpsL1_CA" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1P, "gpsL1P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1_PY, "gpsL1_PY" }, + { SBG_ECOM_SIGNAL_ID_GPS_L1M, "gpsL1M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_ML, "gpsL2C_ML" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_L, "gpsL2C_L" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL, "gpsL2_SEMICL" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_W, "gpsL2_W" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_CA, "gpsL2_CA" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2C_M, "gpsL2C_M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2_PY, "gpsL2_PY" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2M, "gpsL2M" }, + { SBG_ECOM_SIGNAL_ID_GPS_L2P, "gpsL2P" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_IQ, "gpsL5_IQ" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_I, "gpsL5_I" }, + { SBG_ECOM_SIGNAL_ID_GPS_L5_Q, "gpsL5_Q" }, + + { SBG_ECOM_SIGNAL_ID_GLONASS_G1_P, "glonassG1_P" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA, "glonassG1_CA" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G2_P, "glonassG2_P" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA, "glonassG2_CA" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_I, "glonassG3_I" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q, "glonassG3_Q" }, + { SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ, "glonassG3_IQ" }, + + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC, "galileoE1_BC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_C, "galileoE1_C" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_B, "galileoE1_B" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_A, "galileoE1_A" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC, "galileoE1_ABC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ, "galileoE5B_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I, "galileoE5B_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q, "galileoE5B_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ, "galileoE5A_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I, "galileoE5A_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q, "galileoE5A_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ, "galileoE5_IQ" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_I, "galileoE5_I" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q, "galileoE5_Q" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC, "galileoE6_BC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_C, "galileoE6_C" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_B, "galileoE6_B" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC, "galileoE6_ABC" }, + { SBG_ECOM_SIGNAL_ID_GALILEO_E6_A, "galileoE6_A" }, + + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ, "beidouB1IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1I, "beidouB1I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q, "beidouB1Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P, "beidouB1C_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP, "beidouB1C_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D, "beidouB1C_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P, "beidouB1A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP, "beidouB1A_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D, "beidouB1A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ, "beidouB2IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2I, "beidouB2I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P, "beidouB2A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP, "beidouB2A_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D, "beidouB2A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q, "beidouB2Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P, "beidouB2B_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP, "beidouB2B_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D, "beidouB2B_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P, "beidouB2AB_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP, "beidouB2AB_DP" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D, "beidouB2AB_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ, "beidouB3IQ" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3I, "beidouB3I" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q, "beidouB3Q" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D, "beidouB3A_D" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P, "beidouB3A_P" }, + { SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP, "beidouB3A_DP" }, + + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP, "qzssL1C_DP" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_D, "qzssL1C_D" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1C_P, "qzssL1C_P" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_CA, "qzssL1_CA" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF, "qzssL1_SAIF" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L1_SB, "qzssL1_SB" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML, "qzssL2C_ML" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_L, "qzssL2C_L" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L2C_M, "qzssL2C_M" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ, "qzssL5_IQ" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_I, "qzssL5_I" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5_Q, "qzssL5_Q" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ, "qzssL5S_IQ" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_I, "qzssL5S_I" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q, "qzssL5S_Q" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_P, "qzssL6_P" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_DP, "qzssL6_DP" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_D, "qzssL6_D" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_E, "qzssL6_E" }, + { SBG_ECOM_SIGNAL_ID_QZSS_L6_DE, "qzssL6_DE" }, + + { SBG_ECOM_SIGNAL_ID_SBAS_L1_CA, "sbasL1_CA" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_I, "sbasL5_I" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_Q, "sbasL5_Q" }, + { SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ, "sbasL5_IQ" }, + + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_A, "irnssL5_A" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_B, "irnssL5_B" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_C, "irnssL5_C" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC, "irnssL5_BC" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_A, "irnssS9_A" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_B, "irnssS9_B" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_C, "irnssS9_C" }, + { SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC, "irnssS9_BC" }, + + { SBG_ECOM_SIGNAL_ID_LBAND, "lband" } + }; + + for (size_t i = 0; i < SBG_ARRAY_SIZE(signalIdDesc); i++) + { + if (signalIdDesc[i].id == signalId) + { + return signalIdDesc[i].pName; + } + } + + // + // Enforce that the first item is the unknown signal ID and return it + // + assert(signalIdDesc[0].id == SBG_ECOM_SIGNAL_ID_UNKNOWN); + return signalIdDesc[0].pName; +} + +bool sbgEComConstellationIdIsValid(uint8_t constellationId) +{ + bool constellationIdIsValid = false; + + switch (constellationId) + { + case SBG_ECOM_CONSTELLATION_ID_UNKNOWN: + case SBG_ECOM_CONSTELLATION_ID_GPS: + case SBG_ECOM_CONSTELLATION_ID_QZSS: + case SBG_ECOM_CONSTELLATION_ID_GLONASS: + case SBG_ECOM_CONSTELLATION_ID_GALILEO: + case SBG_ECOM_CONSTELLATION_ID_BEIDOU: + case SBG_ECOM_CONSTELLATION_ID_SBAS: + case SBG_ECOM_CONSTELLATION_ID_IRNSS: + case SBG_ECOM_CONSTELLATION_ID_LBAND: + constellationIdIsValid = true; + break; + } + + return constellationIdIsValid; +} + +const char *sbgEComConstellationToStr(SbgEComConstellationId constellationId) +{ + static const char *enumToStrLut[] = + { + [SBG_ECOM_CONSTELLATION_ID_UNKNOWN] = "unknown", + [SBG_ECOM_CONSTELLATION_ID_GPS] = "gps", + [SBG_ECOM_CONSTELLATION_ID_GLONASS] = "glonass", + [SBG_ECOM_CONSTELLATION_ID_GALILEO] = "galileo", + [SBG_ECOM_CONSTELLATION_ID_BEIDOU] = "beidou", + [SBG_ECOM_CONSTELLATION_ID_QZSS] = "qzss", + [SBG_ECOM_CONSTELLATION_ID_SBAS] = "sbas", + [SBG_ECOM_CONSTELLATION_ID_IRNSS] = "irnss", + [SBG_ECOM_CONSTELLATION_ID_LBAND] = "lband", + }; + + if (constellationId < SBG_ARRAY_SIZE(enumToStrLut)) + { + return enumToStrLut[constellationId]; + } + else + { + return enumToStrLut[SBG_ECOM_CONSTELLATION_ID_UNKNOWN]; + } +} diff --git a/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.h b/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.h new file mode 100644 index 0000000..295b7bd --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/defs/sbgEComDefsGnss.h @@ -0,0 +1,265 @@ +/*! + * \file sbgEComDefsGnss.h + * \ingroup main + * \author SBG Systems + * \date 20 September 2022 + * + * \brief Common enumeration and definitions for RAW GNSS data + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_DEFS_GNSS_H +#define SBG_ECOM_DEFS_GNSS_H + +// sbgCommonLib headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Enumeration definitions -// +//----------------------------------------------------------------------// + +/*! + * Signal IDs. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComSignalId +{ + SBG_ECOM_SIGNAL_ID_UNKNOWN = 0, + + // + // GPS constellation (10 to 39) + // + SBG_ECOM_SIGNAL_ID_GPS_L1C_DP = 10, + SBG_ECOM_SIGNAL_ID_GPS_L1C_D = 11, + SBG_ECOM_SIGNAL_ID_GPS_L1C_P = 12, + SBG_ECOM_SIGNAL_ID_GPS_L1_W = 13, + SBG_ECOM_SIGNAL_ID_GPS_L1_CA = 14, + SBG_ECOM_SIGNAL_ID_GPS_L1P = 15, + SBG_ECOM_SIGNAL_ID_GPS_L1_PY = 16, + SBG_ECOM_SIGNAL_ID_GPS_L1M = 17, + SBG_ECOM_SIGNAL_ID_GPS_L2C_ML = 18, + SBG_ECOM_SIGNAL_ID_GPS_L2C_L = 19, + SBG_ECOM_SIGNAL_ID_GPS_L2_SEMICL = 20, + SBG_ECOM_SIGNAL_ID_GPS_L2_W = 21, + SBG_ECOM_SIGNAL_ID_GPS_L2_CA = 22, + SBG_ECOM_SIGNAL_ID_GPS_L2C_M = 23, + SBG_ECOM_SIGNAL_ID_GPS_L2_PY = 24, + SBG_ECOM_SIGNAL_ID_GPS_L2M = 25, + SBG_ECOM_SIGNAL_ID_GPS_L2P = 26, + SBG_ECOM_SIGNAL_ID_GPS_L5_IQ = 27, + SBG_ECOM_SIGNAL_ID_GPS_L5_I = 28, + SBG_ECOM_SIGNAL_ID_GPS_L5_Q = 29, + + // + // GLONASS constellation (40 to 59) + // + SBG_ECOM_SIGNAL_ID_GLONASS_G1_P = 40, + SBG_ECOM_SIGNAL_ID_GLONASS_G1_CA = 41, + SBG_ECOM_SIGNAL_ID_GLONASS_G2_P = 42, + SBG_ECOM_SIGNAL_ID_GLONASS_G2_CA = 43, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_I = 44, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_Q = 45, + SBG_ECOM_SIGNAL_ID_GLONASS_G3_IQ = 46, + + // + // Galileo constellation (60 to 99) + // + SBG_ECOM_SIGNAL_ID_GALILEO_E1_BC = 60, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_C = 61, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_B = 62, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_A = 63, + SBG_ECOM_SIGNAL_ID_GALILEO_E1_ABC = 64, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_IQ = 65, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_I = 66, + SBG_ECOM_SIGNAL_ID_GALILEO_E5B_Q = 67, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_IQ = 68, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_I = 69, + SBG_ECOM_SIGNAL_ID_GALILEO_E5A_Q = 70, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_IQ = 71, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_I = 72, + SBG_ECOM_SIGNAL_ID_GALILEO_E5_Q = 73, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_BC = 74, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_C = 75, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_B = 76, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_ABC = 77, + SBG_ECOM_SIGNAL_ID_GALILEO_E6_A = 78, + + // + // Beidou constellation (100 to 149) + // + SBG_ECOM_SIGNAL_ID_BEIDOU_B1IQ = 100, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1I = 101, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1Q = 102, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_P = 103, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_DP = 104, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1C_D = 105, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_P = 106, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_DP = 107, + SBG_ECOM_SIGNAL_ID_BEIDOU_B1A_D = 108, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2IQ = 109, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2I = 110, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_P = 111, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_DP = 112, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2A_D = 113, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2Q = 114, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_P = 115, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_DP = 116, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2B_D = 117, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_P = 118, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_DP = 119, + SBG_ECOM_SIGNAL_ID_BEIDOU_B2AB_D = 120, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3IQ = 121, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3I = 122, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3Q = 123, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_D = 124, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_P = 125, + SBG_ECOM_SIGNAL_ID_BEIDOU_B3A_DP = 126, + + // + // QZSS constellation (150 to 179) + // + SBG_ECOM_SIGNAL_ID_QZSS_L1C_DP = 150, + SBG_ECOM_SIGNAL_ID_QZSS_L1C_D = 151, + SBG_ECOM_SIGNAL_ID_QZSS_L1C_P = 152, + SBG_ECOM_SIGNAL_ID_QZSS_L1_CA = 153, + SBG_ECOM_SIGNAL_ID_QZSS_L1_SAIF = 154, + SBG_ECOM_SIGNAL_ID_QZSS_L1_SB = 155, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_ML = 156, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_L = 157, + SBG_ECOM_SIGNAL_ID_QZSS_L2C_M = 158, + SBG_ECOM_SIGNAL_ID_QZSS_L5_IQ = 159, + SBG_ECOM_SIGNAL_ID_QZSS_L5_I = 160, + SBG_ECOM_SIGNAL_ID_QZSS_L5_Q = 161, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_IQ = 162, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_I = 163, + SBG_ECOM_SIGNAL_ID_QZSS_L5S_Q = 164, + SBG_ECOM_SIGNAL_ID_QZSS_L6_P = 165, + SBG_ECOM_SIGNAL_ID_QZSS_L6_DP = 166, + SBG_ECOM_SIGNAL_ID_QZSS_L6_D = 167, + SBG_ECOM_SIGNAL_ID_QZSS_L6_E = 168, + SBG_ECOM_SIGNAL_ID_QZSS_L6_DE = 169, + + // + // SBAS system (180 to 199) + // + SBG_ECOM_SIGNAL_ID_SBAS_L1_CA = 180, + SBG_ECOM_SIGNAL_ID_SBAS_L5_I = 181, + SBG_ECOM_SIGNAL_ID_SBAS_L5_Q = 182, + SBG_ECOM_SIGNAL_ID_SBAS_L5_IQ = 183, + + // + // IRNSS / NAVIC constellation (200 to 219) + // + SBG_ECOM_SIGNAL_ID_IRNSS_L5_A = 200, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_B = 201, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_C = 202, + SBG_ECOM_SIGNAL_ID_IRNSS_L5_BC = 203, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_A = 204, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_B = 205, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_C = 206, + SBG_ECOM_SIGNAL_ID_IRNSS_S9_BC = 207, + + // + // L-Band system (220 to 230) + // + SBG_ECOM_SIGNAL_ID_LBAND = 220, +} SbgEComSignalId; + +/*! + * Constellation IDs. + * + * All values must be strictly lower than 16. + * + * These are on-the-wire values. + */ +typedef enum _SbgEComConstellationId +{ + SBG_ECOM_CONSTELLATION_ID_UNKNOWN = 0, + SBG_ECOM_CONSTELLATION_ID_GPS = 1, + SBG_ECOM_CONSTELLATION_ID_GLONASS = 2, + SBG_ECOM_CONSTELLATION_ID_GALILEO = 3, + SBG_ECOM_CONSTELLATION_ID_BEIDOU = 4, + SBG_ECOM_CONSTELLATION_ID_QZSS = 5, + SBG_ECOM_CONSTELLATION_ID_SBAS = 6, + SBG_ECOM_CONSTELLATION_ID_IRNSS = 7, + SBG_ECOM_CONSTELLATION_ID_LBAND = 8, +} SbgEComConstellationId; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Returns a constellation given a signal ID + * + * \param[in] signalId Signal ID value. + * \return Constellation this signal belongs to. + */ +SbgEComConstellationId sbgEComGetConstellationFromSignalId(SbgEComSignalId signalId); + +/*! + * Check if a value belongs to SbgEComSignalId enum. + * + * WARNING: SBG_ECOM_SIGNAL_ID_UNKNOWN is considered to be a valid enum value. + * + * \param[in] signalId Signal ID value. + * \return true if the value is valid + */ +bool sbgEComSignalIdIsValid(uint8_t signalId); + +/*! + * Get a signal ID as a read only C string. + * + * \param[in] signalId Signal ID value. + * \return Signal ID as a read only C string. + */ +const char *sbgEComSignalToStr(SbgEComSignalId signalId); + +/*! + * Check if a value belongs to SbgEComConstellationId enum. + * + * \param[in] constellationId constellation ID value. + * \return true if the value is valid + */ +bool sbgEComConstellationIdIsValid(uint8_t constellationId); + +/*! + * Get a constellation ID as a read only C string. + * + * \param[in] constellationId Constellation ID value. + * \return Constellation ID as a read only C string. + */ +const char *sbgEComConstellationToStr(SbgEComConstellationId constellationId); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_DEFS_GNSS_H diff --git a/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c b/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c new file mode 100644 index 0000000..6766376 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.c @@ -0,0 +1,1201 @@ +// sbgCommonLib headers +#include +#include +#include +#include + +// Local headers +#include "sbgEComProtocol.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Delay before another send attempt when sending a large payload, in milliseconds. + */ +#define SBG_ECOM_PROTOCOL_EXT_SEND_DELAY (50) + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Clear the content of a payload. + * + * Any allocated resource is released, and the payload returns to its constructed state. + * + * \param[in] pPayload Payload. + */ +static void sbgEComProtocolPayloadClear(SbgEComProtocolPayload *pPayload) +{ + assert(pPayload); + + if (pPayload->allocated) + { + free(pPayload->pBuffer); + + pPayload->allocated = false; + } + + pPayload->pBuffer = NULL; + pPayload->size = 0; +} + +/*! + * Set the properties of a payload. + * + * \param[in] pPayload Payload. + * \param[in] allocated True if the given buffer is allocated with malloc(). + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + */ +static void sbgEComProtocolPayloadSet(SbgEComProtocolPayload *pPayload, bool allocated, void *pBuffer, size_t size) +{ + assert(pPayload); + assert(pBuffer); + + pPayload->allocated = allocated; + pPayload->pBuffer = pBuffer; + pPayload->size = size; +} + +/*! + * Discard unused bytes from the work buffer of a protocol. + * + * \param[in] pProtocol Protocol. + */ +static void sbgEComProtocolDiscardUnusedBytes(SbgEComProtocol *pProtocol) +{ + assert(pProtocol); + + if (pProtocol->discardSize != 0) + { + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + + memmove(pProtocol->rxBuffer, &pProtocol->rxBuffer[pProtocol->discardSize], pProtocol->rxBufferSize - pProtocol->discardSize); + + pProtocol->rxBufferSize -= pProtocol->discardSize; + pProtocol->discardSize = 0; + } +} + +/*! + * Read data from the underlying interface into the work buffer of a protocol. + * + * \param[in] pProtocol Protocol. + */ +static void sbgEComProtocolRead(SbgEComProtocol *pProtocol) +{ + SbgErrorCode errorCode; + + assert(pProtocol); + + if (pProtocol->rxBufferSize < sizeof(pProtocol->rxBuffer)) + { + size_t nrBytesRead; + + errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, &pProtocol->rxBuffer[pProtocol->rxBufferSize], &nrBytesRead, sizeof(pProtocol->rxBuffer) - pProtocol->rxBufferSize); + + if (errorCode == SBG_NO_ERROR) + { + pProtocol->rxBufferSize += nrBytesRead; + } + } +} + +/*! + * Find SYNC bytes in the work buffer of a protocol. + * + * The output offset is set if either SBG_NO_ERROR or SBG_NOT_CONTINUOUS_FRAME is returned. + * + * \param[in] pProtocol Protocol. + * \param[in] startOffset Start offset, in bytes. + * \param[out] pOffset Offset, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_CONTINUOUS_FRAME if only the first SYNC byte was found, + * SBG_NOT_READY otherwise. + */ +static SbgErrorCode sbgEComProtocolFindSyncBytes(SbgEComProtocol *pProtocol, size_t startOffset, size_t *pOffset) +{ + SbgErrorCode errorCode; + + assert(pProtocol); + + errorCode = SBG_NOT_READY; + + for (size_t i = startOffset; i < (pProtocol->rxBufferSize - 1); i++) + { + if ((pProtocol->rxBuffer[i] == SBG_ECOM_SYNC_1) && (pProtocol->rxBuffer[i + 1] == SBG_ECOM_SYNC_2)) + { + *pOffset = i; + errorCode = SBG_NO_ERROR; + break; + } + } + + // + // The SYNC bytes were not found, but check if the last byte in the work buffer is the first SYNC byte, + // as it could result from receiving a partial frame. + // + if ((errorCode != SBG_NO_ERROR) && (pProtocol->rxBuffer[pProtocol->rxBufferSize - 1] == SBG_ECOM_SYNC_1)) + { + *pOffset = pProtocol->rxBufferSize - 1; + errorCode = SBG_NOT_CONTINUOUS_FRAME; + } + + return errorCode; +} + +/*! + * Parse a frame in the work buffer of a protocol. + * + * A non-zero number of pages indicates the reception of an extended frame. + * + * \param[in] pProtocol Protocol. + * \param[in] offset Frame offset in the protocol work buffer. + * \param[out] pEndOffset Frame end offset in the protocol work buffer. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pTransferId Transfer ID. + * \param[out] pPageIndex Page index. + * \param[out] pNrPages Number of pages. + * \param[out] pBuffer Payload buffer. + * \param[out] pSize Payload buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if the frame is incomplete, + * SBG_INVALID_FRAME if the frame is invalid, + * SBG_INVALID_CRC if the frame CRC is invalid. + */ +static SbgErrorCode sbgEComProtocolParseFrame(SbgEComProtocol *pProtocol, size_t offset, size_t *pEndOffset, uint8_t *pMsgClass, uint8_t *pMsgId, uint8_t *pTransferId, uint16_t *pPageIndex, uint16_t *pNrPages, void **pBuffer, size_t *pSize) +{ + SbgErrorCode errorCode; + SbgStreamBuffer streamBuffer; + uint8_t msgId; + uint8_t msgClass; + size_t standardPayloadSize; + + assert(pProtocol); + assert(offset < pProtocol->rxBufferSize); + assert(pEndOffset); + assert(pMsgClass); + assert(pMsgId); + assert(pTransferId); + assert(pPageIndex); + assert(pNrPages); + assert(pBuffer); + assert(pSize); + + sbgStreamBufferInitForRead(&streamBuffer, &pProtocol->rxBuffer[offset], pProtocol->rxBufferSize - offset); + + // + // Skip SYNC bytes. + // + sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_CUR_INC); + + msgId = sbgStreamBufferReadUint8(&streamBuffer); + msgClass = sbgStreamBufferReadUint8(&streamBuffer); + standardPayloadSize = sbgStreamBufferReadUint16LE(&streamBuffer); + + errorCode = sbgStreamBufferGetLastError(&streamBuffer); + + if (errorCode == SBG_NO_ERROR) + { + if (standardPayloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) + { + if (sbgStreamBufferGetSize(&streamBuffer) >= (standardPayloadSize + 9)) + { + size_t payloadSize; + uint8_t transferId; + uint16_t pageIndex; + uint16_t nrPages; + + if ((msgClass & 0x80) == 0) + { + payloadSize = standardPayloadSize; + + transferId = 0; + pageIndex = 0; + nrPages = 0; + + errorCode = SBG_NO_ERROR; + } + else + { + msgClass &= 0x7f; + + // + // In extended frames, the payload size includes the extended headers. + // + payloadSize = standardPayloadSize - 5; + + transferId = sbgStreamBufferReadUint8(&streamBuffer); + pageIndex = sbgStreamBufferReadUint16LE(&streamBuffer); + nrPages = sbgStreamBufferReadUint16LE(&streamBuffer); + + if ((transferId & 0xf0) != 0) + { + SBG_LOG_WARNING(SBG_INVALID_FRAME, "reserved bits set in extended headers"); + transferId &= 0xf; + } + + if (pageIndex < nrPages) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid page information : %" PRIu16 "/%" PRIu16, pageIndex, nrPages); + } + } + + if (errorCode == SBG_NO_ERROR) + { + void *pPayloadAddr; + uint16_t frameCrc; + uint8_t lastByte; + + pPayloadAddr = sbgStreamBufferGetCursor(&streamBuffer); + + sbgStreamBufferSeek(&streamBuffer, payloadSize, SB_SEEK_CUR_INC); + + frameCrc = sbgStreamBufferReadUint16LE(&streamBuffer); + lastByte = sbgStreamBufferReadUint8(&streamBuffer); + + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + + if (lastByte == SBG_ECOM_ETX) + { + uint16_t computedCrc; + + // + // The CRC spans from the header (excluding the SYNC bytes) up to the CRC bytes. + // + sbgStreamBufferSeek(&streamBuffer, 2, SB_SEEK_SET); + computedCrc = sbgCrc16Compute(sbgStreamBufferGetCursor(&streamBuffer), standardPayloadSize + 4); + + if (frameCrc == computedCrc) + { + *pEndOffset = offset + standardPayloadSize + 9; + *pMsgClass = msgClass; + *pMsgId = msgId; + *pTransferId = transferId; + *pPageIndex = pageIndex; + *pNrPages = nrPages; + *pBuffer = pPayloadAddr; + *pSize = payloadSize; + + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_INVALID_CRC; + SBG_LOG_ERROR(errorCode, "invalid CRC, frame:%#" PRIx16 " computed:%#" PRIx16, frameCrc, computedCrc); + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid end-of-frame: byte:%#" PRIx8, lastByte); + } + } + } + else + { + errorCode = SBG_NOT_READY; + } + } + else + { + errorCode = SBG_INVALID_FRAME; + SBG_LOG_ERROR(errorCode, "invalid payload size %zu", standardPayloadSize); + } + } + else + { + errorCode = SBG_NOT_READY; + } + + return errorCode; +} + +/*! + * Find a frame in the work buffer of a protocol. + * + * If an extended frame is received, the number of pages is set to a non-zero value. + * + * \param[in] pProtocol Protocol. + * \param[out] pMsgClass Message class. + * \param[out] pMsgId Message ID. + * \param[out] pTransferId Transfer ID. + * \param[out] pPageIndex Page index. + * \param[out] pNrPages Number of pages. + * \param[out] pBuffer Payload buffer. + * \param[out] pSize Payload buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no frame was found. + */ +static SbgErrorCode sbgEComProtocolFindFrame(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, uint8_t *pTransferId, uint16_t *pPageIndex, uint16_t *pNrPages, void **pBuffer, size_t *pSize) +{ + SbgErrorCode errorCode; + size_t startOffset; + + assert(pProtocol); + + errorCode = SBG_NOT_READY; + startOffset = 0; + + while (startOffset < pProtocol->rxBufferSize) + { + size_t offset; + + errorCode = sbgEComProtocolFindSyncBytes(pProtocol, startOffset, &offset); + + if (errorCode == SBG_NO_ERROR) + { + size_t endOffset; + + errorCode = sbgEComProtocolParseFrame(pProtocol, offset, &endOffset, pMsgClass, pMsgId, pTransferId, pPageIndex, pNrPages, pBuffer, pSize); + + if (errorCode == SBG_NO_ERROR) + { + // + // Valid frame found, discard all data up to and including that frame + // on the next read. + // + pProtocol->discardSize = endOffset; + break; + } + else if (errorCode == SBG_NOT_READY) + { + // + // There may be a valid frame at the parse offset, but it's not complete. + // Have all preceding bytes discarded on the next read. + // + pProtocol->discardSize = offset; + break; + } + else + { + // + // Not a valid frame, skip SYNC bytes and try again. + // + startOffset = offset + 2; + errorCode = SBG_NOT_READY; + } + } + else if (errorCode == SBG_NOT_CONTINUOUS_FRAME) + { + // + // The first SYNC byte was found, but not the second. It may be a valid + // frame, so keep the SYNC byte but have all preceding bytes discarded + // on the next read. + // + pProtocol->discardSize = offset; + errorCode = SBG_NOT_READY; + break; + } + else + { + // + // No SYNC byte found, discard all data. + // + pProtocol->rxBufferSize = 0; + errorCode = SBG_NOT_READY; + break; + } + } + + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + + return errorCode; +} + +/*! + * Send a standard frame. + * + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComProtocolSendStandardFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, const void *pData, size_t size) +{ + uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer streamBuffer; + const uint8_t *crcDataStart; + const uint8_t *crcDataEnd; + uint16_t crc; + + assert(pProtocol); + assert((msgClass & 0x80) == 0); + assert(size <= SBG_ECOM_MAX_PAYLOAD_SIZE); + assert(pData || (size == 0)); + + sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); + + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); + + crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); + + sbgStreamBufferWriteUint8(&streamBuffer, msgId); + sbgStreamBufferWriteUint8(&streamBuffer, msgClass); + + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size); + + sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); + + crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); + + crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); + + sbgStreamBufferWriteUint16LE(&streamBuffer, crc); + + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); + + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + + return sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); +} + +/*! + * Send an extended frame. + * + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] transferId Transfer ID. + * \param[in] pageIndex Page index (0 to 65534) + * \param[in] nrPages Total number of pages (1 to 65535) + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode sbgEComProtocolSendExtendedFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, uint8_t transferId, size_t pageIndex, size_t nrPages, const void *pData, size_t size) +{ + SbgErrorCode errorCode; + uint8_t buffer[SBG_ECOM_MAX_BUFFER_SIZE]; + SbgStreamBuffer streamBuffer; + const uint8_t *crcDataStart; + const uint8_t *crcDataEnd; + uint16_t crc; + + assert(pProtocol); + assert((msgClass & 0x80) == 0); + assert((transferId & 0xf0) == 0); + assert(pageIndex < UINT16_MAX); + assert((nrPages > 0) && (nrPages <= UINT16_MAX)); + assert(pageIndex < nrPages); + assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + assert(pData || (size == 0)); + + sbgStreamBufferInitForWrite(&streamBuffer, buffer, sizeof(buffer)); + + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_1); + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_SYNC_2); + + crcDataStart = sbgStreamBufferGetCursor(&streamBuffer); + + sbgStreamBufferWriteUint8(&streamBuffer, msgId); + sbgStreamBufferWriteUint8(&streamBuffer, 0x80 | msgClass); + + // + // For compatibility reasons, the size must span over the extended headers. + // + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)size + 5); + + sbgStreamBufferWriteUint8(&streamBuffer, transferId); + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)pageIndex); + sbgStreamBufferWriteUint16LE(&streamBuffer, (uint16_t)nrPages); + + sbgStreamBufferWriteBuffer(&streamBuffer, pData, size); + + crcDataEnd = sbgStreamBufferGetCursor(&streamBuffer); + + crc = sbgCrc16Compute(crcDataStart, crcDataEnd - crcDataStart); + + sbgStreamBufferWriteUint16LE(&streamBuffer, crc); + + sbgStreamBufferWriteUint8(&streamBuffer, SBG_ECOM_ETX); + + assert(sbgStreamBufferGetLastError(&streamBuffer) == SBG_NO_ERROR); + + for (;;) + { + errorCode = sbgInterfaceWrite(pProtocol->pLinkedInterface, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode != SBG_BUFFER_OVERFLOW) + { + break; + } + + sbgSleep(SBG_ECOM_PROTOCOL_EXT_SEND_DELAY); + } + + return errorCode; +} + +/*! + * Get a transfer ID for the next large send. + * + * This function returns a different ID every time it's called. + * + * \param[in] pProtocol Protocol. + * \return Transfer ID of the next large send. + */ +static SbgErrorCode sbgEComProtocolGetTxId(SbgEComProtocol *pProtocol) +{ + uint8_t transferId; + + assert(pProtocol); + assert((pProtocol->nextLargeTxId & 0xf0) == 0); + + transferId = pProtocol->nextLargeTxId; + pProtocol->nextLargeTxId = (pProtocol->nextLargeTxId + 1) & 0xf0; + + return transferId; +} + +/*! + * Check if a large transfer is in progress. + * + * \param[in] pProtocol Protocol. + * \return True if a large transfer is in progress. + */ +static bool sbgEComProtocolLargeTransferInProgress(const SbgEComProtocol *pProtocol) +{ + assert(pProtocol); + + return pProtocol->pLargeBuffer; +} + +/*! + * Reset The large transfer member variables of a protocol. + * + * \param[in] pProtocol Protocol. + */ +static void sbgEComProtocolResetLargeTransfer(SbgEComProtocol *pProtocol) +{ + assert(pProtocol); + + pProtocol->pLargeBuffer = NULL; + pProtocol->largeBufferSize = 0; + pProtocol->transferId = 0; + pProtocol->pageIndex = 0; + pProtocol->nrPages = 0; +} + +/*! + * Clear any large transfer in progress. + * + * \param[in] pProtocol Protocol. + */ +static void sbgEComProtocolClearLargeTransfer(SbgEComProtocol *pProtocol) +{ + assert(pProtocol); + + free(pProtocol->pLargeBuffer); + + sbgEComProtocolResetLargeTransfer(pProtocol); +} + +/*! + * Process an extended frame. + * + * \param[in] pProtocol Protocol. + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \param[in] transferId Transfer ID. + * \param[in] pageIndex Page index. + * \param[in] nrPages Number of pages. + * \param[in] pBuffer Buffer. + * \param[in] size Buffer size, in bytes. + * \return SBG_NO_ERROR if a large transfer is complete, + * SBG_NOT_READY otherwise. + */ +static SbgErrorCode sbgEComProtocolProcessExtendedFrame(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, uint8_t transferId, uint16_t pageIndex, uint16_t nrPages, const void *pBuffer, size_t size) +{ + SbgErrorCode errorCode; + + assert(pProtocol); + assert((transferId & 0xf0) == 0); + assert(pageIndex < nrPages); + assert(pBuffer || (size == 0)); + assert(size <= SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + + if (pageIndex == 0) + { + size_t capacity; + + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + SBG_LOG_ERROR(SBG_ERROR, "large transfer started while a large transfer is in progress"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + } + + capacity = nrPages * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + + pProtocol->pLargeBuffer = malloc(capacity); + + if (pProtocol->pLargeBuffer) + { + pProtocol->largeBufferSize = 0; + pProtocol->msgClass = msgClass; + pProtocol->msgId = msgId; + pProtocol->transferId = transferId; + pProtocol->pageIndex = 0; + pProtocol->nrPages = nrPages; + + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); + + sbgEComProtocolResetLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + else + { + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "extended frame received while no large transfer in progress"); + + errorCode = SBG_NOT_READY; + } + } + + if (errorCode == SBG_NO_ERROR) + { + if (msgClass == pProtocol->msgClass) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "message class mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (msgId == pProtocol->msgId) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "message ID mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (transferId == pProtocol->transferId) + { + errorCode = SBG_NO_ERROR; + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "transfer ID mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + + if (errorCode == SBG_NO_ERROR) + { + if (nrPages == pProtocol->nrPages) + { + if (pageIndex == pProtocol->pageIndex) + { + size_t offset; + + offset = pageIndex * SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + memcpy(&pProtocol->pLargeBuffer[offset], pBuffer, size); + + pProtocol->largeBufferSize += size; + pProtocol->pageIndex++; + + if (pProtocol->pageIndex != pProtocol->nrPages) + { + errorCode = SBG_NOT_READY; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "extended frame received out of sequence"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + else + { + SBG_LOG_ERROR(SBG_ERROR, "page count mismatch in extended frame"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + + errorCode = SBG_NOT_READY; + } + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComProtocolPayload) -// +//----------------------------------------------------------------------// + +void sbgEComProtocolPayloadConstruct(SbgEComProtocolPayload *pPayload) +{ + assert(pPayload); + + pPayload->allocated = false; + pPayload->pBuffer = NULL; + pPayload->size = 0; +} + +void sbgEComProtocolPayloadDestroy(SbgEComProtocolPayload *pPayload) +{ + assert(pPayload); + + if (pPayload->allocated) + { + free(pPayload->pBuffer); + } +} + +const void *sbgEComProtocolPayloadGetBuffer(const SbgEComProtocolPayload *pPayload) +{ + assert(pPayload); + + return pPayload->pBuffer; +} + +size_t sbgEComProtocolPayloadGetSize(const SbgEComProtocolPayload *pPayload) +{ + assert(pPayload); + + return pPayload->size; +} + +void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload) +{ + void *pBuffer; + + assert(pPayload); + + if (pPayload->pBuffer) + { + if (pPayload->allocated) + { + pBuffer = pPayload->pBuffer; + + sbgEComProtocolPayloadConstruct(pPayload); + } + else + { + pBuffer = malloc(pPayload->size); + + if (pBuffer) + { + memcpy(pBuffer, pPayload->pBuffer, pPayload->size); + + sbgEComProtocolPayloadConstruct(pPayload); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate buffer"); + } + } + } + else + { + pBuffer = NULL; + } + + return pBuffer; +} + +//----------------------------------------------------------------------// +//- Public methods (SbgEComProtocol) -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pProtocol, SbgInterface *pInterface) +{ + assert(pProtocol); + assert(pInterface); + + pProtocol->pLinkedInterface = pInterface; + pProtocol->rxBufferSize = 0; + pProtocol->discardSize = 0; + pProtocol->nextLargeTxId = 0; + + sbgEComProtocolResetLargeTransfer(pProtocol); + + return SBG_NO_ERROR; +} + +SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol) +{ + assert(pProtocol); + + pProtocol->pLinkedInterface = NULL; + pProtocol->rxBufferSize = 0; + pProtocol->discardSize = 0; + pProtocol->nextLargeTxId = 0; + + sbgEComProtocolClearLargeTransfer(pProtocol); + + return SBG_NO_ERROR; +} + +SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + size_t numBytesRead; + uint32_t timeStamp; + + // + // Reset the work buffer + // + pProtocol->rxBufferSize = 0; + pProtocol->discardSize = 0; + pProtocol->nextLargeTxId = 0; + + sbgEComProtocolClearLargeTransfer(pProtocol); + + // + // Try to read all iconming data for at least 100 ms and trash them + /// + timeStamp = sbgGetTime(); + + do + { + errorCode = sbgInterfaceRead(pProtocol->pLinkedInterface, pProtocol->rxBuffer, &numBytesRead, sizeof(pProtocol->rxBuffer)); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "Unable to read data from interface"); + break; + } + } while ((sbgGetTime() - timeStamp) < 100); + + // + // If we still have read some bytes it means we were not able to purge successfully the rx buffer + // + if ( (errorCode == SBG_NO_ERROR) && (numBytesRead > 0) ) + { + errorCode = SBG_ERROR; + SBG_LOG_ERROR(errorCode, "Unable to purge the rx buffer, %zu bytes remaining", numBytesRead); + } + + return errorCode; +} + +SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msgId, const void *pData, size_t size) +{ + SbgErrorCode errorCode; + + if (size <= SBG_ECOM_MAX_PAYLOAD_SIZE) + { + errorCode = sbgEComProtocolSendStandardFrame(pProtocol, msgClass, msgId, pData, size); + } + else + { + size_t nrPages; + + nrPages = sbgDivCeil(size, SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE); + + if (nrPages <= UINT16_MAX) + { + const uint8_t *pBuffer; + size_t offset; + uint8_t transferId; + + pBuffer = pData; + offset = 0; + transferId = sbgEComProtocolGetTxId(pProtocol); + errorCode = SBG_INVALID_PARAMETER; + + for (uint16_t pageIndex = 0; pageIndex < nrPages; pageIndex++) + { + size_t transferSize; + + if ((size - offset) < SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE) + { + transferSize = size - offset; + } + else + { + transferSize = SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE; + } + + errorCode = sbgEComProtocolSendExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, &pBuffer[offset], transferSize); + + if (errorCode != SBG_NO_ERROR) + { + break; + } + + offset += transferSize; + } + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "payload size too large: %zu", size); + } + } + + return errorCode; +} + +SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize) +{ + SbgErrorCode errorCode; + SbgEComProtocolPayload payload; + + sbgEComProtocolPayloadConstruct(&payload); + + errorCode = sbgEComProtocolReceive2(pProtocol, pMsgClass, pMsgId, &payload); + + if (errorCode == SBG_NO_ERROR) + { + size_t size; + + size = sbgEComProtocolPayloadGetSize(&payload); + + if (size <= maxSize) + { + if (pData) + { + const void *pBuffer; + + pBuffer = sbgEComProtocolPayloadGetBuffer(&payload); + + memcpy(pData, pBuffer, size); + } + + if (pSize) + { + *pSize = size; + } + } + else + { + errorCode = SBG_BUFFER_OVERFLOW; + } + } + + sbgEComProtocolPayloadDestroy(&payload); + + return errorCode; +} + +SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload) +{ + SbgErrorCode errorCode; + uint8_t msgClass; + uint8_t msgId; + uint8_t transferId; + uint16_t pageIndex; + uint16_t nrPages; + void *pBuffer; + size_t size; + + assert(pProtocol); + assert(pProtocol->discardSize <= pProtocol->rxBufferSize); + + sbgEComProtocolPayloadClear(pPayload); + + sbgEComProtocolDiscardUnusedBytes(pProtocol); + + sbgEComProtocolRead(pProtocol); + + errorCode = sbgEComProtocolFindFrame(pProtocol, &msgClass, &msgId, &transferId, &pageIndex, &nrPages, &pBuffer, &size); + + if (errorCode == SBG_NO_ERROR) + { + if (nrPages == 0) + { + if (sbgEComProtocolLargeTransferInProgress(pProtocol)) + { + SBG_LOG_ERROR(SBG_ERROR, "standard frame received while a large transfer is in progress"); + SBG_LOG_ERROR(SBG_ERROR, "terminating large transfer"); + + sbgEComProtocolClearLargeTransfer(pProtocol); + } + + if (pMsgClass) + { + *pMsgClass = msgClass; + } + + if (pMsgId) + { + *pMsgId = msgId; + } + + sbgEComProtocolPayloadSet(pPayload, false, pBuffer, size); + } + else + { + errorCode = sbgEComProtocolProcessExtendedFrame(pProtocol, msgClass, msgId, transferId, pageIndex, nrPages, pBuffer, size); + + if (errorCode == SBG_NO_ERROR) + { + if (pMsgClass) + { + *pMsgClass = msgClass; + } + + if (pMsgId) + { + *pMsgId = msgId; + } + + sbgEComProtocolPayloadSet(pPayload, true, pProtocol->pLargeBuffer, pProtocol->largeBufferSize); + sbgEComProtocolResetLargeTransfer(pProtocol); + } + } + } + + return errorCode; +} + +SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor) +{ + assert(pOutputStream); + assert((msgClass & 0x80) == 0); + assert(pStreamCursor); + + // + // Backup the current position in the stream buffer + // + *pStreamCursor = sbgStreamBufferTell(pOutputStream); + + // + // Write the header + // + sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_SYNC_1); + sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_SYNC_2); + + // + // Write the message ID and class + // + sbgStreamBufferWriteUint8LE(pOutputStream, msg); + sbgStreamBufferWriteUint8LE(pOutputStream, msgClass); + + // + // For now, we don't know the payload size so skip it + // + return sbgStreamBufferSeek(pOutputStream, sizeof(uint16_t), SB_SEEK_CUR_INC); +} + +SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor) +{ + SbgErrorCode errorCode; + size_t payloadSize; + size_t currentPos; + uint16_t frameCrc; + + assert(pOutputStream); + + // + // Test if any error has occurred on the stream first + // + errorCode = sbgStreamBufferGetLastError(pOutputStream); + + // + // Is the stream buffer error free ? + // + if (errorCode == SBG_NO_ERROR) + { + // + // Compute the payload size (written data minus the header) + // + payloadSize = sbgStreamBufferGetLength(pOutputStream) - streamCursor - 6; + + // + // Test that the payload size is valid + // + if (payloadSize <= SBG_ECOM_MAX_PAYLOAD_SIZE) + { + // + // Backup the current cursor position + // + currentPos = sbgStreamBufferTell(pOutputStream); + + // + // Goto the payload size field (4th byte in the frame) + // + sbgStreamBufferSeek(pOutputStream, streamCursor+4, SB_SEEK_SET); + + // + // Write the payload size + // + sbgStreamBufferWriteUint16LE(pOutputStream, (uint16_t)payloadSize); + + // + // Go back to the previous position + // + sbgStreamBufferSeek(pOutputStream, currentPos, SB_SEEK_SET); + + // + // Compute the 16 bits CRC on the whole frame except Sync 1 and Sync 2 + // + frameCrc = sbgCrc16Compute((uint8_t*)sbgStreamBufferGetLinkedBuffer(pOutputStream) + streamCursor + 2, payloadSize + 4); + + // + // Append the CRC + // + sbgStreamBufferWriteUint16LE(pOutputStream, frameCrc); + + // + // Append the ETX + // + errorCode = sbgStreamBufferWriteUint8LE(pOutputStream, SBG_ECOM_ETX); + } + else + { + // + // Invalid payload size + // + errorCode = SBG_BUFFER_OVERFLOW; + SBG_LOG_ERROR(errorCode, "Payload of %zu bytes is too big for a valid sbgECom log", payloadSize); + } + } + else + { + // + // Notify error + // + SBG_LOG_ERROR(errorCode, "Unable to finalize frame because of an error on Stream Buffer"); + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.h b/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.h new file mode 100644 index 0000000..bf251d1 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/protocol/sbgEComProtocol.h @@ -0,0 +1,284 @@ +/*! + * \file sbgEComProtocol.h + * \ingroup protocol + * \author SBG Systems + * \date 06 February 2013 + * + * \brief Implementation of the sbgECom binary communication protocol. + * + * You will find below, the frame definition used by Ekinox devices.
+ *
+ * + * + * + * + * + *
Frame structure
Fields SYNC 1 SYNC 2 CMD LEN DATA CRC ETX
Size in bytes 1 1 2 2 (0-4086) 2 1
Value 0xFF 0x5A ? ? ? ? 0x33
+ *
+ * Size in bytes indicates the size of the data field.
+ * The minimum frame size is 9 bytes and the maximum is 512 bytes.
+ *
+ * The CRC is calculated on the whole frame without:
+ * SYNC STX CRC and ETX fields.
+ * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +/*! + * \defgroup protocol Low Level Protocol + * \brief Defines the low level protocol method to receive and send frames. + */ + +#ifndef SBG_ECOM_PROTOCOL_H +#define SBG_ECOM_PROTOCOL_H + +// sbgCommonLib headers +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_MAX_BUFFER_SIZE (4096) /*!< Maximum reception buffer size in bytes. */ +#define SBG_ECOM_MAX_PAYLOAD_SIZE (4086) /*!< Maximum payload size in bytes. */ +#define SBG_ECOM_MAX_EXTENDED_PAYLOAD_SIZE (4081) /*!< Maximum payload size in an extended frame, in bytes. */ +#define SBG_ECOM_SYNC_1 (0xFF) /*!< First synchronization char of the frame. */ +#define SBG_ECOM_SYNC_2 (0x5A) /*!< Second synchronization char of the frame. */ +#define SBG_ECOM_ETX (0x33) /*!< End of frame byte. */ + +#define SBG_ECOM_RX_TIME_OUT (450) /*!< Default time out for new frame reception. */ + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Payload. + * + * This class is used to abstract whether some payload is received from a single + * or multiple frames. + */ +typedef struct _SbgEComProtocolPayload +{ + bool allocated; /*!< True if the buffer is allocated with malloc(). */ + void *pBuffer; /*!< Buffer. */ + size_t size; /*!< Buffer size, in bytes. */ +} SbgEComProtocolPayload; + +/*! + * Struct containing all protocol related data. + * + * The member variables related to large transfers are valid if and only if the large buffer is valid. + */ +typedef struct _SbgEComProtocol +{ + SbgInterface *pLinkedInterface; /*!< Associated interface used by the protocol to read/write bytes. */ + uint8_t rxBuffer[SBG_ECOM_MAX_BUFFER_SIZE]; /*!< The reception buffer. */ + size_t rxBufferSize; /*!< The current reception buffer size in bytes. */ + size_t discardSize; /*!< Number of bytes to discard on the next receive attempt. */ + uint8_t nextLargeTxId; /*!< Transfer ID of the next large send. */ + + // + // Member variables related to large transfer reception. + // + uint8_t *pLargeBuffer; /*!< Buffer for large transfers, allocated with malloc() if valid. */ + size_t largeBufferSize; /*!< Size of the large transfer buffer, in bytes. */ + uint8_t msgClass; /*!< Message class for the current large transfer. */ + uint8_t msgId; /*!< Message ID for the current large transfer. */ + uint8_t transferId; /*!< ID of the current large transfer. */ + uint16_t pageIndex; /*!< Expected page index of the next frame. */ + uint16_t nrPages; /*!< Number of pages in the current transfer. */ +} SbgEComProtocol; + +//----------------------------------------------------------------------// +//- Public methods (SbgEComProtocolPayload) -// +//----------------------------------------------------------------------// + +/*! + * Payload constructor. + * + * \param[in] pPayload Payload. + */ +void sbgEComProtocolPayloadConstruct(SbgEComProtocolPayload *pPayload); + +/*! + * Payload destructor. + * + * \param[in] pPayload Payload. + */ +void sbgEComProtocolPayloadDestroy(SbgEComProtocolPayload *pPayload); + +/*! + * Get the buffer of a payload. + * + * \param[in] pPayload Payload. + * \return Payload buffer. + */ +const void *sbgEComProtocolPayloadGetBuffer(const SbgEComProtocolPayload *pPayload); + +/*! + * Get the size of a payload buffer. + * + * \param[in] pPayload Payload. + * \return Size of the payload buffer, in bytes. + */ +size_t sbgEComProtocolPayloadGetSize(const SbgEComProtocolPayload *pPayload); + +/*! + * Move the buffer of a payload. + * + * If successful, the ownership of the buffer is passed to the caller. Otherwise, the payload + * is unchanged. + * + * The buffer must be released with free() once unused. + * + * \param[in] pPayload Payload. + * \return Payload buffer if successful, NULL otherwise. + */ +void *sbgEComProtocolPayloadMoveBuffer(SbgEComProtocolPayload *pPayload); + +//----------------------------------------------------------------------// +//- Public methods (SbgEComProtocol) -// +//----------------------------------------------------------------------// + +/*! + * Initialize the protocol system used to communicate with the product and return the created handle. + * + * \param[in] pProtocol Pointer on an allocated protocol structure to initialize. + * \param[in] pInterface Interface to use for read/write operations. + * \return SBG_NO_ERROR if we have initialised the protocol system. + */ +SbgErrorCode sbgEComProtocolInit(SbgEComProtocol *pProtocol, SbgInterface *pInterface); + +/*! + * Close the protocol system. + * + * \param[in] pProtocol A valid protocol handle to close. + * \return SBG_NO_ERROR if we have closed and released the protocol system. + */ +SbgErrorCode sbgEComProtocolClose(SbgEComProtocol *pProtocol); + +/*! + * Purge the interface rx buffer as well as the sbgECom rx work buffer. + * + * For example, if the program flow has been interrupted, this method can be helpful to discard all trash received data. + * + * WARNING: This method is blocking for 100ms and actively tries to read incoming data. + * + * \param[in] pProtocol A valid SbgEComProtocol handle. + * \return SBG_NO_ERROR if the incoming data has been purged successfully. + */ +SbgErrorCode sbgEComProtocolPurgeIncoming(SbgEComProtocol *pProtocol); + +/*! + * Send data. + * + * If the size is SBG_ECOM_MAX_PAYLOAD_SIZE or less, the data is sent in a single frame. Otherwise, + * is it fragmented into multiple extended frames, each sent in order, which may block. + * + * \param[in] pProtocol A valid protocol handle. + * \param[in] msgClass Message class. + * \param[in] msg Message ID. + * \param[in] pData Data buffer. + * \param[in] size Data buffer size, in bytes. + * \return SBG_NO_ERROR if the frame has been sent. + */ +SbgErrorCode sbgEComProtocolSend(SbgEComProtocol *pProtocol, uint8_t msgClass, uint8_t msg, const void *pData, size_t size); + +/*! + * Receive a frame. + * + * \param[in] pProtocol A valid protocol handle. + * \param[out] pMsgClass Message class, may be NULL. + * \param[out] pMsgId Message ID, may be NULL. + * \param[out] pData Data buffer. + * \param[out] pSize Number of bytes received. + * \param[in] maxSize Data buffer size, in bytes. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no complete frame has been received, + * SBG_BUFFER_OVERFLOW if the payload of the received frame couldn't fit into the data buffer. + */ +SbgErrorCode sbgEComProtocolReceive(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, void *pData, size_t *pSize, size_t maxSize); + +/*! + * Receive a frame. + * + * This function is equivalent to sbgEComProtocolReceive() with two exceptions : + * - the use of a payload object allows handling payloads not limited by the size of a user-provided buffer + * - the payload object allows direct access to the protocol work buffer to avoid an extra copy per call + * + * Any allocated resource associated with the given payload is released when calling this function. + * + * Because the payload buffer may directly refer to the protocol work buffer on return, it is only valid until + * the next attempt to receive a frame, with any of the receive functions. + * + * \param[in] pProtocol A valid protocol handle. + * \param[out] pMsgClass Message class, may be NULL. + * \param[out] pMsgId Message ID, may be NULL. + * \param[out] pPayload Payload. + * \return SBG_NO_ERROR if successful, + * SBG_NOT_READY if no complete frame has been received. + */ +SbgErrorCode sbgEComProtocolReceive2(SbgEComProtocol *pProtocol, uint8_t *pMsgClass, uint8_t *pMsgId, SbgEComProtocolPayload *pPayload); + +/*! + * Initialize an output stream for an sbgECom frame generation. + * + * This function is helpful to avoid memory copy compared to sbgEComProtocolSend one. + * + * Only standard frames may be sent with this function. + * + * \param[in] pOutputStream Pointer to an allocated and initialized output stream. + * \param[in] msgClass Message class. + * \param[in] msg Message ID. + * \param[out] pStreamCursor The initial output stream cursor that thus points to the begining of the generated message. + * This value should be passed to sbgEComFinalizeFrameGeneration for correct operations. + * \return SBG_NO_ERROR in case of good operation. + */ +SbgErrorCode sbgEComStartFrameGeneration(SbgStreamBuffer *pOutputStream, uint8_t msgClass, uint8_t msg, size_t *pStreamCursor); + +/*! + * Finalize an output stream that has been initialized with sbgEComStartFrameGeneration. + * + * At return, the output stream buffer should point at the end of the generated message. + * You can thus easily create consecutive SBG_ECOM_LOGS with these methods. + * + * \param[in] pOutputStream Pointer to an allocated and initialized output stream. + * \param[in] streamCursor Position in the stream buffer of the generated message first byte. + * This value is returned by sbgEComStartFrameGeneration and is mandatory for correct operations. + * \return SBG_NO_ERROR in case of good operation. + */ +SbgErrorCode sbgEComFinalizeFrameGeneration(SbgStreamBuffer *pOutputStream, size_t streamCursor); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_PROTOCOL_H diff --git a/crates/sbg-rs/sbgECom/src/sbgECanId.h b/crates/sbg-rs/sbgECom/src/sbgECanId.h new file mode 100644 index 0000000..e4790a3 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgECanId.h @@ -0,0 +1,162 @@ +/*! + * \file sbgECanId.h + * \ingroup main + * \author SBG Systems + * \date 10 October 2014 + * + * \brief Defines all sbgECom commands identifiers. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECAN_ID_H +#define SBG_ECAN_ID_H + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Definition of all messages id for sbgECan -// +//----------------------------------------------------------------------// + +/*! + * Enum containing the list of messages that can be output on the can interface. + */ +typedef enum _SbgECanMessageId +{ + // + // Output Messages + // + SBG_ECAN_MSG_STATUS_01 = 0x100, + SBG_ECAN_MSG_STATUS_02 = 0x101, + SBG_ECAN_MSG_STATUS_03 = 0x102, + + SBG_ECAN_MSG_UTC_0 = 0x110, + SBG_ECAN_MSG_UTC_1 = 0x111, + + SBG_ECAN_MSG_IMU_INFO = 0x120, + SBG_ECAN_MSG_IMU_ACCEL = 0x121, + SBG_ECAN_MSG_IMU_GYRO = 0x122, + SBG_ECAN_MSG_IMU_DELTA_VEL = 0x123, + SBG_ECAN_MSG_IMU_DELTA_ANGLE = 0x124, + + SBG_ECAN_MSG_EKF_INFO = 0x130, + SBG_ECAN_MSG_EKF_QUAT = 0x131, + SBG_ECAN_MSG_EKF_EULER = 0x132, + SBG_ECAN_MSG_EKF_ORIENTATION_ACC = 0x133, + SBG_ECAN_MSG_EKF_POS = 0x134, + SBG_ECAN_MSG_EKF_ALTITUDE = 0x135, + SBG_ECAN_MSG_EKF_POS_ACC = 0x136, + SBG_ECAN_MSG_EKF_VEL_NED = 0x137, + SBG_ECAN_MSG_EKF_VEL_NED_ACC = 0x138, + SBG_ECAN_MSG_EKF_VEL_BODY = 0x139, + + SBG_ECAN_MSG_SHIP_MOTION_INFO = 0x140, + SBG_ECAN_MSG_SHIP_MOTION_0 = 0x141, + SBG_ECAN_MSG_SHIP_MOTION_1 = 0x145, + SBG_ECAN_MSG_SHIP_MOTION_2 = 0x149, + + SBG_ECAN_MSG_SHIP_MOTION_HP_INFO = 0x14A, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_SHIP_MOTION_HP_0 = 0x14B, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_SHIP_MOTION_HP_1 = 0x14C, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_SHIP_MOTION_HP_2 = 0x14D, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + + SBG_ECAN_MSG_MAG_0 = 0x150, + SBG_ECAN_MSG_MAG_1 = 0x151, + SBG_ECAN_MSG_MAG_2 = 0x152, + + SBG_ECAN_MSG_ODO_INFO = 0x160, + SBG_ECAN_MSG_ODO_VEL = 0x161, + + SBG_ECAN_MSG_AIR_DATA_INFO = 0x162, + SBG_ECAN_MSG_AIR_DATA_ALTITUDE = 0x163, + SBG_ECAN_MSG_AIR_DATA_AIRSPEED = 0x164, + + SBG_ECAN_MSG_DEPTH_INFO = 0x166, + SBG_ECAN_MSG_DEPTH_ALTITUDE = 0x167, + + SBG_ECAN_MSG_GPS1_VEL_INFO = 0x170, + SBG_ECAN_MSG_GPS1_VEL = 0x171, + SBG_ECAN_MSG_GPS1_VEL_ACC = 0x172, + SBG_ECAN_MSG_GPS1_VEL_COURSE = 0x173, + SBG_ECAN_MSG_GPS1_POS_INFO = 0x174, + SBG_ECAN_MSG_GPS1_POS = 0x175, + SBG_ECAN_MSG_GPS1_POS_ALT = 0x176, + SBG_ECAN_MSG_GPS1_POS_ACC = 0x177, + SBG_ECAN_MSG_GPS1_HDT_INFO = 0x178, + SBG_ECAN_MSG_GPS1_HDT = 0x179, + + SBG_ECAN_MSG_GPS2_VEL_INFO = 0x180, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_VEL = 0x181, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_VEL_ACC = 0x182, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_VEL_COURSE = 0x183, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_POS_INFO = 0x184, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_POS = 0x185, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_POS_ALT = 0x186, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_POS_ACC = 0x187, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_HDT_INFO = 0x188, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_GPS2_HDT = 0x189, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + + SBG_ECAN_MSG_EVENT_INFO_A = 0x200, + SBG_ECAN_MSG_EVENT_TIME_A = 0x201, + SBG_ECAN_MSG_EVENT_INFO_B = 0x202, + SBG_ECAN_MSG_EVENT_TIME_B = 0x203, + SBG_ECAN_MSG_EVENT_INFO_C = 0x204, + SBG_ECAN_MSG_EVENT_TIME_C = 0x205, + SBG_ECAN_MSG_EVENT_INFO_D = 0x206, + SBG_ECAN_MSG_EVENT_TIME_D = 0x207, + SBG_ECAN_MSG_EVENT_INFO_E = 0x208, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + SBG_ECAN_MSG_EVENT_TIME_E = 0x209, /*!< Only for Ekinox, Apogee, Navsight & Quanta */ + + // + // Proprietary CASS logs + // + SBG_ECAN_MSG_CASS_DATINF = 0x210, + SBG_ECAN_MSG_CASS_ACCS = 0x211, + SBG_ECAN_MSG_CASS_OMGS = 0x212, + SBG_ECAN_MSG_CASS_NRPY = 0x213, + SBG_ECAN_MSG_CASS_VEL = 0x214, + SBG_ECAN_MSG_CASS_TIME = 0x215, + SBG_ECAN_MSG_CASS_GPS_INF = 0x216, + SBG_ECAN_MSG_CASS_GPS_COG = 0x217, + SBG_ECAN_MSG_CASS_ADDINF = 0x218, + SBG_ECAN_MSG_CASS_POS1 = 0x219, + SBG_ECAN_MSG_CASS_POS2 = 0x21A, + SBG_ECAN_MSG_CASS_SAT_INF = 0x21B, + SBG_ECAN_MSG_CASS_IACCS = 0x21C, + SBG_ECAN_MSG_CASS_IOMG = 0x21D, + SBG_ECAN_MSG_CASS_RR = 0x21E, + + // + // Automotive specific CAN output + // + SBG_ECAN_MSG_AUTO_TRACK_SLIP_CURV = 0x220, +} SbgECanMessageId; + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECAN_ID_H diff --git a/crates/sbg-rs/sbgECom/src/sbgECom.c b/crates/sbg-rs/sbgECom/src/sbgECom.c new file mode 100644 index 0000000..ec29ecb --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgECom.c @@ -0,0 +1,256 @@ +#include "sbgECom.h" +#include +#include "commands/sbgEComCmdCommon.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pHandle); + assert(pInterface); + + // + // Initialize the sbgECom handle + // + pHandle->pReceiveLogCallback = NULL; + pHandle->pUserArg = NULL; + + // + // Initialize the default number of trials and time out + // + pHandle->numTrials = 3; + pHandle->cmdDefaultTimeOut = SBG_ECOM_DEFAULT_CMD_TIME_OUT; + + // + // Initialize the protocol + // + errorCode = sbgEComProtocolInit(&pHandle->protocolHandle, pInterface); + + return errorCode; +} + +SbgErrorCode sbgEComClose(SbgEComHandle *pHandle) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pHandle); + + // + // Close the protocol + // + errorCode = sbgEComProtocolClose(&pHandle->protocolHandle); + + return errorCode; +} + +SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgBinaryLogData logData; + uint8_t receivedMsg; + uint8_t receivedMsgClass; + size_t payloadSize; + uint8_t payloadData[SBG_ECOM_MAX_PAYLOAD_SIZE]; + + assert(pHandle); + + // + // Try to read a received frame + // + errorCode = sbgEComProtocolReceive(&pHandle->protocolHandle, &receivedMsgClass, &receivedMsg, payloadData, &payloadSize, sizeof(payloadData)); + + // + // Test if we have received a valid frame + // + if (errorCode == SBG_NO_ERROR) + { + // + // Test if the received frame is a binary log + // + if (sbgEComMsgClassIsALog((SbgEComClass)receivedMsgClass)) + { + // + // The received frame is a binary log one + // + errorCode = sbgEComBinaryLogParse((SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg, payloadData, payloadSize, &logData); + + // + // Test if the incoming log has been parsed successfully + // + if (errorCode == SBG_NO_ERROR) + { + // + // Test if we have a valid callback to handle received logs + // + if (pHandle->pReceiveLogCallback) + { + // + // Call the binary log callback using the new method + // + errorCode = pHandle->pReceiveLogCallback(pHandle, (SbgEComClass)receivedMsgClass, receivedMsg, &logData, pHandle->pUserArg); + } + + // + // Clean up resources allocated during parsing, if any. + // + sbgEComBinaryLogCleanup(&logData, (SbgEComClass)receivedMsgClass, (SbgEComMsgId)receivedMsg); + } + else + { + // + // Call the on error callback + // + } + } + else + { + // + // We have received a command, it shouldn't happen + // + } + } + else if (errorCode != SBG_NOT_READY) + { + // + // We have received an invalid frame + // + SBG_LOG_WARNING(errorCode, "Invalid frame received"); + } + + return errorCode; +} + +SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pHandle); + + // + // Try to read all received frames, we thus loop until we get an SBG_NOT_READY error + // + do + { + // + // Try to read and parse one frame + // + errorCode = sbgEComHandleOneLog(pHandle); + } while (errorCode != SBG_NOT_READY); + + return errorCode; +} + +SbgErrorCode sbgEComPurgeIncoming(SbgEComHandle *pHandle) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pHandle); + + errorCode = sbgEComProtocolPurgeIncoming(&pHandle->protocolHandle); + + return errorCode; +} + +void sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg) +{ + assert(pHandle); + + // + // Define the callback and the user argument + // + pHandle->pReceiveLogCallback = pReceiveLogCallback; + pHandle->pUserArg = pUserArg; +} + +void sbgEComSetCmdTrialsAndTimeOut(SbgEComHandle *pHandle, uint32_t numTrials, uint32_t cmdDefaultTimeOut) +{ + assert(pHandle); + assert(numTrials > 0); + assert(cmdDefaultTimeOut > 0); + + // + // Define the new settings + // + pHandle->numTrials = numTrials; + pHandle->cmdDefaultTimeOut = cmdDefaultTimeOut; +} + +void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]) +{ + if (errorMsg) + { + // + // For each error code, copy the error msg + // + switch (errorCode) + { + case SBG_NO_ERROR: + strcpy(errorMsg, "SBG_NO_ERROR: No error."); + break; + case SBG_ERROR: + strcpy(errorMsg, "SBG_ERROR: Generic error."); + break; + case SBG_NULL_POINTER: + strcpy(errorMsg, "SBG_NULL_POINTER: A pointer is null."); + break; + case SBG_INVALID_CRC: + strcpy(errorMsg, "SBG_INVALID_CRC: The received frame has an invalid CRC."); + break; + case SBG_INVALID_FRAME: + strcpy(errorMsg, "SBG_INVALID_FRAME: The received frame is invalid."); + break; + case SBG_TIME_OUT: + strcpy(errorMsg, "SBG_TIME_OUT: We have a time out during frame reception."); + break; + case SBG_WRITE_ERROR: + strcpy(errorMsg, "SBG_WRITE_ERROR: All bytes hasn't been written."); + break; + case SBG_READ_ERROR: + strcpy(errorMsg, "SBG_READ_ERROR: All bytes hasn't been read."); + break; + case SBG_BUFFER_OVERFLOW: + strcpy(errorMsg, "SBG_BUFFER_OVERFLOW: A buffer is too small to contain so much data."); + break; + case SBG_INVALID_PARAMETER: + strcpy(errorMsg, "SBG_INVALID_PARAMETER: An invalid parameter has been founded."); + break; + case SBG_NOT_READY: + strcpy(errorMsg, "SBG_NOT_READY: A device isn't ready (Rx isn't ready for example)."); + break; + case SBG_MALLOC_FAILED: + strcpy(errorMsg, "SBG_MALLOC_FAILED: Failed to allocate a buffer."); + break; + case SGB_CALIB_MAG_NOT_ENOUGH_POINTS: + strcpy(errorMsg, "SGB_CALIB_MAG_NOT_ENOUGH_POINTS: Not enough points were available to perform magnetometers calibration."); + break; + case SBG_CALIB_MAG_INVALID_TAKE: + strcpy(errorMsg, "SBG_CALIB_MAG_INVALID_TAKE: The calibration procedure could not be properly executed due to insufficient precision."); + break; + case SBG_CALIB_MAG_SATURATION: + strcpy(errorMsg, "SBG_CALIB_MAG_SATURATION: Saturation were detected when attempt to calibrate magnetos."); + break; + case SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE: + strcpy(errorMsg, "SBG_CALIB_MAG_POINTS_NOT_IN_A_PLANE: 2D calibration procedure could not be performed."); + break; + case SBG_DEVICE_NOT_FOUND: + strcpy(errorMsg, "SBG_DEVICE_NOT_FOUND: A device couldn't be founded or opened."); + break; + case SBG_OPERATION_CANCELLED: + strcpy(errorMsg, "SBG_OPERATION_CANCELLED: An operation has been cancelled by a user."); + break; + case SBG_NOT_CONTINUOUS_FRAME: + strcpy(errorMsg, "SBG_NOT_CONTINUOUS_FRAME: We have received a frame that isn't a continuous one."); + break; + case SBG_INCOMPATIBLE_HARDWARE: + strcpy(errorMsg, "SBG_INCOMPATIBLE_HARDWARE: Hence valid, the configuration cannot be executed because of incompatible hardware."); + break; + default: + sprintf(errorMsg, "Undefined error code: %u", errorCode); + break; + } + } +} diff --git a/crates/sbg-rs/sbgECom/src/sbgECom.h b/crates/sbg-rs/sbgECom/src/sbgECom.h new file mode 100644 index 0000000..17a7689 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgECom.h @@ -0,0 +1,177 @@ +/*! + * \file sbgECom.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 + * + * \brief Contains main sbgECom methods. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +/*! + * \defgroup main Main Library + * \brief Main sbgECom library initialization and parsing methods. + */ + +#ifndef SBG_ECOM_H +#define SBG_ECOM_H + +#ifdef __cplusplus +extern "C" { +#endif + +// sbgCommonLib headers +#include + +// Local headers +#include "sbgECanId.h" +#include "sbgEComIds.h" +#include "protocol/sbgEComProtocol.h" +#include "binaryLogs/sbgEComBinaryLogs.h" + +//----------------------------------------------------------------------// +//- Predefinitions -// +//----------------------------------------------------------------------// + +/*! + * Interface structure pre-definition. + */ +typedef struct _SbgEComHandle SbgEComHandle; + +//----------------------------------------------------------------------// +//- Callbacks definitions -// +//----------------------------------------------------------------------// + +/*! + * Callback definition called each time a new log is received. + * + * \param[in] pHandle Valid handle on the sbgECom instance that has called this callback. + * \param[in] msgClass Class of the message we have received + * \param[in] msg Message ID of the log received. + * \param[in] pLogData Contains the received log data as an union. + * \param[in] pUserArg Optional user supplied argument. + * \return SBG_NO_ERROR if the received log has been used successfully. + */ +typedef SbgErrorCode (*SbgEComReceiveLogFunc)(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg); + +//----------------------------------------------------------------------// +//- Structures definitions -// +//----------------------------------------------------------------------// + +/*! + * Interface definition that stores methods used to communicate on the interface. + */ +struct _SbgEComHandle +{ + SbgEComProtocol protocolHandle; /*!< Handle on the protocol system. */ + + SbgEComReceiveLogFunc pReceiveLogCallback; /*!< Pointer on the method called each time a new binary log is received. */ + void *pUserArg; /*!< Optional user supplied argument for callbacks. */ + + uint32_t numTrials; /*!< Number of trials when a command is sent (default is 3). */ + uint32_t cmdDefaultTimeOut; /*!< Default time out in ms to get an answer from the device (default 500 ms). */ +}; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Initialize the protocol system used to communicate with the product and return the created handle. + * + * \param[out] pHandle Pointer used to store the allocated and initialized sbgECom handle. + * \param[in] pInterface Interface to use for read/write operations. + * \return SBG_NO_ERROR if we have initialized the protocol system. + */ +SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface); + +/*! + * Close the protocol system and release associated memory. + * + * \param[in] pHandle A valid sbgECom handle to close. + * \return SBG_NO_ERROR if we have closed and released the sbgECom system. + */ +SbgErrorCode sbgEComClose(SbgEComHandle *pHandle); + +/*! + * Try to parse one log from the input interface and then return. + * + * \param[in] pHandle A valid sbgECom handle. + * \return SBG_NO_ERROR if no error occurs during incoming log parsing. + */ +SbgErrorCode sbgEComHandleOneLog(SbgEComHandle *pHandle); + +/*! + * Handle all incoming logs until no more log are available in the input interface. + * + * \param[in] pHandle A valid sbgECom handle. + * \return SBG_NO_ERROR if no error occurs during incoming logs parsing. + */ +SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle); + +/*! + * Purge the interface rx buffer as well as the sbgECom rx work buffer. + * + * For example, if the program flow has been interrupted, this method can be helpful to discard all trash received data. + * + * WARNING: This method is blocking for 100ms and actively tries to read incoming data. + * + * \param[in] pHandle A valid sbgECom handle. + * \return SBG_NO_ERROR if the incoming data has been purged successfully. + */ +SbgErrorCode sbgEComPurgeIncoming(SbgEComHandle *pHandle); + +/*! + * Define the callback that should be called each time a new binary log is received. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] pReceiveLogCallback Pointer on the callback to call when a new log is received. + * \param[in] pUserArg Optional user argument that will be passed to the callback method. + */ +void sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg); + +/*! + * Define the default number of trials that should be done when a command is send to the device as well as the time out. + * + * \param[in] pHandle A valid sbgECom handle. + * \param[in] numTrials Number of trials when a command is sent (starting at 1). + * \param[in] cmdDefaultTimeOut Default time out in milliseconds to wait to receive an answer from the device. + */ +void sbgEComSetCmdTrialsAndTimeOut(SbgEComHandle *pHandle, uint32_t numTrials, uint32_t cmdDefaultTimeOut); + +/*! + * Convert an error code into a human readable string. + * + * \param[in] errorCode The errorCode to convert into a string. + * \param[out] errorMsg String buffer used to hold the error string. + */ +void sbgEComErrorToString(SbgErrorCode errorCode, char errorMsg[256]); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_H + diff --git a/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.c b/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.c new file mode 100644 index 0000000..8c2f207 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.c @@ -0,0 +1,23 @@ +// sbgCommonLib headers +#include +#include + +// Project headers +#include "sbgEComVersion.h" + +// Local headers +#include "sbgEComGetVersion.h" + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +uint32_t sbgEComGetVersion(void) +{ + return SBG_E_COM_VERSION; +} + +const char *sbgEComGetVersionAsString(void) +{ + return SBG_E_COM_VERSION_STR; +} diff --git a/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.h b/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.h new file mode 100644 index 0000000..6f628b7 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgEComGetVersion.h @@ -0,0 +1,64 @@ +/*! + * \file sbgEComGetVersion.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 + * \brief Version information. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_GET_VERSION_H +#define SBG_ECOM_GET_VERSION_H + +// sbgCommonLib headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Returns an integer representing the version of the sbgECom library. + * + * \return An integer representing the version of the sbgECom library.
+ */ +uint32_t sbgEComGetVersion(void); + +/*! + * Retrieve the sbgECom library version as a string (1.0.443-stable). + * + * \return Null terminated string that contains the sbgECom library version. + */ +const char *sbgEComGetVersionAsString(void); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_GET_VERSION_H diff --git a/crates/sbg-rs/sbgECom/src/sbgEComIds.h b/crates/sbg-rs/sbgECom/src/sbgEComIds.h new file mode 100644 index 0000000..dfe39f8 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgEComIds.h @@ -0,0 +1,341 @@ +/*! + * \file sbgEComIds.h + * \ingroup main + * \author SBG Systems + * \date 25 February 2013 + * + * \brief Defines all sbgECom commands identifiers. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_IDS_H +#define SBG_ECOM_IDS_H + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Definition of all class id for sbgECom -// +//----------------------------------------------------------------------// + +/*! + * Enum that defines all the message classes available. + * + * Keep in mind that message classes are encoded on 7 bits. + */ +typedef enum _SbgEComClass +{ + SBG_ECOM_CLASS_LOG_ECOM_0 = 0x00, /*!< Class that contains sbgECom protocol input/output log messages. */ + + SBG_ECOM_CLASS_LOG_ECOM_1 = 0x01, /*!< Class that contains special sbgECom output messages that handle high frequency output. */ + + SBG_ECOM_CLASS_LOG_NMEA_0 = 0x02, /*!< Class that contains NMEA (and NMEA like) output logs.
+ Note: This class is only used for identification purpose and does not contain any sbgECom message. */ + SBG_ECOM_CLASS_LOG_NMEA_1 = 0x03, /*!< Class that contains proprietary NMEA (and NMEA like) output logs.
+ Note: This class is only used for identification purpose and does not contain any sbgECom message. */ + SBG_ECOM_CLASS_LOG_THIRD_PARTY_0 = 0x04, /*!< Class that contains third party output logs. + Note: This class is only used for identification purpose and does not contain any sbgECom message. */ + + SBG_ECOM_CLASS_LOG_CMD_0 = 0x10, /*!< Class that contains sbgECom protocol commands. */ + +} SbgEComClass; + +//----------------------------------------------------------------------// +//- Definition of all messages id for sbgECom -// +//----------------------------------------------------------------------// + +/*! + * Enum that defines all the available ECom output logs from the sbgECom library. + */ +typedef enum _SbgEComLog +{ + SBG_ECOM_LOG_STATUS = 1, /*!< Status general, clock, com aiding, solution, heave */ + + SBG_ECOM_LOG_UTC_TIME = 2, /*!< Provides UTC time reference */ + + SBG_ECOM_LOG_IMU_DATA = 3, /*!< Includes IMU status, acc., gyro, temp delta speeds and delta angles values */ + + SBG_ECOM_LOG_MAG = 4, /*!< Magnetic data with associated accelerometer on each axis */ + SBG_ECOM_LOG_MAG_CALIB = 5, /*!< Magnetometer calibration data (raw buffer) */ + + SBG_ECOM_LOG_EKF_EULER = 6, /*!< Includes roll, pitch, yaw and their accuracies on each axis */ + SBG_ECOM_LOG_EKF_QUAT = 7, /*!< Includes the 4 quaternions values */ + SBG_ECOM_LOG_EKF_NAV = 8, /*!< Position and velocities in NED coordinates with the accuracies on each axis */ + + SBG_ECOM_LOG_SHIP_MOTION = 9, /*!< Heave, surge and sway and accelerations on each axis. */ + + SBG_ECOM_LOG_GPS1_VEL = 13, /*!< GPS velocities from primary or secondary GPS receiver */ + SBG_ECOM_LOG_GPS1_POS = 14, /*!< GPS positions from primary or secondary GPS receiver */ + SBG_ECOM_LOG_GPS1_HDT = 15, /*!< GPS true heading from dual antenna system */ + SBG_ECOM_LOG_GPS1_RAW = 31, /*!< GPS 1 raw data for post processing. */ + SBG_ECOM_LOG_GPS1_SAT = 50, /*!< GPS 1 Satellite data. */ + + SBG_ECOM_LOG_GPS2_VEL = 16, /*!< GPS 2 velocity log data. */ + SBG_ECOM_LOG_GPS2_POS = 17, /*!< GPS 2 position log data. */ + SBG_ECOM_LOG_GPS2_HDT = 18, /*!< GPS 2 true heading log data. */ + SBG_ECOM_LOG_GPS2_RAW = 38, /*!< GPS 2 raw data for post processing. */ + SBG_ECOM_LOG_GPS2_SAT = 51, /*!< GNSS2 Satellite data. */ + + SBG_ECOM_LOG_ODO_VEL = 19, /*!< Provides odometer velocity */ + + SBG_ECOM_LOG_EVENT_A = 24, /*!< Event markers sent when events are detected on sync in A pin */ + SBG_ECOM_LOG_EVENT_B = 25, /*!< Event markers sent when events are detected on sync in B pin */ + SBG_ECOM_LOG_EVENT_C = 26, /*!< Event markers sent when events are detected on sync in C pin */ + SBG_ECOM_LOG_EVENT_D = 27, /*!< Event markers sent when events are detected on sync in D pin */ + SBG_ECOM_LOG_EVENT_E = 28, /*!< Event markers sent when events are detected on sync in E pin */ + + SBG_ECOM_LOG_DVL_BOTTOM_TRACK = 29, /*!< Doppler Velocity Log for bottom tracking data. */ + SBG_ECOM_LOG_DVL_WATER_TRACK = 30, /*!< Doppler Velocity log for water layer data. */ + + SBG_ECOM_LOG_SHIP_MOTION_HP = 32, /*!< Return delayed ship motion such as surge, sway, heave. */ + + SBG_ECOM_LOG_AIR_DATA = 36, /*!< Air Data aiding such as barometric altimeter and true air speed. */ + + SBG_ECOM_LOG_USBL = 37, /*!< Raw USBL position data for subsea navigation. */ + + + SBG_ECOM_LOG_IMU_SHORT = 44, /*!< Short IMU message recommended for post processing usages. */ + + SBG_ECOM_LOG_EVENT_OUT_A = 45, /*!< Event marker used to time stamp each generated Sync Out A signal. */ + SBG_ECOM_LOG_EVENT_OUT_B = 46, /*!< Event marker used to time stamp each generated Sync Out B signal. */ + + SBG_ECOM_LOG_DEPTH = 47, /*!< Depth sensor measurement log used for subsea navigation. */ + SBG_ECOM_LOG_DIAG = 48, /*!< Diagnostic log. */ + + SBG_ECOM_LOG_RTCM_RAW = 49, /*!< RTCM raw data. */ + + SBG_ECOM_LOG_ECOM_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ +} SbgEComLog; + +/*! + * Enum that defines all the available ECom output logs in the class SBG_ECOM_CLASS_LOG_ECOM_1 + */ +typedef enum _SbgEComLog1MsgId +{ + SBG_ECOM_LOG_FAST_IMU_DATA = 0, /*!< Provides accelerometers, gyroscopes, time and status at 1KHz rate. */ + SBG_ECOM_LOG_ECOM_1_NUM_MESSAGES /*!< Helper definition to know the number of ECom messages */ +} SbgEComLog1; + +/*! + * Enum that defines all the available Nmea output logs from the sbgECom library. + */ +typedef enum _SbgEComNmeaLog +{ + SBG_ECOM_LOG_NMEA_GGA = 0, /*!< Latitude, Longitude, Altitude, Quality indicator. */ + SBG_ECOM_LOG_NMEA_RMC = 1, /*!< Latitude, Longitude, velocity, course over ground. */ + SBG_ECOM_LOG_NMEA_ZDA = 2, /*!< UTC Time. */ + SBG_ECOM_LOG_NMEA_HDT = 3, /*!< Heading (True). */ + SBG_ECOM_LOG_NMEA_GST = 4, /*!< GPS Pseudorange Noise Statistics. */ + SBG_ECOM_LOG_NMEA_VBW = 5, /*!< Water referenced and ground referenced speed data. */ + SBG_ECOM_LOG_NMEA_DPT = 7, /*!< Depth sensor output. */ + SBG_ECOM_LOG_NMEA_VTG = 8, /*!< Track an Speed over the ground. */ + SBG_ECOM_LOG_NMEA_RTO = 9, /*!< Rate and direction of turn. */ + SBG_ECOM_LOG_NMEA_GSV = 10, /*!< GNSS Satellites in View with azimuth, elevation and SNR information */ + SBG_ECOM_LOG_NMEA_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ +} SbgEComNmeaLog; + +/*! + * Enum that defines all the available Proprietary Nmea output logs from the sbgECom library. + */ +typedef enum _SbgEComIdNmea1Log +{ + SBG_ECOM_LOG_NMEA_1_PRDID = 0, /*!< RDI proprietary sentence. Pitch, Roll, Heading */ + SBG_ECOM_LOG_NMEA_1_PSBGI = 1, /*!< SBG Systems proprietary sentence. Rotation rates, accelerations. */ + SBG_ECOM_LOG_NMEA_1_PASHR = 2, /*!< Proprietary sentence. Roll, Pitch, Heading, Heave. */ + SBG_ECOM_LOG_NMEA_1_PSBGB = 3, /*!< SBG Systems proprietary sentence. Attitude, heading, heave, angular rates, velocity. */ + + SBG_ECOM_LOG_NMEA_1_PHINF = 5, /*!< Ixblue NMEA like log used to output Status information. */ + SBG_ECOM_LOG_NMEA_1_PHTRO = 6, /*!< Ixblue NMEA like log used to output Roll and Pitch. */ + SBG_ECOM_LOG_NMEA_1_PHLIN = 7, /*!< Ixblue NMEA like log used to output Surge, Sway and Heave. */ + SBG_ECOM_LOG_NMEA_1_PHOCT = 8, /*!< Ixblue NMEA like log used to output attitude and ship motion. */ + SBG_ECOM_LOG_NMEA_1_INDYN = 9, /*!< Ixblue NMEA like log used to output position, heading, attitude, attitude rate and speed. */ + + SBG_ECOM_LOG_NMEA_1_GGK = 10, /*!< Trimble NMEA like log with Time, Latitude, Longitude, Ellipsoidal height */ + SBG_ECOM_LOG_NMEA_1_PPS = 11, /*!< Trimble (Applanix) NMEA like log with UTC and PPS information. */ + + SBG_ECOM_LOG_NMEA_1_WASSP = 12, /*!< WASSP NMEA like log similar to PASHR one. */ + + SBG_ECOM_LOG_NMEA_1_PSBGA = 13, /*!< SBG Systems proprietary sentence that reports EKF attitude and status. */ + + SBG_ECOM_LOG_NMEA_1_NUM_MESSAGES /*!< Helper definition to know the number of NMEA messages */ +} SbgEComIdNmea1Log; + +/*! + * Enum that defines all the available Proprietary output logs from the sbgECom library. + */ +typedef enum _SbgEComIdThirdParty +{ + SBG_ECOM_THIRD_PARTY_TSS1 = 0, /*!< Roll, Pitch, Heave, heave accelerations */ + SBG_ECOM_THIRD_PARTY_KVH = 1, /*!< Roll, Pitch, Yaw */ + + SBG_ECOM_THIRD_PARTY_PD0 = 2, /*!< Teledyne PD0 DVL proprietary frame. */ + SBG_ECOM_THIRD_PARTY_SIMRAD_1000 = 3, /*!< Konsberg SimRad 1000 proprietary frame that outputs Roll, Pitch and Heading. */ + SBG_ECOM_THIRD_PARTY_SIMRAD_3000 = 4, /*!< Konsberg SimRad 3000 proprietary frame that outputs Roll, Pitch and Heading. */ + + SBG_ECOM_THIRD_PARTY_SEAPATH_B26 = 5, /*!< Konsberg Seapth Binary Log 26 used for MBES FM mode. */ + SBG_ECOM_THIRD_PARTY_DOLOG_HRP = 6, /*!< DOLOG Heading, Roll, Pitch proprietary and binary message. */ + SBG_ECOM_THIRD_PARTY_AHRS_500 = 7, /*!< Crossbow AHRS-500 Data Packet output with attitude, rate, acceleration and status. */ + SBG_ECOM_THIRD_PARTY_ADA_01 = 8, /*!< ADA specific Data Packet with IMU/INS/Status data */ + + SBG_ECOM_THIRD_PARTY_AT_ITINS = 9, /*!< Cobham Aviator UAV 200 navigation (orientation & position) data */ + + SBG_ECOM_THIRD_PARTY_KONGSBERG_MB = 10, /*!< Kongsberg multibeam binary log. */ + + SBG_ECOM_LOG_THIRD_PARTY_NUM_MESSAGES /*!< Helper definition to know the number of third party messages */ +} SbgEComIdThirdParty; + + +/*! + * Enum that defines all the available commands for the sbgECom library. + */ +typedef enum _SbgEComCmd +{ + /* Acknowledge */ + SBG_ECOM_CMD_ACK = 0, /*!< Acknowledge */ + + /* Special settings commands */ + SBG_ECOM_CMD_SETTINGS_ACTION = 1, /*!< Performs various settings actions */ + SBG_ECOM_CMD_IMPORT_SETTINGS = 2, /*!< Imports a new settings structure to the sensor */ + SBG_ECOM_CMD_EXPORT_SETTINGS = 3, /*!< Export the whole configuration from the sensor */ + + /* Device info */ + SBG_ECOM_CMD_INFO = 4, /*!< Get basic device information */ + + /* Sensor parameters */ + SBG_ECOM_CMD_INIT_PARAMETERS = 5, /*!< Initial configuration */ + SBG_ECOM_CMD_MOTION_PROFILE_ID = 7, /*!< Set/get motion profile information */ + SBG_ECOM_CMD_IMU_ALIGNMENT_LEVER_ARM = 8, /*!< Sensor alignment and lever arm on vehicle configuration */ + SBG_ECOM_CMD_AIDING_ASSIGNMENT = 9, /*!< Aiding assignments such as RTCM / GPS / Odometer configuration */ + + /* Magnetometer configuration */ + SBG_ECOM_CMD_MAGNETOMETER_MODEL_ID = 11, /*!< Set/get magnetometer error model information */ + SBG_ECOM_CMD_MAGNETOMETER_REJECT_MODE = 12, /*!< Magnetometer aiding rejection mode */ + SBG_ECOM_CMD_SET_MAG_CALIB = 13, /*!< Set magnetic soft and hard Iron calibration data */ + + /* Magnetometer on-board calibration */ + SBG_ECOM_CMD_START_MAG_CALIB = 14, /*!< Start / reset internal magnetic field logging for calibration. */ + SBG_ECOM_CMD_COMPUTE_MAG_CALIB = 15, /*!< Compute a magnetic calibration based on previously logged data. */ + + /* GNSS configuration */ + SBG_ECOM_CMD_GNSS_1_MODEL_ID = 17, /*!< Set/get GNSS model information */ + SBG_ECOM_CMD_GNSS_1_LEVER_ARM_ALIGNMENT = 18, /*!< DEPRECATED: GNSS installation configuration (lever arm, antenna alignments) */ + SBG_ECOM_CMD_GNSS_1_INSTALLATION = 46, /*!< Define or retrieve the GNSS 1 main and secondary lever arms configuration. */ + SBG_ECOM_CMD_GNSS_1_REJECT_MODES = 19, /*!< GNSS aiding rejection modes configuration. */ + + /* Odometer configuration */ + SBG_ECOM_CMD_ODO_CONF = 20, /*!< Odometer gain, direction configuration */ + SBG_ECOM_CMD_ODO_LEVER_ARM = 21, /*!< Odometer installation configuration (lever arm) */ + SBG_ECOM_CMD_ODO_REJECT_MODE = 22, /*!< Odometer aiding rejection mode configuration. */ + + /* Interfaces configuration */ + SBG_ECOM_CMD_UART_CONF = 23, /*!< UART interfaces configuration */ + SBG_ECOM_CMD_CAN_BUS_CONF = 24, /*!< CAN bus interface configuration */ + SBG_ECOM_CMD_CAN_OUTPUT_CONF = 25, /*!< CAN identifiers configuration */ + + /* Events configuration */ + SBG_ECOM_CMD_SYNC_IN_CONF = 26, /*!< Synchronization inputs configuration */ + SBG_ECOM_CMD_SYNC_OUT_CONF = 27, /*!< Synchronization outputs configuration */ + + /* Output configuration */ + SBG_ECOM_CMD_NMEA_TALKER_ID = 29, /*!< NMEA talker ID configuration */ + SBG_ECOM_CMD_OUTPUT_CONF = 30, /*!< Output configuration */ + + /* Advanced configuration */ + SBG_ECOM_CMD_ADVANCED_CONF = 32, /*!< Advanced settings configuration */ + + /* Features related commands */ + SBG_ECOM_CMD_FEATURES = 33, /*!< Retrieve device features */ + + /* Licenses related commands */ + SBG_ECOM_CMD_LICENSE_APPLY = 34, /*!< Upload and apply a new license */ + + /* Message class output switch */ + SBG_ECOM_CMD_OUTPUT_CLASS_ENABLE = 35, /*!< Enable/disable the output of an entire class */ + + /* Ethernet configuration */ + SBG_ECOM_CMD_ETHERNET_CONF = 36, /*!< Set/get Ethernet configuration such as DHCP mode and IP address. */ + SBG_ECOM_CMD_ETHERNET_INFO = 37, /*!< Return the current IP used by the device. */ + + /* Validity thresholds */ + SBG_ECOM_CMD_VALIDITY_THRESHOLDS = 38, /*!< Set/get Validity flag thresholds for position, velocity, attitude and heading */ + + /* DVL configuration */ + SBG_ECOM_CMD_DVL_MODEL_ID = 39, /*!< Set/get DVL model id to use */ + SBG_ECOM_CMD_DVL_INSTALLATION = 40, /*!< DVL installation configuration (lever arm, alignments) */ + SBG_ECOM_CMD_DVL_REJECT_MODES = 41, /*!< DVL aiding rejection modes configuration. */ + + /* AirData configuration */ + SBG_ECOM_CMD_AIRDATA_MODEL_ID = 42, /*!< Set/get AirData model id and protocol to use. */ + SBG_ECOM_CMD_AIRDATA_LEVER_ARM = 43, /*!< AirData installation configuration (lever arm, offsets) */ + SBG_ECOM_CMD_AIRDATA_REJECT_MODES = 44, /*!< AirData aiding rejection modes configuration. */ + + /* Odometer configuration (using CAN) */ + SBG_ECOM_CMD_ODO_CAN_CONF = 45, /*!< Configuration for CAN based odometer (CAN ID & DBC) */ + + /* REST API related commands */ + SBG_ECOM_CMD_API_GET = 46, /*!< Command equivalent to the HTTP GET method for a REST API. */ + SBG_ECOM_CMD_API_POST = 47, /*!< Command equivalent to the HTTP POST method for a REST API. */ + + /* Misc. */ + SBG_ECOM_LOG_ECOM_NUM_CMDS /*!< Helper definition to know the number of commands */ +} SbgEComCmd; + +/*! + * This type defines any message identifier. + * Because message identifiers enum will be different with each class id, we use a generic uint8_t rather than an enum. + */ +typedef uint8_t SbgEComMsgId; + +//----------------------------------------------------------------------// +//- Inline helpers for log IDs -// +//----------------------------------------------------------------------// + +/*! + * Test if the message class is a binary log one. + * + * \param[in] msgClass Message class. + * \return TRUE if the message class corresponds to a binary log. + */ +SBG_INLINE bool sbgEComMsgClassIsALog(SbgEComClass msgClass) +{ + // + // Test if this class id is part of the enum + // + if ((msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) || (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) ) + { + return true; + } + else + { + return false; + } +} + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_IDS_H diff --git a/crates/sbg-rs/sbgECom/src/sbgEComLib.h b/crates/sbg-rs/sbgECom/src/sbgEComLib.h new file mode 100644 index 0000000..714f72c --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgEComLib.h @@ -0,0 +1,69 @@ +/*! + * \file sbgEComLib.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 + * + * \brief Main header file for the SBG Systems Enhanced Communication Library. + * + * Only this main header file should be included to use the library. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_LIB_H +#define SBG_ECOM_LIB_H + +#ifdef __cplusplus +extern "C" { +#endif + +// sbgCommonLib headers +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Local headers +#include "sbgECanId.h" +#include "sbgEComIds.h" +#include "commands/sbgEComCmd.h" +#include "protocol/sbgEComProtocol.h" +#include "binaryLogs/sbgEComBinaryLogs.h" +#include "sbgEComVersion.h" +#include "sbgEComGetVersion.h" + +//----------------------------------------------------------------------// +//- Footer (close extern C block) -// +//----------------------------------------------------------------------// +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_LIB_H diff --git a/crates/sbg-rs/sbgECom/src/sbgEComVersion.h b/crates/sbg-rs/sbgECom/src/sbgEComVersion.h new file mode 100644 index 0000000..2fe6083 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/sbgEComVersion.h @@ -0,0 +1,70 @@ +/*! + * \file sbgEComVersion.h + * \ingroup main + * \author SBG Systems + * \date 05 February 2013 + * + * \brief Header file that contains all versions related information such as change log. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_E_COM_VERSION_H +#define SBG_E_COM_VERSION_H + +// sbgCommonLib headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +//----------------------------------------------------------------------// +//- Version definitions -// +//----------------------------------------------------------------------// + +#define SBG_E_COM_VERSION_MAJOR 3 +#define SBG_E_COM_VERSION_MINOR 2 +#define SBG_E_COM_VERSION_REV 4011 +#define SBG_E_COM_VERSION_BUILD SBG_VERSION_QUALIFIER_STABLE + +#define SBG_E_COM_VERSION SBG_VERSION_SOFTWARE(SBG_E_COM_VERSION_MAJOR,SBG_E_COM_VERSION_MINOR,SBG_E_COM_VERSION_REV,SBG_E_COM_VERSION_BUILD) + +/* + * Backward compatibility macro definitions. + */ + #ifndef SBG_STR + #define SBG_STR(X) #X +#endif +#ifndef SBG_ASSTR + #define SBG_ASSTR(X) SBG_STR(X) +#endif +#define SBG_E_COM_VERSION_STR SBG_ASSTR(SBG_E_COM_VERSION_MAJOR) "." SBG_ASSTR(SBG_E_COM_VERSION_MINOR) "." SBG_ASSTR(SBG_E_COM_VERSION_REV) "-stable\0" + +#ifdef __cplusplus +} +#endif + +#endif // SBG_E_COM_VERSION_H diff --git a/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.c b/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.c new file mode 100644 index 0000000..4096908 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.c @@ -0,0 +1,620 @@ +// sbgCommonLib headers +#include +#include +#include + +// Project headers +#include + +// Local headers +#include "sbgEComTransfer.h" + +//----------------------------------------------------------------------// +//- Private methods -// +//----------------------------------------------------------------------// +/*! + * Initiates an upload transfer sequence with a device. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[in] size Total size of the upload. + * \return SBG_NO_ERROR when the transfer was initiated successfully. + */ +static SbgErrorCode sbgEComTransferSendInit(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, size_t size) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer streamBuffer; + uint8_t outputBuffer[6]; + uint32_t trial; + + assert(pHandle); + + // + // Initialize stream buffer that will contain payload + // + sbgStreamBufferInitForWrite(&streamBuffer, outputBuffer, sizeof(outputBuffer)); + + // + // Build transfer payload (a SBG_ECOM_TRANSFER_START command and the total size of the upload) + // + sbgStreamBufferWriteUint16LE(&streamBuffer, SBG_ECOM_TRANSFER_START); + sbgStreamBufferWriteSizeT32LE(&streamBuffer, size); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in ECom protocol + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + // + // If the device accepts the transfer, it returns an ack, wait for the answer. + // + errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); + + // + // Test if the response is positive from device + // + if (errorCode == SBG_NO_ERROR) + { + // + // Ack received, no need for other trial. + // + break; + } + } + } + + return errorCode; +} + +/*! + * Send one packet of data on a initiated upload transfer. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[in] pBuffer Pointer to the buffer containing the data to send. + * \param[in] offset The offset from the start of the transfer. + * \param[in] packetSize The size of this packet. + * \return SBG_NO_ERROR if the packet was sent and acknowledged by the device. + */ +static SbgErrorCode sbgEComTransferSendData(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t offset, size_t packetSize) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer streamBuffer; + uint8_t outputBuffer[SBG_ECOM_TRANSFER_PACKET_SIZE+6]; + uint32_t trial; + + assert(pHandle); + assert(pBuffer); + assert(packetSize > 0); + + // + // Initialize stream buffer for output + // + sbgStreamBufferInitForWrite(&streamBuffer, outputBuffer, sizeof(outputBuffer)); + + // + // Build payload: a SBG_ECOM_TRANSFER_DATA command, the offset from the start of the transfer, and the data + // + sbgStreamBufferWriteUint16LE(&streamBuffer, SBG_ECOM_TRANSFER_DATA); + sbgStreamBufferWriteSizeT32LE(&streamBuffer, offset); + sbgStreamBufferWriteBuffer(&streamBuffer, pBuffer, packetSize); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in a ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&streamBuffer), sbgStreamBufferGetLength(&streamBuffer)); + + if (errorCode == SBG_NO_ERROR) + { + // + // If the device receives the frame successfully received, it responds with an ACK, wait for the answer + // + errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); + + // + // Test if the response is positive from device + // + if (errorCode == SBG_NO_ERROR) + { + // + // Ack received, no need for other trial + // + break; + } + } + } + + return errorCode; +} + +/*! + * Ends ongoing upload transfer sequence with a device. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \return SBG_NO_ERROR when the transfer ended successfully. + */ +static SbgErrorCode sbgEComTransferSendEnd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + // + // Build payload, only a SBG_ECOM_TRANSFER_END cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send upload end payload encapsulated in a ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // If the device finishes the sequence successfully, it responds with an ACK, wait for answer + // + errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); + + // + // Test if the response is positive from device + // + if (errorCode == SBG_NO_ERROR) + { + // + // ACK received, no need for other trial + // + break; + } + } + } + + return errorCode; +} + +/*! + * Initiates a download sequences with a device. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[out] pSize Size of the transfer initiated, returned from the device. + * \return SBG_NO_ERROR when the transfer initiated successfully. + */ +static SbgErrorCode sbgEComTransferReceiveInit(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, size_t *pSize) +{ + SbgErrorCode errorCode = SBG_ERROR; + SbgEComProtocolPayload receivedPayload; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint16_t transferCmd; + size_t transferSize; + uint32_t trial; + + assert(pHandle); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload, only a SBG_ECOM_TRANSFER_START cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_START); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in an ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // Wait for reponse, the device should respond with a ECOM_TRANSFER_START command and the transfer size + // If it can not initiate the transfer, it will respond with a NACK + // + errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + + // + // Init stream buffer on received payload to process it + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Retrieve parameters, the first one is the transfer command + // The second one is the total transfer size + // + transferCmd = sbgStreamBufferReadUint16LE(&inputStream); + transferSize = sbgStreamBufferReadSizeT32LE(&inputStream); + + // + // The device should have answered with SBG_ECOM_TRANSFER_START transfer command + // + if (transferCmd == SBG_ECOM_TRANSFER_START) + { + // + // Update output variable with the transfer size + // + *pSize = transferSize; + + // + // No need for other trials, exit loop/ + // + break; + } + else + { + // + // Invalid transfer command response + // + errorCode = SBG_ERROR; + } + } + else if (errorCode != SBG_TIME_OUT) + { + SBG_LOG_ERROR(errorCode, "Invalid answer received"); + } + else + { + SBG_LOG_ERROR(errorCode, "No response received"); + } + } + else + { + SBG_LOG_ERROR(errorCode, "Unable to send the command"); + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +/*! + * Receive one packet of data on a initiated download transfer. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[in] pBuffer Pointer to the buffer where to write the packet. + * \param[in] offset The offset from the start of the buffer. + * \param[in] packetSize The size of the data asked to the device. + * \return SBG_NO_ERROR if the packet was successfully received. + */ +static SbgErrorCode sbgEComTransferReceiveData(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t offset, size_t packetSize) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgEComProtocolPayload receivedPayload; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[10]; + uint32_t trial; + + assert(pHandle); + assert(pBuffer); + assert(packetSize > 0); + + sbgEComProtocolPayloadConstruct(&receivedPayload); + + // + // Build payload: an SBG_ECOM_TRANSFER_DATA transfer command, the offset from the start of the transfer, the size of the packet the device must send + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_DATA); + sbgStreamBufferWriteSizeT32LE(&outputStream, offset); + sbgStreamBufferWriteSizeT32LE(&outputStream, packetSize); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send transfer payload encapsulated in an ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // Wait for reponse, the device should respond with a ECOM_TRANSFER_DATA, the offset from the start of the transfer and the data payload + // If it can not provide the data, it will respond with a NACK + // + errorCode = sbgEComReceiveCmd2(pHandle, msgClass, msg, &receivedPayload, pHandle->cmdDefaultTimeOut); + + if (errorCode == SBG_NO_ERROR) + { + SbgStreamBuffer inputStream; + uint16_t transferCmd; + size_t rcvdOffset; + + // + // Initialize stream buffer for read on input buffer + // + sbgStreamBufferInitForRead(&inputStream, sbgEComProtocolPayloadGetBuffer(&receivedPayload), sbgEComProtocolPayloadGetSize(&receivedPayload)); + + // + // Read response fields, first is the transfer command, second is the offset + // + transferCmd = sbgStreamBufferReadUint16LE(&inputStream); + rcvdOffset = sbgStreamBufferReadSizeT32LE(&inputStream); + + // + // Test that it's a SBG_ECOM_TRANSFER_DATA command + // The data is at the offset asked and the size corresponds + // + if ( (transferCmd == SBG_ECOM_TRANSFER_DATA) && (offset == rcvdOffset) && (packetSize == (sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t)))) ) + { + // + // Read all the buffer + // + sbgStreamBufferReadBuffer(&inputStream, pBuffer, sbgEComProtocolPayloadGetSize(&receivedPayload) - (sizeof(uint16_t) + sizeof(uint32_t))); + + // + // No need for other trials, exit loop + // + break; + } + } + } + } + + sbgEComProtocolPayloadDestroy(&receivedPayload); + + return errorCode; +} + +/*! + * Function that ends a download sequence with a device. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \return SBG_NO_ERROR when the transfer ended successfully. + */ +static SbgErrorCode sbgEComTransferReceiveEnd(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgStreamBuffer outputStream; + uint8_t outputBuffer[2]; + uint32_t trial; + + assert(pHandle); + + // + // Build payload, only a SBG_ECOM_TRANSFER_END cmd + // + sbgStreamBufferInitForWrite(&outputStream, outputBuffer, sizeof(outputBuffer)); + sbgStreamBufferWriteUint16LE(&outputStream, SBG_ECOM_TRANSFER_END); + + // + // Send command (multiple times in case of failures) + // + for (trial = 0; trial < pHandle->numTrials; trial++) + { + // + // Send upload end payload encapsulated in a ECom protocol frame + // + errorCode = sbgEComProtocolSend(&pHandle->protocolHandle, msgClass, msg, sbgStreamBufferGetLinkedBuffer(&outputStream), sbgStreamBufferGetLength(&outputStream)); + + if (errorCode == SBG_NO_ERROR) + { + // + // If the device is able to finish transfer sequence, it responds with an ACK + // + errorCode = sbgEComWaitForAck(pHandle, msgClass, msg, pHandle->cmdDefaultTimeOut); + + // + // Test if the response is positive from device + // + if (errorCode == SBG_NO_ERROR) + { + // + // No need for other trial, exit loop + // + break; + } + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t size) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgSplitBuffer splitBuffer; + size_t i; + + assert(pHandle); + assert(pBuffer); + assert(size > 0); + + // + // Make sure we are not trying to send a buffer that is too large + // + if (size <= SBG_ECOM_TRANSFER_MAX_SIZE) + { + // + // Initiate data transfer + // + errorCode = sbgEComTransferSendInit(pHandle, msgClass, msg, size); + + // + // Check that the transfer was correctly initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Initialize split buffer that will help with splitting up provided buffer + // + sbgSplitBufferInitForRead(&splitBuffer, pBuffer, size, SBG_ECOM_TRANSFER_PACKET_SIZE); + + // + // Transfer sub buffer one by one + // + for (i = 0; i < sbgSplitBufferGetSubBufferNbr(&splitBuffer); i++) + { + // + // Send a sub buffer + // + errorCode = sbgEComTransferSendData(pHandle, msgClass, msg, sbgSplitBufferGetSubBuffer(&splitBuffer, i), sbgSplitBufferGetSubBufferOffset(&splitBuffer, i), sbgSplitBufferGetSubBufferSize(&splitBuffer, i)); + + // + // Test if the sub buffer has been sent + // + if (errorCode != SBG_NO_ERROR) + { + // + // Unable to send a sub buffer, abort send operation. + // + break; + } + } + + // + // Test if any error occurred during data transfer + // + if (errorCode == SBG_NO_ERROR) + { + // + // End data transfer + // + errorCode = sbgEComTransferSendEnd(pHandle, msgClass, msg); + } + } + } + else + { + // + // Trying to send a buffer that is too large + // + errorCode = SBG_INVALID_PARAMETER; + } + + return errorCode; +} + +SbgErrorCode sbgEComTransferReceive(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t *pActualSize, size_t bufferSize) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + SbgSplitBuffer splitBuffer; + size_t transferSize; + size_t i; + + assert(pHandle); + assert(pBuffer); + assert(pActualSize); + assert(bufferSize > 0); + + // + // initiate data transfer + // + errorCode = sbgEComTransferReceiveInit(pHandle, msgClass, msg, &transferSize); + + // + // Make sure the receive transfer has been correctly initialized + // + if (errorCode == SBG_NO_ERROR) + { + // + // Test that the provided buffer is large enough to receive all data + // + if (transferSize <= bufferSize) + { + // + // Initialize Split buffer to help with sub buffer receive + // + sbgSplitBufferInitForWrite(&splitBuffer, pBuffer, transferSize, SBG_ECOM_TRANSFER_PACKET_SIZE); + + // + // Receive buffers one by one + // + for (i = 0; i < sbgSplitBufferGetSubBufferNbr(&splitBuffer); i++) + { + // + // Receive a sub buffer + // + errorCode = sbgEComTransferReceiveData(pHandle, msgClass, msg, sbgSplitBufferGetSubBuffer(&splitBuffer, i), sbgSplitBufferGetSubBufferOffset(&splitBuffer, i), sbgSplitBufferGetSubBufferSize(&splitBuffer, i)); + + // + // Make sure that the sub buffer has been correctly received + // + if (errorCode != SBG_NO_ERROR) + { + // + // An error occurred, abort data transfer + // + break; + } + } + + // + // Test if any error occurred during transfer + // + if (errorCode == SBG_NO_ERROR) + { + // + // End data transfer + // + errorCode = sbgEComTransferReceiveEnd(pHandle, msgClass, msg); + + // + // Make sure that the transfer has been correctly ended + // + if (errorCode == SBG_NO_ERROR) + { + // + // Since the transfer was successful update output variable pActualSize + // + *pActualSize = transferSize; + } + } + } + else + { + // + // Provided buffer is too small + // + errorCode = SBG_INVALID_PARAMETER; + } + } + + return errorCode; +} diff --git a/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.h b/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.h new file mode 100644 index 0000000..1a22700 --- /dev/null +++ b/crates/sbg-rs/sbgECom/src/transfer/sbgEComTransfer.h @@ -0,0 +1,100 @@ +/*! + * \file sbgEComTransfer.h + * \ingroup protocol + * \author SBG Systems + * \date 19 November 2013 + * + * \brief Handle large send/receive transfer for specific ECom Protocol commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense The MIT license + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * \endlicense + */ + +#ifndef SBG_ECOM_TRANSFER_H +#define SBG_ECOM_TRANSFER_H + +// sbgCommonLib headers +#include + +// Project headers +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//----------------------------------------------------------------------// +//- Global definitions -// +//----------------------------------------------------------------------// + +#define SBG_ECOM_TRANSFER_MAX_SIZE (8192u) /*!< Maximum buffer that can be transmitted using the sbgECom transfer protocol. */ +#define SBG_ECOM_TRANSFER_PACKET_SIZE (512u) /*!< Max packet size transmitted in a single frame */ + +//----------------------------------------------------------------------// +//- Communication protocol structs and definitions -// +//----------------------------------------------------------------------// + +/*! + * Defines the ECom transfer commands + */ +typedef enum _SbgEComTransferCmd +{ + SBG_ECOM_TRANSFER_START = 0, /*!< Command to initiate a transfer. */ + SBG_ECOM_TRANSFER_DATA = 1, /*!< Command to transmit/receive data. */ + SBG_ECOM_TRANSFER_END = 2 /*!< Command to end a transfer. */ +} SbgEComTransferCmd; + +//----------------------------------------------------------------------// +//- Public methods -// +//----------------------------------------------------------------------// + +/*! + * Specific method to handle a large send into multiple frames. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[in] pBuffer Pointer to the buffer containing the data to send. + * \param[in] size The size of the buffer. + * \return SBG_NO_ERROR in case of a successful upload. + */ +SbgErrorCode sbgEComTransferSend(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, const void *pBuffer, size_t size); + +/*! + * Specific method to handle a large receive from the device. + * + * \param[in] pHandle Pointer to a valid SbgEComHandle. + * \param[in] msgClass Original protocol class asking for transfer. + * \param[in] msg Original protocol message id asking for transfer. + * \param[in] pBuffer Pointer to the buffer where to write data. + * \param[out] pActualSize The final size written into the buffer. + * \param[in] bufferSize The size of the buffer. + * \return SBG_NO_ERROR in case of a successful download. + */ +SbgErrorCode sbgEComTransferReceive(SbgEComHandle *pHandle, uint8_t msgClass, uint8_t msg, void *pBuffer, size_t *pActualSize, size_t bufferSize); + +#ifdef __cplusplus +} +#endif + +#endif // SBG_ECOM_TRANSFER_H diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/README.md b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/README.md new file mode 100644 index 0000000..0ee1616 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/README.md @@ -0,0 +1,70 @@ +# sbgBasicLogger + +The sbgBasicLogger let you log and display sbgECom binary messages into CSV like text files. + +## Output +The output is as close as possible as the sbgECom message definition. +You can both output incoming messages in text files and/or display the data in the terminal. + +## Decimation +The tool can also decimate incoming IMU data using a simple moving average filter. +You can specify decimation to apply before displayed the data to the terminal and also one before writing the data to text files. + +Only the following logs are decimated: + - IMU short + - IMU fast + - IMU data + +## Interfaces +The sbgBasicLogger can be used to parse incoming data from a serial or an Ethernet UDP interface. +You can also select an binary file containing raw sbgECom dump. This tool can thus be used to easily export sbgECom data to CVS like text files. + +# Usage + +The sbgBasicLogger implements a simple to use command line interface (CLI): + +```sh +sbgBasicLogger [-hvwpH] [-a IP address] [-I UDP port in] [-O UDP port out] [-s SERIAL_DEVICE] [-r SERIAL_BAUDRATE] [-i INPUT-FILE] [--dir=DIRECTORY] [-d FILE DECIMATION] [-c CONSOLE DECIMATION] +``` + +## Serial example and only print on console + +```sh +sbgBasicLogger -s -r -p +``` + +## Serial example, print on console and write files into + +```sh +sbgBasicLogger -s -r -p -w --dir=directory/ +``` + +## UDP example and only print on console + +```sh +sbgBasicLogger -a -I -O -p +``` + +## Input file example and only print on console + +```sh +sbgBasicLogger -i -p +``` + +## Options +``` + -h, --help display this help and exit + -v, --version display sbgECom version and exit + -a, --addr-ip=IP address open an UDP interface + -I, --udp-port-in=UDP port in UDP port in + -O, --udp-port-out=UDP port out UDP port out + -s, --serial-device=SERIAL_DEVICE open a serial interface + -r, --serial-baudrate=SERIAL_BAUDRATE serial baudrate + -i, --input-file=INPUT-FILE input file + -w, --write-logs write logs in different files + --dir=DIRECTORY directory to write logs into + -d, --file-decimation=FILE DECIMATION file decimation + -c, --console-decimation=CONSOLE DECIMATION output stream decimation + -p, --print-logs print the logs on the output stream + -H, --disable-header disable header for files +``` \ No newline at end of file diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/main.c b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/main.c new file mode 100644 index 0000000..a59a1a9 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/main.c @@ -0,0 +1,1046 @@ +/*! + * \file main.c + * \author SBG Systems + * \date July 29, 2021 + * + * \brief Tool to manage SBG logs in text format. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * \endlicense + */ + +// sbgCommonLib headers +#include +#include +#include +#include +#include +#include + +// sbgECom headers +#include +#include +#include + +// Argtable3 headers +#include + +// Local headers +#include "sbgBasicLoggerHandler.h" + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Program name. + */ +#define PROGRAM_NAME "sbgBasicLogger" + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * User arguments. + */ +typedef struct _UserArgs +{ + bool writeHeader; /*!< True to write data header. */ + bool writingFiles; /*!< True to write data into files. */ + size_t fileDecimation; /*!< File decimation. */ + bool consoleEnabled; /*!< True to enable console. */ + size_t consoleDecimation; /*!< Console decimation. */ + SbgString path; /*!< Output path string to write log files into. */ + const char *pPathStr; /*!< Output path string shortcut. */ +} UserArgs; + +/*! + * SbgBasicLoggerManager. + */ +typedef struct _SbgBasicLoggerManager +{ + UserArgs userArgs; /*!< User arguments. */ + + SbgEComHandle ecomHandle; /*!< ECom handler. */ + SbgInterface ecomInterface; /*!< SBG interface. */ + + SbgLogUtcData lastUtcData; /*!< Latest UTC time reference used to time stamp messages. */ + + SbgBasicLoggerImuHandler imuShort; /*!< IMU short logs handler. */ + SbgBasicLoggerImuHandler imu; /*!< IMU logs handler. */ + SbgBasicLoggerImuHandler imuFast; /*!< IMU fast logs handler. */ + SbgBasicLoggerStatusHandler status; /*!< Status logs handler. */ + SbgBasicLoggerDiagHandler diag; /*!< Diagnostic logs handler. */ + SbgBasicLoggerUtcHandler utc; /*!< Diagnostic logs handler. */ + + SbgBasicLoggerEkfEulerHandler ekfEuler; /*!< EKF Euler logs handler. */ + SbgBasicLoggerEkfQuatHandler ekfQuat; /*!< EKF quat logs handler. */ + SbgBasicLoggerEkfNavHandler ekfNav; /*!< EKF nav logs handler. */ + + SbgBasicLoggerShipMotionHandler shipMotion; /*!< Ship motion logs handler. */ + SbgBasicLoggerShipMotionHpHandler shipMotionHp; /*!< Ship motion HP logs handler. */ + + SbgBasicLoggerGpsHdtHandler gps1Hdt; /*!< GPS 1 hdt logs handler. */ + SbgBasicLoggerGpsPosHandler gps1Pos; /*!< GPS 1 pos logs handler. */ + SbgBasicLoggerGpsVelHandler gps1Vel; /*!< GPS 1 vel logs handler. */ + SbgBasicLoggerGpsRawHandler gps1Raw; /*!< GPS 1 raw logs handler. */ + SbgBasicLoggerGpsSatHandler gps1Sat; /*!< GPS 1 Satellite in View logs handler. */ + + SbgBasicLoggerGpsHdtHandler gps2Hdt; /*!< GPS 2 hdt logs handler. */ + SbgBasicLoggerGpsPosHandler gps2Pos; /*!< GPS 2 pos logs handler. */ + SbgBasicLoggerGpsVelHandler gps2Vel; /*!< GPS 2 vel logs handler. */ + SbgBasicLoggerGpsRawHandler gps2Raw; /*!< GPS 2 raw logs handler. */ + SbgBasicLoggerGpsSatHandler gps2Sat; /*!< GPS 2 Satellite in View logs handler. */ + + SbgBasicLoggerOdometerHandler odometer; /*!< Odometer logs handler. */ + SbgBasicLoggerDvlHandler dvlBottom; /*!< DVL bottom logs handler. */ + SbgBasicLoggerDvlHandler dvlWater; /*!< DVL water logs handler. */ + SbgBasicLoggerAirHandler air; /*!< Air logs handler. */ + SbgBasicLoggerUsblHandler usbl; /*!< USBL logs handler. */ + SbgBasicLoggerDepthHandler depth; /*!< Depth logs handler. */ + SbgBasicLoggerRawRtcmHandler rawRtcm; /*!< Raw RTCM logs handler. */ + + SbgBasicLoggerEventHandler eventInA; /*!< Event in A logs handler. */ + SbgBasicLoggerEventHandler eventInB; /*!< Event in B logs handler. */ + SbgBasicLoggerEventHandler eventInC; /*!< Event in C logs handler. */ + SbgBasicLoggerEventHandler eventInD; /*!< Event in D logs handler. */ + SbgBasicLoggerEventHandler eventInE; /*!< Event in E logs handler. */ + + SbgBasicLoggerEventHandler eventOutA; /*!< Event out A logs handler. */ + SbgBasicLoggerEventHandler eventOutB; /*!< Event out B logs handler. */ + + SbgBasicLoggerMagHandler mag; /*!< Mag logs handler. */ +} SbgBasicLoggerManager; + +//----------------------------------------------------------------------// +//- Private variables -// +//----------------------------------------------------------------------// + +/*! + * SBG basic logger singleton. + */ +static SbgBasicLoggerManager gSbgBasicLogger; + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Callback definition used to route log error messages. + * + * \param[in] pFileName The file in which the log was triggered. + * \param[in] pFunctionName The function where the log was triggered. + * \param[in] line The line in the file where the log was triggered. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] type Associated log message type. + * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. + * \param[in] pMessage The message to log. + */ +static void onLogCallback(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType logType, SbgErrorCode errorCode, const char *pMessage) +{ + assert(pFunctionName); + assert(pCategory); + assert(pMessage); + + SBG_UNUSED_PARAMETER(pFileName); + SBG_UNUSED_PARAMETER(pCategory); + + switch (logType) + { + case SBG_DEBUG_LOG_TYPE_ERROR: + fprintf(stderr, "*ERR * %10s %s(%"PRIu32"): %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, line, pMessage); + break; + case SBG_DEBUG_LOG_TYPE_WARNING: + fprintf(stderr, "*WARN* %10s %s(%"PRIu32"): %s\n\r", sbgErrorCodeToString(errorCode), pFunctionName, line, pMessage); + break; + case SBG_DEBUG_LOG_TYPE_INFO: + fprintf(stderr, "*INFO* %s(%"PRIu32"): %s\n\r", pFunctionName, line, pMessage); + break; + case SBG_DEBUG_LOG_TYPE_DEBUG: + fprintf(stderr, "*DBG * %s(%"PRIu32") : % s\n\r", pFunctionName, line, pMessage); + break; + default: + fprintf(stderr, "error: unexpected logType (0x%x)\n\r", logType); + break; + } +} + +//----------------------------------------------------------------------// +//- SbgBasicLoggerManager methods -// +//----------------------------------------------------------------------// + +/*! + * Process an UTC data. + * + * \param[in] pManager SBG basic logger. + * \param[in] pUtc UTC data. + */ +static void sbgBasicLoggerManagerProcessUtc(SbgBasicLoggerManager *pManager, const SbgLogUtcData *pUtcData) +{ + assert(pManager); + assert(pUtcData); + + pManager->lastUtcData = *pUtcData; + sbgBasicLoggerUtcHandlerProcess(&pManager->utc, pUtcData); +} + +/*! + * Process a new IMU short data. + * + * \param[in] pManager SBG basic logger. + * \param[in] pImuShort New IMU short data. + */ +static void sbgBasicLoggerManagerProcessImuShort(SbgBasicLoggerManager *pManager, const SbgLogImuShort *pImuShort) +{ + SbgBasicLoggerImu newData; + + assert(pManager); + assert(pImuShort); + + newData.timestamp = pImuShort->timeStamp; + newData.status = pImuShort->status; + newData.temperature = sbgLogImuShortGetTemperature(pImuShort); + + for (size_t i = 0; i < 3; i++) + { + newData.deltaAngle[i] = sbgLogImuShortGetDeltaAngle(pImuShort, i); + newData.deltaVelocity[i] = sbgLogImuShortGetDeltaVelocity(pImuShort, i); + } + + sbgBasicLoggerImuHandlerProcess(&pManager->imuShort, &newData); +} + +/*! + * Process a new IMU data. + * + * \param[in] pManager SBG basic logger. + * \param[in] pImu New IMU data. + */ +static void sbgBasicLoggerManagerProcessImu(SbgBasicLoggerManager *pManager, const SbgLogImuData *pImu) +{ + SbgBasicLoggerImu newData; + + assert(pManager); + assert(pImu); + + newData.timestamp = pImu->timeStamp; + newData.status = pImu->status; + newData.temperature = pImu->temperature; + + for (size_t i = 0; i < 3; i++) + { + newData.deltaAngle[i] = pImu->deltaAngle[i]; + newData.deltaVelocity[i] = pImu->deltaVelocity[i]; + } + + sbgBasicLoggerImuHandlerProcess(&pManager->imu, &newData); +} + +/*! + * Process a new IMU fast data. + * + * \param[in] pManager SBG basic logger. + * \param[in] pImuShort New IMU fast data. + */ +static void sbgBasicLoggerManagerProcessImuFast(SbgBasicLoggerManager *pManager, const SbgLogFastImuData *pFast) +{ + SbgBasicLoggerImu newData; + + assert(pManager); + assert(pFast); + + newData.timestamp = pFast->timeStamp; + newData.status = pFast->status; + newData.temperature = 0.0f; + + for (size_t i = 0; i < 3; i++) + { + newData.deltaAngle[i] = pFast->gyroscopes[i]; + newData.deltaVelocity[i] = pFast->accelerometers[i]; + } + + sbgBasicLoggerImuHandlerProcess(&pManager->imuFast, &newData); +} + +/*! + * Callback used to handle received logs. + * + * \param[in] pECom SbgECom instance. + * \param[in] msgClass Class of the received message. + * \param[in] msg Received message ID. + * \param[in] pLogData Received data. + * \param[in] pUserArg Optional user argument. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode onLogReceived(SbgEComHandle *pECom, SbgEComClass msgClass, SbgEComMsgId msg, const SbgBinaryLogData *pLogData, void *pUserArg) +{ + SbgBasicLoggerManager *pManager; + + assert(pLogData); + assert(pUserArg); + + SBG_UNUSED_PARAMETER(pECom); + + pManager = (SbgBasicLoggerManager*)pUserArg; + + if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_0) + { + switch (msg) + { + case SBG_ECOM_LOG_STATUS: + sbgBasicLoggerStatusHandlerProcess(&pManager->status, &pLogData->statusData); + break; + + case SBG_ECOM_LOG_UTC_TIME: + sbgBasicLoggerManagerProcessUtc(pManager, &pLogData->utcData); + break; + + case SBG_ECOM_LOG_IMU_DATA: + sbgBasicLoggerManagerProcessImu(pManager, &pLogData->imuData); + break; + + case SBG_ECOM_LOG_MAG: + sbgBasicLoggerMagHandlerProcess(&pManager->mag, &pLogData->magData); + break; + + case SBG_ECOM_LOG_MAG_CALIB: + // + // Known but not sbgBasicLoggerManagerProcessed. + // + break; + + case SBG_ECOM_LOG_EKF_EULER: + sbgBasicLoggerEkfEulerHandlerProcess(&pManager->ekfEuler, &pLogData->ekfEulerData); + break; + + case SBG_ECOM_LOG_EKF_QUAT: + sbgBasicLoggerEkfQuatHandlerProcess(&pManager->ekfQuat, &pLogData->ekfQuatData); + break; + + case SBG_ECOM_LOG_EKF_NAV: + sbgBasicLoggerEkfNavHandlerProcess(&pManager->ekfNav, &pLogData->ekfNavData); + break; + + case SBG_ECOM_LOG_SHIP_MOTION: + sbgBasicLoggerShipMotionHandlerProcess(&pManager->shipMotion, &pLogData->shipMotionData); + break; + + case SBG_ECOM_LOG_GPS1_VEL: + sbgBasicLoggerGpsVelHandlerProcess(&pManager->gps1Vel, &pLogData->gpsVelData); + break; + + case SBG_ECOM_LOG_GPS1_POS: + sbgBasicLoggerGpsPosHandlerProcess(&pManager->gps1Pos, &pLogData->gpsPosData); + break; + + case SBG_ECOM_LOG_GPS1_HDT: + sbgBasicLoggerGpsHdtHandlerProcess(&pManager->gps1Hdt, &pLogData->gpsHdtData); + break; + + case SBG_ECOM_LOG_GPS1_RAW: + sbgBasicLoggerGpsRawHandlerProcess(&pManager->gps1Raw, &pLogData->gpsRawData); + break; + + case SBG_ECOM_LOG_GPS1_SAT: + sbgBasicLoggerGpsSatHandlerProcess(&pManager->gps1Sat, &pLogData->satGroupData); + break; + + case SBG_ECOM_LOG_GPS2_VEL: + sbgBasicLoggerGpsVelHandlerProcess(&pManager->gps2Vel, &pLogData->gpsVelData); + break; + + case SBG_ECOM_LOG_GPS2_POS: + sbgBasicLoggerGpsPosHandlerProcess(&pManager->gps2Pos, &pLogData->gpsPosData); + break; + + case SBG_ECOM_LOG_GPS2_HDT: + sbgBasicLoggerGpsHdtHandlerProcess(&pManager->gps2Hdt, &pLogData->gpsHdtData); + break; + + case SBG_ECOM_LOG_GPS2_RAW: + sbgBasicLoggerGpsRawHandlerProcess(&pManager->gps2Raw, &pLogData->gpsRawData); + break; + + case SBG_ECOM_LOG_GPS2_SAT: + sbgBasicLoggerGpsSatHandlerProcess(&pManager->gps2Sat, &pLogData->satGroupData); + break; + + case SBG_ECOM_LOG_ODO_VEL: + sbgBasicLoggerOdometerHandlerProcess(&pManager->odometer, &pLogData->odometerData); + break; + + case SBG_ECOM_LOG_EVENT_A: + sbgBasicLoggerEventHandlerProcess(&pManager->eventInA, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_EVENT_B: + sbgBasicLoggerEventHandlerProcess(&pManager->eventInB, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_EVENT_C: + sbgBasicLoggerEventHandlerProcess(&pManager->eventInC, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_EVENT_D: + sbgBasicLoggerEventHandlerProcess(&pManager->eventInD, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_EVENT_E: + sbgBasicLoggerEventHandlerProcess(&pManager->eventInE, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_DVL_BOTTOM_TRACK: + sbgBasicLoggerDvlHandlerProcess(&pManager->dvlBottom, &pLogData->dvlData); + break; + + case SBG_ECOM_LOG_DVL_WATER_TRACK: + sbgBasicLoggerDvlHandlerProcess(&pManager->dvlWater, &pLogData->dvlData); + break; + + case SBG_ECOM_LOG_SHIP_MOTION_HP: + sbgBasicLoggerShipMotionHpHandlerProcess(&pManager->shipMotionHp, &pLogData->shipMotionData); + break; + + case SBG_ECOM_LOG_AIR_DATA: + sbgBasicLoggerAirHandlerProcess(&pManager->air, &pLogData->airData); + break; + + case SBG_ECOM_LOG_USBL: + sbgBasicLoggerUsblHandlerProcess(&pManager->usbl, &pLogData->usblData); + break; + + case SBG_ECOM_LOG_IMU_SHORT: + sbgBasicLoggerManagerProcessImuShort(pUserArg, &pLogData->imuShort); + break; + + case SBG_ECOM_LOG_EVENT_OUT_A: + sbgBasicLoggerEventHandlerProcess(&pManager->eventOutA, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_EVENT_OUT_B: + sbgBasicLoggerEventHandlerProcess(&pManager->eventOutB, &pLogData->eventMarker); + break; + + case SBG_ECOM_LOG_DEPTH: + sbgBasicLoggerDepthHandlerProcess(&pManager->depth, &pLogData->depthData); + break; + + case SBG_ECOM_LOG_DIAG: + sbgBasicLoggerDiagHandlerProcess(&pManager->diag, &pManager->lastUtcData, &pLogData->diagData); + break; + + case SBG_ECOM_LOG_RTCM_RAW: + sbgBasicLoggerRawRtcmHandlerProcess(&pManager->rawRtcm, &pLogData->rtcmRawData); + break; + + default: + fprintf(stdout, "unkwown log: %u", msg); + break; + } + } + else if (msgClass == SBG_ECOM_CLASS_LOG_ECOM_1) + { + switch (msg) + { + case SBG_ECOM_LOG_FAST_IMU_DATA: + sbgBasicLoggerManagerProcessImuFast(pUserArg, &pLogData->fastImuData); + break; + + default: + fprintf(stdout, "unkwown log1: %u", msg); + break; + } + } + else if (msgClass == SBG_ECOM_CLASS_LOG_NMEA_0) + { + // + // Known but not sbgBasicLoggerManagerProcessed. + // + } + else if (msgClass == SBG_ECOM_CLASS_LOG_NMEA_1) + { + // + // Known but not sbgBasicLoggerManagerProcessed. + // + } + else if (msgClass == SBG_ECOM_CLASS_LOG_THIRD_PARTY_0) + { + // + // Known but not sbgBasicLoggerManagerProcessed. + // + } + else if (msgClass == SBG_ECOM_CLASS_LOG_CMD_0) + { + // + // Known but not sbgBasicLoggerManagerProcessed. + // + } + else + { + fprintf(stdout, "unkwown class: %u", msgClass); + } + + return SBG_NO_ERROR; +} + +/*! + * Construct all log handlers. + * + * \param[in] pManager SBG basic logger. + */ +static void sbgBasicLoggerConstructLogHandlers(SbgBasicLoggerManager *pManager) +{ + assert(pManager); + + sbgBasicLoggerImuShortHandlerConstruct(&pManager->imuShort, + pManager->userArgs.consoleEnabled, + pManager->userArgs.consoleDecimation, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.fileDecimation, + pManager->userArgs.writeHeader); + + sbgBasicLoggerImuHandlerConstruct(&pManager->imu, + pManager->userArgs.consoleEnabled, + pManager->userArgs.consoleDecimation, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.fileDecimation, + pManager->userArgs.writeHeader); + + sbgBasicLoggerImuFastHandlerConstruct(&pManager->imuFast, + pManager->userArgs.consoleEnabled, + pManager->userArgs.consoleDecimation, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.fileDecimation, + pManager->userArgs.writeHeader); + + + sbgBasicLoggerDiagHandlerConstruct(&pManager->diag, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerUtcHandlerConstruct(&pManager->utc, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerStatusHandlerConstruct(&pManager->status, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEkfEulerHandlerConstruct(&pManager->ekfEuler, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEkfQuatHandlerConstruct(&pManager->ekfQuat, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEkfNavHandlerConstruct(&pManager->ekfNav, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerShipMotionHandlerConstruct(&pManager->shipMotion, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerShipMotionHpHandlerConstruct(&pManager->shipMotionHp, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGpsHdt1HandlerConstruct(&pManager->gps1Hdt, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGpsPos1HandlerConstruct(&pManager->gps1Pos, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGpsVel1HandlerConstruct(&pManager->gps1Vel, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGps1RawHandlerConstruct(&pManager->gps1Raw, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerGps1SatHandlerConstruct(&pManager->gps1Sat, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerGpsHdt2HandlerConstruct(&pManager->gps2Hdt, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGpsPos2HandlerConstruct(&pManager->gps2Pos, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGpsVel2HandlerConstruct(&pManager->gps2Vel, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerGps2RawHandlerConstruct(&pManager->gps2Raw, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerGps2SatHandlerConstruct(&pManager->gps2Sat, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerOdometerHandlerConstruct(&pManager->odometer, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerDvlBottomHandlerConstruct(&pManager->dvlBottom, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerDvlWaterHandlerConstruct(&pManager->dvlWater, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerAirHandlerConstruct(&pManager->air, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerUsblHandlerConstruct(&pManager->usbl, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerDepthHandlerConstruct(&pManager->depth, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerRawRtcmHandlerConstruct(&pManager->rawRtcm, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr); + + sbgBasicLoggerEventInAHandlerConstruct(&pManager->eventInA, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventInBHandlerConstruct(&pManager->eventInB, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventInCHandlerConstruct(&pManager->eventInC, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventInDHandlerConstruct(&pManager->eventInD, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventInEHandlerConstruct(&pManager->eventInE, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventOutAHandlerConstruct(&pManager->eventOutA, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerEventOutBHandlerConstruct(&pManager->eventOutB, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); + + sbgBasicLoggerMagHandlerConstruct(&pManager->mag, + pManager->userArgs.consoleEnabled, + pManager->userArgs.writingFiles, + pManager->userArgs.pPathStr, + pManager->userArgs.writeHeader); +} + +/*! + * Receive logs. + * + * \param[in] pManager Sbg Basic logger. + */ +static SbgErrorCode sbgBasicLoggerManagerReceiveLogs(SbgBasicLoggerManager *pManager) +{ + SbgErrorCode errorCode; + + assert(pManager); + + errorCode = sbgEComInit(&pManager->ecomHandle, &pManager->ecomInterface); + + if (errorCode == SBG_NO_ERROR) + { + sbgBasicLoggerConstructLogHandlers(pManager); + + sbgEComSetReceiveLogCallback(&pManager->ecomHandle, onLogReceived, pManager); + + while (1) + { + errorCode = sbgEComHandle(&pManager->ecomHandle); + + if (errorCode == SBG_NOT_READY) + { + if (pManager->ecomInterface.type == SBG_IF_TYPE_FILE) + { + break; + } + + // + // Save CPU time. + // + sbgSleep(1); + } + } + + sbgEComClose(&pManager->ecomHandle); + } + + return errorCode; +} + +//----------------------------------------------------------------------// +// Public functions // +//----------------------------------------------------------------------// + +/*! + * Program entry point. + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char **argv) +{ + SbgBasicLoggerManager *pManager; + int exitCode = EXIT_SUCCESS; + bool printHelp = false; + + struct arg_lit *pHelpArg; + struct arg_lit *pVersionArg; + struct arg_str *pUdpAddrArg; + struct arg_int *pUdpPortInArg; + struct arg_int *pUdpPortOutArg; + struct arg_str *pSerialDeviceArg; + struct arg_int *pSerialBaudrateArg; + struct arg_file *pInputFileArg; + struct arg_lit *pWriteLogs; + struct arg_str *pWriteLogsDirArg; + struct arg_int *pFileDecimationArg; + struct arg_int *pScreenDecimationArg; + struct arg_lit *pPrintLogs; + struct arg_lit *pLogsHeader; + struct arg_end *pEndArg; + + void *argTable[] = + { + pHelpArg = arg_lit0( "h", "help", "display this help and exit"), + pVersionArg = arg_lit0( "v", "version", "display sbgECom version and exit"), + + pUdpAddrArg = arg_str0( "a", "addr-ip", "IP address", "open an UDP interface"), + pUdpPortInArg = arg_int0( "I", "udp-port-in", "UDP port in", "UDP port in"), + pUdpPortOutArg = arg_int0( "O", "udp-port-out", "UDP port out", "UDP port out"), + + pSerialDeviceArg = arg_str0( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), + pSerialBaudrateArg = arg_int0( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), + + pInputFileArg = arg_file0( "i", "input-file", "INPUT-FILE", "input file"), + + pWriteLogs = arg_lit0( "w", "write-logs", "write logs in different files"), + pWriteLogsDirArg = arg_str0( NULL, "dir", "DIRECTORY", "directory to write logs into"), + + pFileDecimationArg = arg_int0( "d", "file-decimation", "FILE DECIMATION", "file decimation"), + pScreenDecimationArg = arg_int0( "c", "console-decimation", "CONSOLE DECIMATION", "output stream decimation"), + + pPrintLogs = arg_lit0( "p", "print-logs", "print the logs on the output stream"), + + pLogsHeader = arg_lit0( "H", "disable-header", "disable header for files"), + + pEndArg = arg_end(20), + }; + + memset(&gSbgBasicLogger, 0, sizeof(gSbgBasicLogger)); + + pManager = &gSbgBasicLogger; + + sbgCommonLibSetLogCallback(onLogCallback); + + if (arg_nullcheck(argTable) == 0) + { + int argError; + + argError = arg_parse(argc, argv, argTable); + + if (pHelpArg->count != 0) + { + printf("Usage: %s", PROGRAM_NAME); + arg_print_syntax(stdout, argTable, "\n\n"); + printf("Manage sbgECom logs in text format.\n\n"); + + printf("Serial example: %s -s -r -p\n", PROGRAM_NAME); + printf(" UDP example: %s -a -I -O -p\n", PROGRAM_NAME); + printf(" File example: %s -i -p\n\n", PROGRAM_NAME); + + printf("Logs affected by decimation:\n\t- IMU short\n\t- IMU fast\n\t- IMU data\n\n"); + + arg_print_glossary(stdout, argTable, " %-50s %s\n"); + } + else if (pVersionArg->count != 0) + { + printf("%s\n", sbgEComGetVersionAsString()); + } + else if (argError == 0) + { + if (pLogsHeader->count != 0) + { + pManager->userArgs.writeHeader = false; + } + else + { + pManager->userArgs.writeHeader = true; + } + + if (pWriteLogs->count != 0) + { + pManager->userArgs.writingFiles = true; + pManager->userArgs.fileDecimation = 0; + + if (pWriteLogsDirArg->count != 0) + { + SbgErrorCode errorCode; + + errorCode = sbgStringConstructCString(&pManager->userArgs.path, pWriteLogsDirArg->sval[0]); + + if (errorCode == SBG_NO_ERROR) + { + if ((errorCode == SBG_NO_ERROR) && !sbgStringEndsWith(&pManager->userArgs.path, "/")) + { + sbgStringAppendCString(&pManager->userArgs.path, "/"); + } + } + else + { + fprintf(stderr, "Invalid path format.\n"); + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + else + { + sbgStringConstructCString(&pManager->userArgs.path, ""); + } + + pManager->userArgs.pPathStr = sbgStringGetCString(&pManager->userArgs.path); + } + + if (pFileDecimationArg->count != 0) + { + pManager->userArgs.fileDecimation = pFileDecimationArg->ival[0]; + } + else + { + pManager->userArgs.fileDecimation = 1; + } + + if (pPrintLogs->count != 0) + { + pManager->userArgs.consoleEnabled = true; + pManager->userArgs.consoleDecimation = 0; + } + + if (pScreenDecimationArg->count != 0) + { + pManager->userArgs.consoleDecimation = pScreenDecimationArg->ival[0]; + } + else + { + pManager->userArgs.consoleDecimation = 1; + } + + if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0) && + ((pInputFileArg->count != 0) || + (pUdpAddrArg->count != 0) || + (pUdpPortInArg->count != 0) || + (pUdpPortOutArg->count != 0))) + { + fprintf(stderr, "Too many arguments.\n"); + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if ((pInputFileArg->count != 0) && + ((pSerialDeviceArg->count != 0) || + (pSerialBaudrateArg->count != 0) || + (pUdpAddrArg->count != 0) || + (pUdpPortInArg->count != 0) || + (pUdpPortOutArg->count != 0))) + { + fprintf(stderr, "Too many arguments.\n"); + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if ((pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0) && + ((pInputFileArg->count != 0) || + (pSerialDeviceArg->count != 0) || + (pSerialBaudrateArg->count != 0))) + { + fprintf(stderr, "Too many arguments.\n"); + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if (!pManager->userArgs.consoleEnabled && !pManager->userArgs.writingFiles) + { + fprintf(stderr, "Specify if you want to print (-p) logs on console or write (-w) into files.\n"); + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if (exitCode == EXIT_SUCCESS) + { + if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0)) + { + SbgErrorCode errorCode; + + errorCode = sbgInterfaceSerialCreate(&pManager->ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + exitCode = EXIT_FAILURE; + } + } + else if (pInputFileArg->count != 0) + { + SbgErrorCode errorCode; + + errorCode = sbgInterfaceFileOpen(&pManager->ecomInterface, pInputFileArg->filename[0]); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to open file interface"); + exitCode = EXIT_FAILURE; + } + } + else if ((pUdpAddrArg->count != 0) && (pUdpPortInArg->count != 0) && (pUdpPortOutArg->count != 0)) + { + SbgErrorCode errorCode; + + errorCode = sbgInterfaceUdpCreate(&pManager->ecomInterface, sbgNetworkIpFromString(pUdpAddrArg->sval[0]), pUdpPortOutArg->ival[0], pUdpPortInArg->ival[0]); + + if (errorCode != SBG_NO_ERROR) + { + SBG_LOG_ERROR(errorCode, "unable to open file interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (exitCode == EXIT_SUCCESS) + { + SbgErrorCode errorCode; + + errorCode = sbgBasicLoggerManagerReceiveLogs(pManager); + + if (errorCode != SBG_NO_ERROR) + { + exitCode = EXIT_FAILURE; + } + + sbgInterfaceDestroy(&pManager->ecomInterface); + } + } + else + { + printHelp = true; + } + + if (printHelp) + { + arg_print_errors(stderr, pEndArg, PROGRAM_NAME); + fprintf(stderr, "Try '%s --help' for more information.\n", PROGRAM_NAME); + exitCode = EXIT_FAILURE; + } + + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate memory"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c new file mode 100644 index 0000000..b127303 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.c @@ -0,0 +1,58 @@ +// sbgCommonLib +#include + +// sbgECom headers +#include + +// Local header +#include "sbgBasicLoggerAccumulators.h" + +//----------------------------------------------------------------------// +//- Private methods for SbgEComBasicLoggerAcc -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerImuAccZeroInit(SbgBasicLoggerImuAcc *pAccumulator) +{ + assert(pAccumulator); + + memset(pAccumulator, 0x00, sizeof(*pAccumulator)); +} + +void sbgBasicLoggerImuAccAdd(SbgBasicLoggerImuAcc *pAccumulator, const SbgBasicLoggerImu *pNewImuData) +{ + assert(pAccumulator); + assert(pNewImuData); + + pAccumulator->imuAccumulated.timestamp = pNewImuData->timestamp; + pAccumulator->imuAccumulated.status |= pNewImuData->status; + + for (size_t i = 0; i < 3; i++) + { + pAccumulator->imuAccumulated.deltaAngle[i] += pNewImuData->deltaAngle[i]; + pAccumulator->imuAccumulated.deltaVelocity[i] += pNewImuData->deltaVelocity[i]; + } + + pAccumulator->imuAccumulated.temperature += pNewImuData->temperature; + + pAccumulator->nrAcc++; +} + +void sbgBasicLoggerImuAccGet(SbgBasicLoggerImuAcc *pAccumulator, SbgBasicLoggerImu *pDecimatedImu) +{ + assert(pAccumulator); + assert(pDecimatedImu); + + pDecimatedImu->timestamp = pAccumulator->imuAccumulated.timestamp; + pDecimatedImu->status = pAccumulator->imuAccumulated.status; + + for (size_t i = 0; i < 3; i++) + { + pDecimatedImu->deltaAngle[i] = pAccumulator->imuAccumulated.deltaAngle[i] / (double)pAccumulator->nrAcc; + pDecimatedImu->deltaVelocity[i] = pAccumulator->imuAccumulated.deltaVelocity[i] / (double)pAccumulator->nrAcc; + } + + pDecimatedImu->temperature = pAccumulator->imuAccumulated.temperature / (double)pAccumulator->nrAcc; + + sbgBasicLoggerImuAccZeroInit(pAccumulator); +} + diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h new file mode 100644 index 0000000..f636fd5 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerAccumulators.h @@ -0,0 +1,115 @@ +/*! + * \file sbgBasicLoggerAccumulators.h + * \author SBG Systems + * \date July 29, 2021 + * + * \brief Helper accumulators for IMU data. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * \endlicense + */ + +#ifndef SBG_BASIC_LOGGER_ACCUMULATORS_H +#define SBG_BASIC_LOGGER_ACCUMULATORS_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Define a set of IMU data + */ +typedef struct _SbgBasicLoggerImu +{ + uint32_t timestamp; /*!< The data time stamp in us */ + uint16_t status; /*!< IMU data status as defined in sbgECom logs */ + double deltaAngle[3]; /*!< X, Y, Z delta angles values in rad.s^-1 */ + double deltaVelocity[3]; /*!< X, Y, Z delta velocities values in m.s^-2 */ + double temperature; /*!< IMU temperature in °C. */ +} SbgBasicLoggerImu; + +/*! + * Used to accumulate and decimate gyros / accels values. + */ +typedef struct _SbgBasicLoggerImuAcc +{ + size_t nrAcc; /*!< Number of accumulated samples. */ + SbgBasicLoggerImu imuAccumulated; /*!< Accumulated IMU data. */ +} SbgBasicLoggerImuAcc; + + +/*! + * Define a set of raw IMU data. + */ +typedef struct _SbgBasicLoggerImuRaw +{ + uint32_t timestamp; /*!< Time in us since the sensor power up. */ + uint16_t status; /*!< IMU status bitmask. */ + + int64_t rawAccelerometers[3]; /*!< X,Y,Z raw accelerometers signed data. No scale factor defined. */ + int64_t rawGyroscopes[3]; /*!< X,Y,Z raw gyroscopes signed data. No scale factor defined. */ + int64_t rawMagnetometers[3]; /*!< X,Y,Z raw magnetometers signed data. No scale factor defined. */ + + int64_t rawTempAccels[3]; /*!< X,Y,Z raw accelerometers temperature signed data. No scale factor defined. */ + int64_t rawTempGyros[3]; /*!< X,Y,Z raw gyroscopes temperature signed data. No scale factor defined. */ + int64_t rawTempMags[3]; /*!< X,Y,Z raw magnetometers temperature signed data. No scale factor defined. */ + + int64_t rawAuxValues[3]; /*!< Spare raw sensor values for specifics needs. */ + int64_t rawTempAuxValues[3]; /*!< Spare raw sensor temperature values for specifics needs. */ +} SbgBasicLoggerImuRaw; + +/*! + * Used to accumulate and decimate raw IMU values. + */ +typedef struct _SbgBasicLoggerRawImuAcc +{ + size_t nrAcc; /*!< Number of accumulated samples. */ + SbgBasicLoggerImuRaw rawImuAcc; /*!< Accumulated raw IMU data. */ +} SbgBasicLoggerRawImuAcc; + +//----------------------------------------------------------------------// +//- Private methods for SbgEComBasicLoggerAcc -// +//----------------------------------------------------------------------// + +/*! + * Zero initialize / reset an IMU accumulator instance. + * + * \param[in] pAccumulator Instance on the IMU accumulator to reset. + */ +void sbgBasicLoggerImuAccZeroInit(SbgBasicLoggerImuAcc *pAccumulator); + +/*! + * Accumulate a new set of IMU data + * + * \param[in] pAccumulator Instance on the IMU accumulator. + * \param[in] pNewImuData New set of IMU data to accumulate + */ +void sbgBasicLoggerImuAccAdd(SbgBasicLoggerImuAcc *pAccumulator, const SbgBasicLoggerImu *pNewImuData); + +/*! + * Compute the decimated IMU values and reset the accumulator for next iteration. + * + * \param[in] pAccumulator Instance on the IMU accumulator. + * \param[out] pDecimatedImu The decimated IMU values. + */ +void sbgBasicLoggerImuAccGet(SbgBasicLoggerImuAcc *pAccumulator, SbgBasicLoggerImu *pDecimatedImu); + + +#endif // SBG_BASIC_LOGGER_ACCUMULATORS_H diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c new file mode 100644 index 0000000..89a958f --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.c @@ -0,0 +1,88 @@ +// sbgCommonLib headers +#include + +// sbgECom headers +#include +#include + +// Local headers +#include "sbgBasicLoggerFile.h" + +//----------------------------------------------------------------------// +//- Private variables -// +//----------------------------------------------------------------------// + +static SbgBasicLoggerFileDesc gSbgBasicLoggerFileDescs[] = +{ + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_STATUS, .pName = "STATUS", .pFileName = "log_status.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_UTC_TIME, .pName = "UTC", .pFileName = "log_utc.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DIAG, .pName = "DIAG", .pFileName = "log_diag.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_IMU_DATA, .pName = "IMU_DATA", .pFileName = "log_imu.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_IMU_SHORT, .pName = "IMU_SHORT", .pFileName = "log_imuShort.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_MAG, .pName = "MAG", .pFileName = "log_mag.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_MAG_CALIB, .pName = "MAG_CALIB", .pFileName = "log_magCalib.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_EULER, .pName = "EULER", .pFileName = "log_euler.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_QUAT, .pName = "QUAT", .pFileName = "log_quat.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EKF_NAV, .pName = "NAV", .pFileName = "log_nav.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_SHIP_MOTION, .pName = "SHIP_MOTION", .pFileName = "log_shipMotion.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_SHIP_MOTION_HP, .pName = "SHIP_MOTION_HP", .pFileName = "log_shipMotionHP.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_VEL, .pName = "GPS1_VEL", .pFileName = "log_gps1Vel.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_POS, .pName = "GPS1_POS", .pFileName = "log_gps1Pos.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_HDT, .pName = "GPS1_HDT", .pFileName = "log_gps1Hdt.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_RAW, .pName = "GPS1_RAW", .pFileName = "log_gps1Raw.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS1_SAT, .pName = "GPS1_SAT", .pFileName = "log_gps1Sat.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_VEL, .pName = "GPS2_VEL", .pFileName = "log_gps2Vel.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_POS, .pName = "GPS2_POS", .pFileName = "log_gps2Pos.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_HDT, .pName = "GPS2_HDT", .pFileName = "log_gps2Hdt.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_RAW, .pName = "GPS2_RAW", .pFileName = "log_gps2Raw.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_GPS2_SAT, .pName = "GPS2_SAT", .pFileName = "log_gps2Sat.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_RTCM_RAW, .pName = "RTCM_RAW", .pFileName = "log_rtcmRaw.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_ODO_VEL, .pName = "ODO_VEL", .pFileName = "log_odo.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DVL_BOTTOM_TRACK, .pName = "DVL_BOTTOM", .pFileName = "log_dvl_bottom.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DVL_WATER_TRACK, .pName = "DVL_WATER", .pFileName = "log_dvl_water.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_AIR_DATA, .pName = "AIR_DATA", .pFileName = "log_air.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_USBL, .pName = "USBL", .pFileName = "log_usbl.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_DEPTH, .pName = "DEPTH", .pFileName = "log_depth.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_A, .pName = "EVENT_IN_A", .pFileName = "log_eventInA.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_B, .pName = "EVENT_IN_B", .pFileName = "log_eventInB.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_C, .pName = "EVENT_IN_C", .pFileName = "log_eventInC.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_D, .pName = "EVENT_IN_D", .pFileName = "log_eventInD.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_E, .pName = "EVENT_IN_E", .pFileName = "log_eventInE.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_OUT_A, .pName = "EVENT_OUT_A", .pFileName = "log_eventOutA.txt" }, + { .class = SBG_ECOM_CLASS_LOG_ECOM_0, .id = SBG_ECOM_LOG_EVENT_OUT_B, .pName = "EVENT_OUT_B", .pFileName = "log_eventOutB.txt" }, + + { .class = SBG_ECOM_CLASS_LOG_ECOM_1, .id = SBG_ECOM_LOG_FAST_IMU_DATA, .pName = "IMU_FAST", .pFileName = "log_imuFast.txt" }, + +}; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +const SbgBasicLoggerFileDesc *sbgBasicLoggerFileGetDesc(SbgEComClass msgClass, SbgEComMsgId msgId) +{ + const SbgBasicLoggerFileDesc *pDesc = NULL; + + for (size_t i = 0; i < SBG_ARRAY_SIZE(gSbgBasicLoggerFileDescs); i++) + { + const SbgBasicLoggerFileDesc *pTmpDesc; + + pTmpDesc = &gSbgBasicLoggerFileDescs[i]; + + if ((pTmpDesc->class == msgClass) && (pTmpDesc->id == msgId)) + { + pDesc = pTmpDesc; + break; + } + } + + return pDesc; +} diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h new file mode 100644 index 0000000..c92badc --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerFile.h @@ -0,0 +1,60 @@ +/*! + * \file sbgBasicLoggerAccumulators.h + * \author SBG Systems + * \date July 29, 2021 + * + * \brief Ease output log files management. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * \endlicense + */ + +#ifndef SBG_BASIC_LOGGER_FILE_H +#define SBG_BASIC_LOGGER_FILE_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * SBG basic logger file descriptor. + */ +typedef struct _SbgBasicLoggerFileDesc +{ + SbgEComClass class; /*!< Message class. */ + SbgEComMsgId id; /*!< Message ID. */ + const char *pName; /*!< The log name. */ + const char *pFileName; /*!< File name suffix. */ +} SbgBasicLoggerFileDesc; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * Return the file descriptor. + * + * \param[in] msgClass Message class. + * \param[in] msgId Message ID. + * \return File descriptor. + */ +const SbgBasicLoggerFileDesc *sbgBasicLoggerFileGetDesc(SbgEComClass msgClass, SbgEComMsgId msgId); + +#endif // SBG_BASIC_LOGGER_FILE_H diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c new file mode 100644 index 0000000..64bea96 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.c @@ -0,0 +1,2064 @@ +// sbgCommonLib headers +#include +#include + +// sbgECom headers +#include + +// Local headers +#include "sbgBasicLoggerAccumulators.h" +#include "sbgBasicLoggerHandler.h" + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Write IMU short data into an output stream. + * + * \param[in] pImuShort IMU short data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteImu(const SbgBasicLoggerImu *pImuShort, void *pStream) +{ + assert(pImuShort); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.12lf\t%0.3lf\n", + pImuShort->timestamp, pImuShort->status, + pImuShort->deltaVelocity[0], pImuShort->deltaVelocity[1], pImuShort->deltaVelocity[2], + sbgRadToDegd(pImuShort->deltaAngle[0]), sbgRadToDegd(pImuShort->deltaAngle[1]), sbgRadToDegd(pImuShort->deltaAngle[2]), + pImuShort->temperature); + + fflush(pStream); +} + +/*! + * Write IMU data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteImuHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\tdeltaVelX\tdeltaVelY\tdeltaVelZ\tdeltaAngleX\tdeltaAngleY\tdeltaAngleZ\ttemperature\n"); + fprintf(pStream, "(us)\t(N/A)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\t(deg.s^-1)\t(deg.s^-1)\t(deg.s^-1)\t(°C)\n"); + fflush(pStream); +} + + +/*! + * Write a diag data into an output stream. + * + * \param[in] pDiag Diag data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteDiag(const SbgLogDiagData *pDiag, const char *pUtcTimeStr, void *pStream) +{ + assert(pDiag); + assert(pUtcTimeStr); + assert(pStream); + + switch (pDiag->type) + { + case SBG_DEBUG_LOG_TYPE_ERROR: + fprintf(pStream, "%s *ERR * [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); + break; + case SBG_DEBUG_LOG_TYPE_WARNING: + fprintf(pStream, "%s *WARN* [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); + break; + case SBG_DEBUG_LOG_TYPE_INFO: + fprintf(pStream, "%s *INFO* %s\n", pUtcTimeStr, pDiag->string); + break; + case SBG_DEBUG_LOG_TYPE_DEBUG: + fprintf(pStream, "%s *DBG * %s\n", pUtcTimeStr, pDiag->string); + break; + default: + fprintf(pStream, "%s *UKN * [%s]: %s\n", pUtcTimeStr, sbgErrorCodeToString(pDiag->errorCode), pDiag->string); + } + + fflush(pStream); +} + +/*! + * Write an UTC data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteUtcHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\tgpsTimeOfWeek\tyear\tmonth\tday\thour\tminute\tsecond\tnanosecond\n"); + fprintf(pStream, "(us)\t(NA)\t(ms)\t(yyyy)\t(mm)\t(dd)\t(hh)\t(mm)\t(ss)\t(ns)\n"); + fflush(pStream); +} + +/*! + * Write an UTC data into an output stream. + * + * \param[in] pUtc UTC data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteUtc(const SbgLogUtcData *pUtc, void *pStream) +{ + assert(pUtc); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu32"\t%"PRIu16"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%0.2"PRId8"\t%"PRId32"\n", + pUtc->timeStamp, + pUtc->status, + pUtc->gpsTimeOfWeek, + pUtc->year, + pUtc->month, + pUtc->day, + pUtc->hour, + pUtc->minute, + pUtc->second, + pUtc->nanoSecond + ); + fflush(pStream); +} + +/*! + * Write a status data into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteStatusHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tgeneral\tcom2\tcom\taiding\n"); + fprintf(pStream, "(us)\t(NA)\t(NA)\t(NA)\t(NA)\n"); + fflush(pStream); +} + +/*! + * Write a status data into an output stream. + * + * \param[in] pStatus Status data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteStatus(const SbgLogStatusData *pStatus, void *pStream) +{ + assert(pStatus); + assert(pStream); + + fprintf(pStream, "%" PRIu32 "\t%" PRIu16 "\t%" PRIu16 "\t%" PRIu32 "\t%" PRIu32 "\n", + pStatus->timeStamp, + pStatus->generalStatus, + pStatus->comStatus2, + pStatus->comStatus, + pStatus->aidingStatus + ); + + fflush(pStream); +} + +/*! + * Write an EKF Euler data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfEulerHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\troll\tpitch\tyaw\trollStd\tpitchStd\tyawStd\n"); + fprintf(pStream, "(us)\t(NA)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\t(deg)\n"); + fflush(pStream); +} + +/*! + * Write an EKF Euler data into an output stream. + * + * \param[in] pData EKF Euler data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfEuler(const SbgLogEkfEulerData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + sbgRadToDegf(pData->euler[0]), + sbgRadToDegf(pData->euler[1]), + sbgRadToDegf(pData->euler[2]), + sbgRadToDegf(pData->eulerStdDev[0]), + sbgRadToDegf(pData->eulerStdDev[1]), + sbgRadToDegf(pData->eulerStdDev[2]) + ); + fflush(pStream); +} + +/*! + * Write an EKF Euler data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfQuatHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\tW\tX\tY\tZ\trollStd\tpitchStd\tyawStd\n"); + fprintf(pStream, "(us)\t(NA)\t(NA)\t(NA)\t(NA)\t(NA)\t(deg)\t(deg)\t(deg)\n"); + fflush(pStream); +} + +/*! + * Write an EKF quat data into an output stream. + * + * \param[in] pData EKF quat data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfQuat(const SbgLogEkfQuatData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->quaternion[0], + pData->quaternion[1], + pData->quaternion[2], + pData->quaternion[3], + sbgRadToDegf(pData->eulerStdDev[0]), + sbgRadToDegf(pData->eulerStdDev[1]), + sbgRadToDegf(pData->eulerStdDev[2]) + ); + fflush(pStream); +} + +/*! + * Write an EKF nav data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfNavHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\tvelN\tvelE\tvelD\tvelStdN\tvelStdE\tvelStdD\tlatitude\tlongitude\taltitude\tlatitudeStd\tlongitudeStd\taltitudeStd\tundulation\n"); + fprintf(pStream, "(us)\t(NA)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(deg)\t(deg)\t(m)\t(m)\t(m)\t(m)\t(m)\n"); + fflush(pStream); +} + +/*! + * Write an EKF nav data into an output stream. + * + * \param[in] pData EKF nav data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEkfNav(const SbgLogEkfNavData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->velocity[0], + pData->velocity[1], + pData->velocity[2], + pData->velocityStdDev[0], + pData->velocityStdDev[1], + pData->velocityStdDev[2], + pData->position[0], + pData->position[1], + pData->position[2], + pData->positionStdDev[0], + pData->positionStdDev[1], + pData->positionStdDev[2], + pData->undulation + ); + fflush(pStream); +} + +/*! + * Write a ship motion data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteShipMotionHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tsurge\tsway\theave\tsurgeVel\tswayVel\theaveVel\tsurgeAccel\tswayAccel\theaveAccel\theavePeriod\n"); + fprintf(pStream, "(us)\t(NA)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(m)\t(s)\n"); + fflush(pStream); +} + +/*! + * Write a ship motion data into an output stream. + * + * \param[in] pData Ship motion data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteShipMotion(const SbgLogShipMotionData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", + pData->timeStamp, + pData->status, + pData->shipMotion[0], + pData->shipMotion[1], + pData->shipMotion[2], + pData->shipVel[0], + pData->shipVel[1], + pData->shipVel[2], + pData->shipAccel[0], + pData->shipAccel[1], + pData->shipAccel[2], + pData->mainHeavePeriod + ); + fflush(pStream); +} + +/*! + * Write a GPS vel data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsVelHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\ttimeOfWeek\tvelN\tvelE\tvelD\tvelNStd\tvelEStd\tvelDStd\tcourse\tcourseStd\n"); + fprintf(pStream, "(us)\t(NA)\t(ms)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(degrees)\t(degrees)\n"); + fflush(pStream); +} + +/*! + * Write a GPS vel data into an output stream. + * + * \param[in] pData GPS vel data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsVel(const SbgLogGpsVel *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->timeOfWeek, + pData->velocity[0], + pData->velocity[1], + pData->velocity[2], + pData->velocityAcc[0], + pData->velocityAcc[1], + pData->velocityAcc[2], + pData->course, + pData->courseAcc + ); + fflush(pStream); +} + +/*! + * Write a GPS data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\ttimeOfWeek\tstatus\taltitude\n"); + fprintf(pStream, "(us)\t(ms)\t(NA)\t(m)\n"); + fflush(pStream); +} + +/*! + * Write a GPS data into an output stream. + * + * \param[in] pData GPS data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGps(const SbgLogGpsPos *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu32"\t%"PRIu32"\t%lf\n", + pData->timeStamp, + pData->timeOfWeek, + pData->status, + pData->altitude); + fflush(pStream); +} + +/*! + * Write a GPS hdt data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsHdtHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\ttimeOfWeek\theading\theadingStd\tpitch\tpitchStd\tbaseline\n"); + fprintf(pStream, "(us)\t(NA)\t(ms)\t(degrees)\t(degrees)\t(degrees)\t(degrees)\t(m)\n"); + fflush(pStream); +} + +/*! + * Write a GPS hdt data into an output stream. + * + * \param[in] pData GPS hdt data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsHdt(const SbgLogGpsHdt *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu32"\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->timeOfWeek, + pData->heading, + pData->headingAccuracy, + pData->pitch, + pData->pitchAccuracy, + pData->baseline + ); + fflush(pStream); +} + +/*! + * Write a GPS Satellite data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsSatHeader(void *pStream) +{ + assert(pStream); +} + +/*! + * Write a GPS Satellite data into an output stream. + * + * \param[in] pData GPS satellite data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsSat(const SbgLogSatGroupData *pData, void *pStream) +{ + size_t satelliteIdx; + size_t signalIdx; + + assert(pData); + assert(pStream); + + fprintf(pStream, "%10"PRIu32" us | Satellites: %3zu ====================================================================\n", + pData->timeStamp, + pData->nrSatellites); + + for (satelliteIdx = 0; satelliteIdx < pData->nrSatellites; satelliteIdx++) + { + const SbgLogSatData *pSatEntry = &pData->pSatData[satelliteIdx]; + SbgEComSatElevationStatus elevationStatus; + char elevationStatusChr; + + elevationStatus = sbgLogSatDataGetElevationStatus(pSatEntry); + + if (elevationStatus == SBG_ECOM_SAT_ELEVATION_STATUS_SETTING) + { + elevationStatusChr = 's'; + } + else if (elevationStatus == SBG_ECOM_SAT_ELEVATION_STATUS_RISING) + { + elevationStatusChr = 'r'; + } + else + { + elevationStatusChr = '*'; + } + + fprintf(pStream, " [ID: %2"PRIu8" %-8s] Elevation: %3"PRIi8"%c Azimuth: %3"PRIi16" nrSignals: %2zu %-10s%-10s\n", + pSatEntry->id, + sbgLogSatDataGetConstellationIdAsStr(pSatEntry), + pSatEntry->elevation, + elevationStatusChr, + pSatEntry->azimuth, + pSatEntry->nrSignals, + sbgLogSatDataGetHealthStatusAsStr(pSatEntry), + sbgLogSatDataGetTrackingStatusAsStr(pSatEntry)); + + for (signalIdx = 0; signalIdx < pSatEntry->nrSignals; signalIdx++) + { + const SbgLogSatSignalData *pSignalEntry = &pSatEntry->pSignalData[signalIdx]; + char snrStr[16]; + + if (sbgLogSatSignalDataSnrIsValid(pSignalEntry)) + { + sprintf(snrStr, "%-3"PRIu8, pSignalEntry->snr); + } + else + { + strcpy(snrStr, " -"); + } + + fprintf(pStream, " %-14s SNR: %3s %-10s %-10s\n", + sbgLogSatSignalDataGetSignalIdAsStr(pSignalEntry), + snrStr, + sbgLogSatSignalDataGetHealthStatusAsStr(pSignalEntry), + sbgLogSatSignalDataGetTrackingStatusAsStr(pSignalEntry)); + } + } + + fflush(pStream); +} + +/*! + * Write an odometer data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteOdometerHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tvelocity\n"); + fprintf(pStream, "(us)\t(NA)\t(m.s-1)\n"); + fflush(pStream); +} + +/*! + * Write an odometer data into an output stream. + * + * \param[in] pData Odometer data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteOdometer(const SbgLogOdometerData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\n", + pData->timeStamp, + pData->status, + pData->velocity + ); + fflush(pStream); +} + +/*! + * Write a DVL data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteDvlHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tvelX\tvelY\tvelZ\tvelQualityX\tvelQualityY\tvelQualityZ\n"); + fprintf(pStream, "(us)\t(NA)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\t(m.s-1)\n"); + fflush(pStream); +} + +/*! + * Write an DVL data into an output stream. + * + * \param[in] pData DVL data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteDvl(const SbgLogDvlData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->velocity[0], + pData->velocity[1], + pData->velocity[2], + pData->velocityQuality[0], + pData->velocityQuality[1], + pData->velocityQuality[2] + ); + fflush(pStream); +} + +/*! + * Write an air data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteAirHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tpressure\taltitude\tpressureDiff\tairSpeed\tairTemp\n"); + fprintf(pStream, "(us)\t(NA)\t(pascal)\t(meters)\t(pascal)\t(m.s-1)\t(celcius degrees)\n"); + fflush(pStream); +} + +/*! + * Write an air data into an output stream. + * + * \param[in] pData Air data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteAir(const SbgLogAirData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->pressureAbs, + pData->altitude, + pData->pressureDiff, + pData->trueAirspeed, + pData->airTemperature + ); + fflush(pStream); +} + +/*! + * Write an USBL data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteUsblHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tlatitude\tlongitude\tdepth\tlatitudeStd\tlongitudeStd\tdepthStd\n"); + fprintf(pStream, "(us)\t(NA)\t(degrees)\t(degrees)\t(meters)\t(meters)\t(meters)\t(meters)\n"); + fflush(pStream); +} + +/*! + * Write an USBL data into an output stream. + * + * \param[in] pData Air data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteUsbl(const SbgLogUsblData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->latitude, + pData->longitude, + pData->depth, + pData->latitudeAccuracy, + pData->longitudeAccuracy, + pData->depthAccuracy + ); + fflush(pStream); +} + +/*! + * Write a depth data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteDepthHeader(void *pStream) +{ + assert(pStream); + fprintf(pStream, "timestamp\tstatus\tpressure\tdepth\n"); + fprintf(pStream, "(us)\t(NA)\t(pascal)\t(meters)\n"); + fflush(pStream); +} + +/*! + * Write an depth data into an output stream. + * + * \param[in] pData Depth data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteDepth(const SbgLogDepth *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%lf\t%lf\n", + pData->timeStamp, + pData->status, + pData->pressureAbs, + pData->altitude + ); + fflush(pStream); +} + +/*! + * Write a GPS raw data into an output stream. + * + * \param[in] pData GPS raw data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteRawRtcm(const SbgLogRawData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%zu\n", + pData->bufferSize); + fflush(pStream); +} + +/*! + * Write a GPS raw data into an output stream. + * + * \param[in] pData GPS raw data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteGpsRaw(const SbgLogRawData *pData, void *pStream) +{ + assert(pData); + assert(pStream); + + fprintf(pStream, "%zu\n", + pData->bufferSize); + fflush(pStream); +} + +/*! + * Write event data header into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEventHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\ttimeOffset0\ttimeOffset1\ttimeOffset2\ttimeOffset3\n"); + fflush(pStream); +} + +/*! + * Write event data into an output stream. + * + * \param[in] pEvent Event data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteEvent(const SbgLogEvent *pEvent, void *pStream) +{ + assert(pEvent); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\t%"PRIu16"\n", + pEvent->timeStamp, + pEvent->status, + pEvent->timeOffset0, + pEvent->timeOffset1, + pEvent->timeOffset2, + pEvent->timeOffset3); + fflush(pStream); +} + +/*! + * Write mag data into an output stream. + * + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteMagHeader(void *pStream) +{ + assert(pStream); + + fprintf(pStream, "timestamp\tstatus\tmagX\tmagY\tmagZ\taccelX\taccelY\taccelZ\n"); + fprintf(pStream, "(us)\t(NA)\t(A.U)\t(A.U)\t(A.U)\t(m.s^-2)\t(m.s^-2)\t(m.s^-2)\n"); + fflush(pStream); +} + +/*! + * Write mag data into an output stream. + * + * \param[in] pMag Mag data. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerHandlerWriteMag(const SbgLogMag *pMag, void *pStream) +{ + assert(pMag); + assert(pStream); + + fprintf(pStream, "%"PRIu32"\t%"PRIu16"\t%f\t%f\t%f\t%f\t%f\t%f\n", + pMag->timeStamp, + pMag->status, + pMag->magnetometers[0], pMag->magnetometers[1], pMag->magnetometers[2], + pMag->accelerometers[0], pMag->accelerometers[1], pMag->accelerometers[2]); + fflush(pStream); +} + +static SbgErrorCode sbgBasicLoggerHandlerGetUtcTimeFromTimeStamp(uint32_t timeStamp, SbgLogUtcData *pLastUtcData, struct tm *pRefUtcTimestamp, uint32_t *pUs) +{ + SbgErrorCode errorCode = SBG_NO_ERROR; + + assert(pLastUtcData); + assert(pRefUtcTimestamp); + assert(pUs); + + if (sbgEComLogUtcGetClockUtcStatus(pLastUtcData->status) != SBG_ECOM_UTC_INVALID) + { + time_t nrSeconds; + int64_t nrMicroseconds; + int32_t elapsedTime; + + pRefUtcTimestamp->tm_sec = pLastUtcData->second; + pRefUtcTimestamp->tm_min = pLastUtcData->minute; + pRefUtcTimestamp->tm_hour = pLastUtcData->hour; + pRefUtcTimestamp->tm_mday = pLastUtcData->day; + pRefUtcTimestamp->tm_mon = pLastUtcData->month - 1; + pRefUtcTimestamp->tm_year = pLastUtcData->year - 1900; + pRefUtcTimestamp->tm_isdst = -1; + + nrSeconds = mktime(pRefUtcTimestamp); + nrMicroseconds = (int64_t)nrSeconds * 1000000; + elapsedTime = timeStamp - pLastUtcData->timeStamp; + nrMicroseconds += elapsedTime; + + nrSeconds = nrMicroseconds / 1000000; + nrMicroseconds %= 1000000; + + *pRefUtcTimestamp = *localtime(&nrSeconds); + *pUs = (uint32_t)nrMicroseconds; + } + else + { + errorCode = SBG_ERROR; + } + + return errorCode; +} + +//----------------------------------------------------------------------// +//- sbgBasicLoggerCommonHandler methods -// +//----------------------------------------------------------------------// + +/*! + * Check if a handler is initialized. + * + * \param[in] pHandler Common handler. + * \return True if initialized. + */ +static bool sbgBasicLoggerCommonHandlerInitialized(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(pHandler); + + if (pHandler->pDesc) + { + return true; + } + else + { + return false; + } +} + +/*! + * Common handler constructor. + * + * \param[in] pHandler IMU short handler. + * \param[in] consoleEnabled True to print IMU short data on console. + * \param[in] writingFile True to write IMU short data into a file. + */ +static void sbgBasicLoggerCommonHandlerConstruct(SbgBasicLoggerCommonHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader, SbgEComClass class, SbgEComMsgId id) +{ + assert(pHandler); + + pHandler->consoleEnabled = consoleEnabled; + pHandler->writingFile = writingFile; + pHandler->pDesc = sbgBasicLoggerFileGetDesc(class, id); + pHandler->pFILE = NULL; + + pHandler->consoleDecimation = consoleDecimation; + pHandler->fileDecimation = fileDecimation; + pHandler->writeHeader = writeHeader; + pHandler->headerWritten = false; + + pHandler->pPathStr = pPathStr; + pHandler->class = class; + pHandler->id = id; +} + +/*! + * Check if a file is opened. + * + * \param[in] pHandler Common handler. + * \return True if opened. + */ +static bool sbgBasicLoggerCommonHandlerFileOpened(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + return pHandler->pFILE; +} + +/*! + * Check if writing file is enabled. + * + * \param[in] pHandler Common handler. + * \return True if enabled. + */ +static bool sbgBasicLoggerCommonHandlerWritingFile(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + return pHandler->writingFile; +} + +/*! + * Check if console is enabled. + * + * \param[in] pHandler Common handler. + * \return True if enabled. + */ +static bool sbgBasicLoggerCommonHandlerConsoleEnabled(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + return pHandler->consoleEnabled; +} + +/*! + * Check if header writing is enabled. + * + * \param[in] pHandler Common handler. + * \return True if enabled. + */ +static bool sbgBasicLoggerCommonHandlerHeaderWriting(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + return pHandler->writeHeader; +} + +/*! + * Check if header have been written. + * + * \param[in] pHandler Common handler. + * \return True if written. + */ +static bool sbgBasicLoggerCommonHandlerHeaderWritten(SbgBasicLoggerCommonHandler *pHandler) +{ + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + return pHandler->headerWritten; +} + +/*! + * Get the path of the handler. + * + * \param[in] pHandler Common handler. + * \return File path. + */ +static FILE *sbgBasicLoggerCommonHandlerGetPath(SbgBasicLoggerCommonHandler *pHandler) +{ + if (!sbgBasicLoggerCommonHandlerFileOpened(pHandler)) + { + if (pHandler->pDesc) + { + SbgErrorCode errorCode; + SbgString fileStr; + SbgString pathStr; + + errorCode = sbgStringConstructCString(&pathStr, pHandler->pPathStr); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgStringConstructCString(&fileStr, pHandler->pDesc->pFileName); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgStringAppend(&pathStr, &fileStr); + + if (errorCode == SBG_NO_ERROR) + { + pHandler->pFILE = fopen(sbgStringGetCString(&pathStr), "wt"); + } + + sbgStringDestroy(&fileStr); + } + + sbgStringDestroy(&pathStr); + } + } + } + + return pHandler->pFILE; +} + +/*! + * Write log info on an output stream. + * + * \param[in] pHandler Common handler. + * \param[in] pStream Output stream. + */ +static void sbgBasicLoggerCommonHandlerWriteLogInfo(SbgBasicLoggerCommonHandler *pHandler, void *pStream) +{ + const SbgBasicLoggerFileDesc *pMsgDesc; + + assert(sbgBasicLoggerCommonHandlerInitialized(pHandler)); + assert(pStream); + + pMsgDesc = sbgBasicLoggerFileGetDesc(pHandler->class, pHandler->id); + + if (pMsgDesc) + { + fprintf(pStream, "%14s: ", pMsgDesc->pName); + } + else + { + fprintf(pStream, "class:%0.2u id:%0.2u: ", pHandler->class, pHandler->id); + } +} + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +//----------------------------------------------------------------------// +//- IMU short handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerImuShortHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) +{ + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_SHORT); + + sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); + sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); +} + +void sbgBasicLoggerImuHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) +{ + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_IMU_DATA); + + sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); + sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); +} + +void sbgBasicLoggerImuFastHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader) +{ + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, consoleDecimation, writingFile, pPathStr, fileDecimation, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_1, SBG_ECOM_LOG_FAST_IMU_DATA); + + sbgBasicLoggerImuAccZeroInit(&pHandler->consoleAcc); + sbgBasicLoggerImuAccZeroInit(&pHandler->fileAcc); +} + +void sbgBasicLoggerImuHandlerProcess(SbgBasicLoggerImuHandler *pHandler, const SbgBasicLoggerImu *pNewData) +{ + assert(pNewData); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if ((sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler)) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteImuHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerImuAccAdd(&pHandler->fileAcc, pNewData); + + if (pHandler->fileAcc.nrAcc >= pHandler->commonHandler.fileDecimation) + { + SbgBasicLoggerImu decimated; + + sbgBasicLoggerImuAccGet(&pHandler->fileAcc, &decimated); + + sbgBasicLoggerHandlerWriteImu(&decimated, pOutputFile); + } + } + else + { + SBG_LOG_ERROR(SBG_NULL_POINTER, "no file found for IMU short log"); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerImuAccAdd(&pHandler->consoleAcc, pNewData); + + if (pHandler->consoleAcc.nrAcc >= pHandler->commonHandler.consoleDecimation) + { + SbgBasicLoggerImu decimated; + + sbgBasicLoggerImuAccGet(&pHandler->consoleAcc, &decimated); + + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteImu(&decimated, stdout); + } + } +} + +//----------------------------------------------------------------------// +//- IMU cluster handler methods -// +//----------------------------------------------------------------------// + + +//----------------------------------------------------------------------// +//- Diagnostic handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerDiagHandlerConstruct(SbgBasicLoggerDiagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DIAG); +} + +void sbgBasicLoggerDiagHandlerProcess(SbgBasicLoggerDiagHandler *pHandler, SbgLogUtcData *pLastUtcData, const SbgLogDiagData *pDiag) +{ + struct tm utcTime; + char utcTimeStr[128]; + uint32_t us; + + assert(pHandler); + assert(pDiag); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + if (sbgBasicLoggerHandlerGetUtcTimeFromTimeStamp(pDiag->timestamp, pLastUtcData, &utcTime, &us) == SBG_NO_ERROR) + { + char dateStr[64]; + + strftime(dateStr, sizeof(dateStr), "%Y-%m-%dT%H:%M:%S", &utcTime); + snprintf(utcTimeStr, sizeof(utcTimeStr), "%s:%.3" PRIu32 "Z", dateStr, us / 1000); + } + else + { + snprintf(utcTimeStr, sizeof(utcTimeStr), "%10u", pDiag->timestamp); + } + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + sbgBasicLoggerHandlerWriteDiag(pDiag, utcTimeStr, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerHandlerWriteDiag(pDiag, utcTimeStr, stdout); + } +} + +//----------------------------------------------------------------------// +//- UTC handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerUtcHandlerConstruct(SbgBasicLoggerUtcHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_UTC_TIME); +} + +void sbgBasicLoggerUtcHandlerProcess(SbgBasicLoggerUtcHandler *pHandler, const SbgLogUtcData *pUtc) +{ + assert(pHandler); + assert(pUtc); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteUtcHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteUtc(pUtc, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteUtc(pUtc, stdout); + } +} + +//----------------------------------------------------------------------// +//- Status handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerStatusHandlerConstruct(SbgBasicLoggerStatusHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_STATUS); +} + +void sbgBasicLoggerStatusHandlerProcess(SbgBasicLoggerStatusHandler *pHandler, const SbgLogStatusData *pStatus) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pStatus); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteStatusHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteStatus(pStatus, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteStatus(pStatus, stdout); + } +} + +//----------------------------------------------------------------------// +//- EKF Euler handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerEkfEulerHandlerConstruct(SbgBasicLoggerEkfEulerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_EULER); +} + +void sbgBasicLoggerEkfEulerHandlerProcess(SbgBasicLoggerEkfEulerHandler *pHandler, const SbgLogEkfEulerData *pEkfEuler) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pEkfEuler); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteEkfEulerHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteEkfEuler(pEkfEuler, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteEkfEuler(pEkfEuler, stdout); + } +} + +//----------------------------------------------------------------------// +//- EKF quat handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerEkfQuatHandlerConstruct(SbgBasicLoggerEkfQuatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_QUAT); +} + +void sbgBasicLoggerEkfQuatHandlerProcess(SbgBasicLoggerEkfQuatHandler *pHandler, const SbgLogEkfQuatData *pEkfQuat) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pEkfQuat); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteEkfQuatHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteEkfQuat(pEkfQuat, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteEkfQuat(pEkfQuat, stdout); + } +} + +//----------------------------------------------------------------------// +//- EKF nav handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerEkfNavHandlerConstruct(SbgBasicLoggerEkfNavHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EKF_NAV); +} + +void sbgBasicLoggerEkfNavHandlerProcess(SbgBasicLoggerEkfNavHandler *pHandler, const SbgLogEkfNavData *pEkfNav) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pEkfNav); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteEkfNavHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteEkfNav(pEkfNav, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteEkfNav(pEkfNav, stdout); + } +} + +//----------------------------------------------------------------------// +//- Ship motion methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerShipMotionHandlerConstruct(SbgBasicLoggerShipMotionHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_SHIP_MOTION); +} + +void sbgBasicLoggerShipMotionHandlerProcess(SbgBasicLoggerShipMotionHandler *pHandler, const SbgLogShipMotionData *pShipMotion) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pShipMotion); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteShipMotionHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, stdout); + } +} + +//----------------------------------------------------------------------// +//- Ship motion HP methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerShipMotionHpHandlerConstruct(SbgBasicLoggerShipMotionHpHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_SHIP_MOTION_HP); +} + +void sbgBasicLoggerShipMotionHpHandlerProcess(SbgBasicLoggerShipMotionHpHandler *pHandler, const SbgLogShipMotionData *pShipMotion) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pShipMotion); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteShipMotionHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteShipMotion(pShipMotion, stdout); + } +} + +//----------------------------------------------------------------------// +//- GPS vel handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerGpsVel1HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_VEL); +} + +void sbgBasicLoggerGpsVel2HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_VEL); +} + +void sbgBasicLoggerGpsVelHandlerProcess(SbgBasicLoggerGpsVelHandler *pHandler, const SbgLogGpsVel *pGps) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pGps); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteGpsVelHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteGpsVel(pGps, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteGpsVel(pGps, stdout); + } +} + +//----------------------------------------------------------------------// +//- GPS pos handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerGpsPos1HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_POS); +} + +void sbgBasicLoggerGpsPos2HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_POS); +} + +void sbgBasicLoggerGpsPosHandlerProcess(SbgBasicLoggerGpsPosHandler *pHandler, const SbgLogGpsPos *pGps) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pGps); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteGpsHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteGps(pGps, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteGps(pGps, stdout); + } +} + +//----------------------------------------------------------------------// +//- GPS hdt handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerGpsHdt1HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_HDT); +} + +void sbgBasicLoggerGpsHdt2HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_HDT); +} + +void sbgBasicLoggerGpsHdtHandlerProcess(SbgBasicLoggerGpsHdtHandler *pHandler, const SbgLogGpsHdt *pGps) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pGps); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteGpsHdtHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteGpsHdt(pGps, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteGpsHdt(pGps, stdout); + } +} + +//----------------------------------------------------------------------// +//- Raw GPS handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerGps1RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_RAW); +} + +void sbgBasicLoggerGps2RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_RAW); +} + +void sbgBasicLoggerGpsRawHandlerProcess(SbgBasicLoggerGpsRawHandler *pHandler, const SbgLogRawData *pRawGps) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pRawGps); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + sbgBasicLoggerHandlerWriteGpsRaw(pRawGps, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteGpsRaw(pRawGps, stdout); + } +} + +//----------------------------------------------------------------------// +//- GNSS Satellite in View handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerGps1SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS1_SAT); +} + +void sbgBasicLoggerGps2SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_GPS2_SAT); +} + +void sbgBasicLoggerGpsSatHandlerProcess(SbgBasicLoggerGpsSatHandler *pHandler, const SbgLogSatGroupData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteGpsSatHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteGpsSat(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteGpsSat(pData, stdout); + } +} +//----------------------------------------------------------------------// +//- Odometer handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerOdometerHandlerConstruct(SbgBasicLoggerOdometerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_ODO_VEL); +} + +void sbgBasicLoggerOdometerHandlerProcess(SbgBasicLoggerOdometerHandler *pHandler, const SbgLogOdometerData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteOdometerHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteOdometer(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteOdometer(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- DVL handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerDvlBottomHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DVL_BOTTOM_TRACK); +} + +void sbgBasicLoggerDvlWaterHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DVL_WATER_TRACK); +} + +void sbgBasicLoggerDvlHandlerProcess(SbgBasicLoggerDvlHandler *pHandler, const SbgLogDvlData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteDvlHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteDvl(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteDvl(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- Air handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerAirHandlerConstruct(SbgBasicLoggerAirHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA); +} + +void sbgBasicLoggerAirHandlerProcess(SbgBasicLoggerAirHandler *pHandler, const SbgLogAirData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteAirHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteAir(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteAir(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- USBL handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerUsblHandlerConstruct(SbgBasicLoggerUsblHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_USBL); +} + +void sbgBasicLoggerUsblHandlerProcess(SbgBasicLoggerUsblHandler *pHandler, const SbgLogUsblData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteUsblHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteUsbl(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteUsbl(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- Depth handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerDepthHandlerConstruct(SbgBasicLoggerDepthHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_DEPTH); +} + +void sbgBasicLoggerDepthHandlerProcess(SbgBasicLoggerDepthHandler *pHandler, const SbgLogDepth *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteDepthHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteDepth(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerHandlerWriteDepth(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- Raw RTCM handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerRawRtcmHandlerConstruct(SbgBasicLoggerRawRtcmHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, 0, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_RTCM_RAW); +} + +void sbgBasicLoggerRawRtcmHandlerProcess(SbgBasicLoggerRawRtcmHandler *pHandler, const SbgLogRawData *pData) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pData); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + sbgBasicLoggerHandlerWriteRawRtcm(pData, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteRawRtcm(pData, stdout); + } +} + +//----------------------------------------------------------------------// +//- Events handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerEventInAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_A); +} + +void sbgBasicLoggerEventInBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_B); +} + +void sbgBasicLoggerEventInCHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_C); +} + +void sbgBasicLoggerEventInDHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_D); +} + +void sbgBasicLoggerEventInEHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_E); +} + +void sbgBasicLoggerEventOutAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_OUT_A); +} + +void sbgBasicLoggerEventOutBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_EVENT_OUT_B); +} + +void sbgBasicLoggerEventHandlerProcess(SbgBasicLoggerEventHandler *pHandler, const SbgLogEvent *pEvent) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pEvent); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteEventHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteEvent(pEvent, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteEvent(pEvent, stdout); + } +} + +//----------------------------------------------------------------------// +//- Mag handler methods -// +//----------------------------------------------------------------------// + +void sbgBasicLoggerMagHandlerConstruct(SbgBasicLoggerMagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader) +{ + assert(pHandler); + assert(!sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + sbgBasicLoggerCommonHandlerConstruct(&pHandler->commonHandler, consoleEnabled, 0, writingFile, pPathStr, 0, writeHeader, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG); +} + +void sbgBasicLoggerMagHandlerProcess(SbgBasicLoggerMagHandler *pHandler, const SbgLogMag *pMag) +{ + assert(pHandler); + assert(sbgBasicLoggerCommonHandlerInitialized(&pHandler->commonHandler)); + assert(pMag); + + if (sbgBasicLoggerCommonHandlerWritingFile(&pHandler->commonHandler)) + { + FILE *pOutputFile; + + pOutputFile = sbgBasicLoggerCommonHandlerGetPath(&pHandler->commonHandler); + + if (pOutputFile) + { + if (sbgBasicLoggerCommonHandlerHeaderWriting(&pHandler->commonHandler) && (!sbgBasicLoggerCommonHandlerHeaderWritten(&pHandler->commonHandler))) + { + sbgBasicLoggerHandlerWriteMagHeader(pHandler->commonHandler.pFILE); + pHandler->commonHandler.headerWritten = true; + } + + sbgBasicLoggerHandlerWriteMag(pMag, pOutputFile); + } + } + + if (sbgBasicLoggerCommonHandlerConsoleEnabled(&pHandler->commonHandler)) + { + sbgBasicLoggerCommonHandlerWriteLogInfo(&pHandler->commonHandler, stdout); + sbgBasicLoggerHandlerWriteMag(pMag, stdout); + } +} diff --git a/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h new file mode 100644 index 0000000..6b93367 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgBasicLogger/src/sbgBasicLoggerHandler.h @@ -0,0 +1,823 @@ +/*! + * \file sbgBasicLoggerHandler.h + * \author SBG Systems + * \date July 30, 2021 + * + * \brief Helper to handle logs. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * \endlicense + */ + +#ifndef SBG_BASIC_LOGGER_HANDLER_H +#define SBG_BASIC_LOGGER_HANDLER_H + +// sbgCommonLib headers +#include + +// sbgECom headers +#include + +// Local headers +#include "sbgBasicLoggerAccumulators.h" +#include "sbgBasicLoggerFile.h" + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Common log handler. + */ +typedef struct _SbgBasicLoggerCommonHandler +{ + bool consoleEnabled; /*!< True to print data on the console. */ + size_t consoleDecimation; /*!< Console decimation. */ + size_t fileDecimation; /*!< File decimation. */ + bool writingFile; /*!< True to save data into a file. */ + bool writeHeader; /*!< True to write header. */ + bool headerWritten; /*!< True if headers have been written. */ + const SbgBasicLoggerFileDesc *pDesc; /*!< File descriptor. */ + FILE *pFILE; /*!< File. */ + + const char *pPathStr; /*!< String path directory. */ + SbgEComClass class; /*!< Handler class. */ + SbgEComMsgId id; /*!< Handler message ID. */ +} SbgBasicLoggerCommonHandler; + +/*! + * IMU short log handler. + */ +typedef struct _SbgBasicLoggerImuHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ + + SbgBasicLoggerImuAcc consoleAcc; /*!< Decimated IMU data for the console. */ + SbgBasicLoggerImuAcc fileAcc; /*!< Decimated IMU data for the file. */ +} SbgBasicLoggerImuHandler; + + +/*! + * Diag log handler. + */ +typedef struct _SbgBasicLoggerDiagHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerDiagHandler; + +/*! + * UTC log handler. + */ +typedef struct _SbgBasicLoggerUtcHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerUtcHandler; + +/*! + * Status log handler. + */ +typedef struct _SbgBasicLoggerStatusHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerStatusHandler; + +/*! + * EKF Euler log handler. + */ +typedef struct _SbgBasicLoggerEkfEulerHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerEkfEulerHandler; + +/*! + * EKF quat log handler. + */ +typedef struct _SbgBasicLoggerEkfQuatHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerEkfQuatHandler; + +/*! + * EKF nav log handler. + */ +typedef struct _SbgBasicLoggerEkfNavHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerEkfNavHandler; + +/*! + * Ship motion log handler. + */ +typedef struct _SbgBasicLoggerShipMotionHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerShipMotionHandler; + +/*! + * Ship motion HP log handler. + */ +typedef struct _SbgBasicLoggerShipMotionHpHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerShipMotionHpHandler; + +/*! + * GPS vel log handler. + */ +typedef struct _SbgBasicLoggerGpsVelHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerGpsVelHandler; + +/*! + * GPS pos log handler. + */ +typedef struct _SbgBasicLoggerGpsPosHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerGpsPosHandler; + +/*! + * GPS hdt log handler. + */ +typedef struct _SbgBasicLoggerGpsHdtHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerGpsHdtHandler; + +/*! + * GPS RAW log handler. + */ +typedef struct _SbgBasicLoggerGpsRawHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerGpsRawHandler; + +/*! + * GPS Satellite in View log handler. + */ +typedef struct _SbgBasicLoggerGpsSatHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerGpsSatHandler; + +/*! + * Odometer log handler. + */ +typedef struct _SbgBasicLoggerOdometerHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerOdometerHandler; + +/*! + * DVL log handler. + */ +typedef struct _SbgBasicLoggerDvlHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerDvlHandler; + +/*! + * Air log handler. + */ +typedef struct _SbgBasicLoggerAirHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerAirHandler; + +/*! + * USBL log handler. + */ +typedef struct _SbgBasicLoggerUsblHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerUsblHandler; + +/*! + * Depth log handler. + */ +typedef struct _SbgBasicLoggerDepthHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerDepthHandler; + +/*! + * RTCM log handler. + */ +typedef struct _SbgBasicLoggerRawRtcmHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerRawRtcmHandler; + +/*! + * Event log handler. + */ +typedef struct _SbgBasicLoggerEventHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerEventHandler; + +/*! + * Magnetometer log handler. + */ +typedef struct _SbgBasicLoggerMagHandler +{ + SbgBasicLoggerCommonHandler commonHandler; /*!< Common with all handler logs. */ +} SbgBasicLoggerMagHandler; + +//----------------------------------------------------------------------// +//- Public functions -// +//----------------------------------------------------------------------// + +/*! + * IMU short handler constructor. + * + * \param[in] pHandler IMU short handler. + * \param[in] consoleEnabled True to print IMU short data on console. + * \param[in] consoleDecimation Console decimation. + * \param[in] writingFile True to write IMU short data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] fileDecimation File decimation. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerImuShortHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); + +/*! + * IMU data handler constructor. + * + * \param[in] pHandler IMU data handler. + * \param[in] consoleEnabled True to print IMU short data on console. + * \param[in] consoleDecimation Console decimation. + * \param[in] writingFile True to write IMU short data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] fileDecimation File decimation. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerImuHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); + +/*! + * IMU fast handler constructor. + * + * \param[in] pHandler IMU fast data handler. + * \param[in] consoleEnabled True to print IMU short data on console. + * \param[in] consoleDecimation Console decimation. + * \param[in] writingFile True to write IMU short data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] fileDecimation File decimation. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerImuFastHandlerConstruct(SbgBasicLoggerImuHandler *pHandler, bool consoleEnabled, size_t consoleDecimation, bool writingFile, const char *pPathStr, size_t fileDecimation, bool writeHeader); + +/*! + * Process an IMU short handler with a new data. + * + * \param[in] pHandler IMU short handler. + * \param[in] pNewData New IMU short data. + */ +void sbgBasicLoggerImuHandlerProcess(SbgBasicLoggerImuHandler *pHandler, const SbgBasicLoggerImu *pNewData); + + +/*! + * Diagnostic handler constructor. + * + * \param[in] pHandler Diag handler. + * \param[in] consoleEnabled True to print diag data on console. + * \param[in] writingFile True to write diag data into a file. + * \param[in] pPathStr Path to write file logs. + */ +void sbgBasicLoggerDiagHandlerConstruct(SbgBasicLoggerDiagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * Process a diagnostic data. + * + * \param[in] pHandler Diag log handler. + * \param[in] pLastUtcData Last UTC data, used of timestamp. + * \param[in] pDiag Diagnostic data. + */ +void sbgBasicLoggerDiagHandlerProcess(SbgBasicLoggerDiagHandler *pHandler, SbgLogUtcData *pLastUtcData, const SbgLogDiagData *pDiag); + +/*! + * UTC handler constructor. + * + * \param[in] pHandler UTC handler. + * \param[in] consoleEnabled True to print UTC data on console. + * \param[in] writingFile True to write UTC data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerUtcHandlerConstruct(SbgBasicLoggerUtcHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process an UTC data. + * + * \param[in] pHandler UTC log handler. + * \param[in] pLastUtcData Last UTC data, used of timestamp. + * \param[in] pDiag Diagnostic data. + */ +void sbgBasicLoggerUtcHandlerProcess(SbgBasicLoggerUtcHandler *pHandler, const SbgLogUtcData *pUtc); + +/*! + * Status handler constructor. + * + * \param[in] pHandler Status handler. + * \param[in] consoleEnabled True to print status data on console. + * \param[in] writingFile True to write status data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerStatusHandlerConstruct(SbgBasicLoggerStatusHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a status data. + * + * \param[in] pHandler Status log handler. + * \param[in] pDiag Status data. + */ +void sbgBasicLoggerStatusHandlerProcess(SbgBasicLoggerStatusHandler *pHandler, const SbgLogStatusData *pStatus); + +/*! + * EKF Euler handler constructor. + * + * \param[in] pHandler EKF Euler handler. + * \param[in] consoleEnabled True to print EKF Euler data on console. + * \param[in] writingFile True to write EKF Euler data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEkfEulerHandlerConstruct(SbgBasicLoggerEkfEulerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a EKF Euler data. + * + * \param[in] pHandler EKF Euler handler. + * \param[in] pEkfEuler EKF Euler data. + */ +void sbgBasicLoggerEkfEulerHandlerProcess(SbgBasicLoggerEkfEulerHandler *pHandler, const SbgLogEkfEulerData *pEkfEuler); + +/*! + * EKF quat handler constructor. + * + * \param[in] pHandler EKF quat handler. + * \param[in] consoleEnabled True to print EKF quat data on console. + * \param[in] writingFile True to write EKF quat data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEkfQuatHandlerConstruct(SbgBasicLoggerEkfQuatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a EKF quat data. + * + * \param[in] pHandler EKF quat handler. + * \param[in] pEkfQuat EKF quat data. + */ +void sbgBasicLoggerEkfQuatHandlerProcess(SbgBasicLoggerEkfQuatHandler *pHandler, const SbgLogEkfQuatData *pEkfEuler); + +/*! + * EKF nav handler constructor. + * + * \param[in] pHandler EKF nav handler. + * \param[in] consoleEnabled True to print EKF nav data on console. + * \param[in] writingFile True to write EKF nav data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEkfNavHandlerConstruct(SbgBasicLoggerEkfNavHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a EKF nav data. + * + * \param[in] pHandler EKF nav handler. + * \param[in] pEkfEuler EKF nav data. + */ +void sbgBasicLoggerEkfNavHandlerProcess(SbgBasicLoggerEkfNavHandler *pHandler, const SbgLogEkfNavData *pEkfEuler); + +/*! + * Ship motion handler constructor. + * + * \param[in] pHandler Ship motion handler. + * \param[in] consoleEnabled True to print ship motion data on console. + * \param[in] writingFile True to write ship motion data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerShipMotionHandlerConstruct(SbgBasicLoggerShipMotionHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a ship motion data. + * + * \param[in] pHandler SHip motion handler. + * \param[in] pShipMotion Ship motion data. + */ +void sbgBasicLoggerShipMotionHandlerProcess(SbgBasicLoggerShipMotionHandler *pHandler, const SbgLogShipMotionData *pShipMotion); + +/*! + * Ship motion HP handler constructor. + * + * \param[in] pHandler Ship motion HP handler. + * \param[in] consoleEnabled True to print ship motion HP data on console. + * \param[in] writingFile True to write ship motion HP data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerShipMotionHpHandlerConstruct(SbgBasicLoggerShipMotionHpHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a ship motion HP data. + * + * \param[in] pHandler Ship motion HP handler. + * \param[in] pShipMotion Ship motion HP data. + */ +void sbgBasicLoggerShipMotionHpHandlerProcess(SbgBasicLoggerShipMotionHpHandler *pHandler, const SbgLogShipMotionData *pShipMotion); + +/*! + * GPS 1 vel handler constructor. + * + * \param[in] pHandler GPS vel handler. + * \param[in] consoleEnabled True to print GPS vel data on console. + * \param[in] writingFile True to write GPS vel data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsVel1HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * GPS 2 vel handler constructor. + * + * \param[in] pHandler GPS vel handler. + * \param[in] consoleEnabled True to print GPS vel data on console. + * \param[in] writingFile True to write GPS vel data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsVel2HandlerConstruct(SbgBasicLoggerGpsVelHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a GPS vel data. + * + * \param[in] pHandler GPS vel handler. + * \param[in] pGps GPS vel data. + */ +void sbgBasicLoggerGpsVelHandlerProcess(SbgBasicLoggerGpsVelHandler *pHandler, const SbgLogGpsVel *pGps); + +/*! + * GPS handler constructor. + * + * \param[in] pHandler GPS handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsPos1HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * GPS 2 handler constructor. + * + * \param[in] pHandler GPS handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsPos2HandlerConstruct(SbgBasicLoggerGpsPosHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a GPS data. + * + * \param[in] pHandler GPS handler. + * \param[in] pGps GPS data. + */ +void sbgBasicLoggerGpsPosHandlerProcess(SbgBasicLoggerGpsPosHandler *pHandler, const SbgLogGpsPos *pGps); + +/*! + * GPS 1 hdt handler constructor. + * + * \param[in] pHandler GPS hdt handler. + * \param[in] consoleEnabled True to print GPS vel data on console. + * \param[in] writingFile True to write GPS vel data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsHdt1HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * GPS 2 hdt handler constructor. + * + * \param[in] pHandler GPS vel handler. + * \param[in] consoleEnabled True to print GPS hdt data on console. + * \param[in] writingFile True to write GPS hdt data into a file. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGpsHdt2HandlerConstruct(SbgBasicLoggerGpsHdtHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a GPS hdt data. + * + * \param[in] pHandler GPS vel handler. + * \param[in] pGps GPS vel data. + */ +void sbgBasicLoggerGpsHdtHandlerProcess(SbgBasicLoggerGpsHdtHandler *pHandler, const SbgLogGpsHdt *pGps); + +/*! + * GPS raw handler constructor. + * + * \param[in] pHandler GPS raw handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGps1RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * GPS raw handler constructor. + * + * \param[in] pHandler GPS raw handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGps2RawHandlerConstruct(SbgBasicLoggerGpsRawHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * Process a GPS raw data. + * + * \param[in] pHandler GPS raw handler. + * \param[in] pGps GPS raw data. + */ +void sbgBasicLoggerGpsRawHandlerProcess(SbgBasicLoggerGpsRawHandler *pHandler, const SbgLogRawData *pRawGps); + +/*! + * GPS 1 Satellite in View handler constructor. + * + * \param[in] pHandler GPS Sat handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGps1SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * GPS 2 Satellite in View handler constructor. + * + * \param[in] pHandler GPS Sat handler. + * \param[in] consoleEnabled True to print GPS data on console. + * \param[in] writingFile True to write GPS data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerGps2SatHandlerConstruct(SbgBasicLoggerGpsSatHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * Process a GPS Satellite in View data. + * + * \param[in] pHandler GPS Sat handler. + * \param[in] pData GPS Sat data. + */ +void sbgBasicLoggerGpsSatHandlerProcess(SbgBasicLoggerGpsSatHandler *pHandler, const SbgLogSatGroupData *pData); + +/*! + * Odometer handler constructor. + * + * \param[in] pHandler Odometer handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerOdometerHandlerConstruct(SbgBasicLoggerOdometerHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process an odometer data. + * + * \param[in] pHandler Odometer handler. + * \param[in] pData Odometer data. + */ +void sbgBasicLoggerOdometerHandlerProcess(SbgBasicLoggerOdometerHandler *pHandler, const SbgLogOdometerData *pData); + +/*! + * DVL bottom handler constructor. + * + * \param[in] pHandler DVL bottom handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerDvlBottomHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * DVL water handler constructor. + * + * \param[in] pHandler DVL water handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerDvlWaterHandlerConstruct(SbgBasicLoggerDvlHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a DVL data. + * + * \param[in] pHandler DVL handler. + * \param[in] pData DVL data. + */ +void sbgBasicLoggerDvlHandlerProcess(SbgBasicLoggerDvlHandler *pHandler, const SbgLogDvlData *pData); + +/*! + * Air handler constructor. + * + * \param[in] pHandler Air handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerAirHandlerConstruct(SbgBasicLoggerAirHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process an air data. + * + * \param[in] pHandler Air handler. + * \param[in] pData Air data. + */ +void sbgBasicLoggerAirHandlerProcess(SbgBasicLoggerAirHandler *pHandler, const SbgLogAirData *pData); + +/*! + * USBL handler constructor. + * + * \param[in] pHandler USBL handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerUsblHandlerConstruct(SbgBasicLoggerUsblHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a USBL data. + * + * \param[in] pHandler USBL handler. + * \param[in] pData USBL data. + */ +void sbgBasicLoggerUsblHandlerProcess(SbgBasicLoggerUsblHandler *pHandler, const SbgLogUsblData *pData); + +/*! + * Depth handler constructor. + * + * \param[in] pHandler Depth handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerDepthHandlerConstruct(SbgBasicLoggerDepthHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a depth data. + * + * \param[in] pHandler Depth handler. + * \param[in] pData Depth data. + */ +void sbgBasicLoggerDepthHandlerProcess(SbgBasicLoggerDepthHandler *pHandler, const SbgLogDepth *pData); + +/*! + * Raw RTCM handler constructor. + * + * \param[in] pHandler Raw RTCM handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerRawRtcmHandlerConstruct(SbgBasicLoggerRawRtcmHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr); + +/*! + * Process a raw RTCM data. + * + * \param[in] pHandler Raw RTCM handler. + * \param[in] pData Raw RTCM data. + */ +void sbgBasicLoggerRawRtcmHandlerProcess(SbgBasicLoggerRawRtcmHandler *pHandler, const SbgLogRawData *pData); + + +/*! + * Event in A handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventInAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event in B handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventInBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event in C handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventInCHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event in D handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventInDHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event in E handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventInEHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event out A handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventOutAHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Event out B handler constructor. + * + * \param[in] pHandler Event handler. + * \param[in] consoleEnabled True to print data on console. + * \param[in] writingFile True to write data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerEventOutBHandlerConstruct(SbgBasicLoggerEventHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process an event data. + * + * \param[in] pHandler Event handler. + * \param[in] pEvent Event data. + */ +void sbgBasicLoggerEventHandlerProcess(SbgBasicLoggerEventHandler *pHandler, const SbgLogEvent *pEvent); + +/*! + * Mag handler constructor. + * + * \param[in] pHandler Mag handler. + * \param[in] consoleEnabled True to print mag data on console. + * \param[in] writingFile True to write mag data into a file. + * \param[in] pPathStr Path to write file logs. + * \param[in] writeHeader True to print and write header. + */ +void sbgBasicLoggerMagHandlerConstruct(SbgBasicLoggerMagHandler *pHandler, bool consoleEnabled, bool writingFile, const char *pPathStr, bool writeHeader); + +/*! + * Process a mag data. + * + * \param[in] pHandler Mag handler. + * \param[in] pMag Mag data. + */ +void sbgBasicLoggerMagHandlerProcess(SbgBasicLoggerMagHandler *pHandler, const SbgLogMag *pMag); + +#endif // SBG_BASIC_LOGGER_HANDLER_H diff --git a/crates/sbg-rs/sbgECom/tools/sbgEComApi/README.md b/crates/sbg-rs/sbgECom/tools/sbgEComApi/README.md new file mode 100644 index 0000000..cccc961 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgEComApi/README.md @@ -0,0 +1,80 @@ +# sbgEComApi + +The sbgEComApi command line tool let you read/update your product configuration through the JSON REST API. +A typical REST API works using GET / POST requests other HTTP protocols. + +The sbgEComApi tool encapsulate GET/POST requests so it can be used over a serial interface. +This tool is perfect to create complex and automated configuration scripts with no code. + +# Usage + +The sbgEComApi implements a simple to use command line interface (CLI) with two main operations. + +## Read Configuration (GET) + +You can read the whole product configuration or a specific configuration node. +The following example, reads the Port A configuration. +It uses the COM5 at 921600 bps to communicate with the device: + +```sh +sbgEComApi.exe -s COM5 -r 921600 /api/v1/settings/ports/comA -g +``` + +You should get the following response: +```js +{"mode":"rs232", "parity":"none", "baudrate":921600} +``` + +## Update Configuration (POST) + +You can update the whole product configuration at once or each setting one by one. +In the following example, we just update the Port A baudrate to 230400 bps. +The tool uses the computer COM5 at 921600 bps to communicate with the device: + +```sh +sbgEComApi.exe -s COM5 -r 921600 /api/v1/settings/ports/comA/baudrate -p -b 230400 +``` + +You should get the following response: +```js +{"title":"request successful","needReboot":false} +``` + +# Options +You can access the tool help using the --help argument. + +``` +Usage: sbgEComApi [-gpS] [--help] [--version] -s SERIAL_DEVICE -r SERIAL_BAUDRATE [-q QUERY] [-b BODY] [-B BODY_FILE] [-o OUTPUT_FILE] PATH +Access a RESTful SBG ECom server. + + --help display this help and exit + --version display version info and exit + -s, --serial-device=SERIAL_DEVICE open a serial interface + -r, --serial-baudrate=SERIAL_BAUDRATE serial baudrate + -g, --method-get use the GET method (default) + -p, --method-post use the POST method + -q, --query=QUERY query string + -b, --body=BODY body (POST method only) + -B, --body-file=BODY_FILE file containing the body (POST method only) + -S, --print-status print the status code on the output stream + -o, --output-file=OUTPUT_FILE output file + PATH path + +BODY or BODY_FILE may only be provided when using the POST method. + +If provided, BODY_FILE may not contain binary data. + +PATH is a URI path component. + +Exit codes : + 0: 200 OK + 60: 400 BAD REQUEST + 61: 401 UNAUTHORIZED + 63: 403 FORBIDDEN + 64: 404 NOT FOUND + 69: 409 CONFLICT + 82: 422 UNPROCESSABLE ENTITY + 100: 200 INTERNAL SERVER ERROR +If an error occurs and is unrelated to the status code, or if the status code is unknown, +return EXIT_FAILURE (1). +``` \ No newline at end of file diff --git a/crates/sbg-rs/sbgECom/tools/sbgEComApi/src/main.c b/crates/sbg-rs/sbgECom/tools/sbgEComApi/src/main.c new file mode 100644 index 0000000..d6ff9d6 --- /dev/null +++ b/crates/sbg-rs/sbgECom/tools/sbgEComApi/src/main.c @@ -0,0 +1,716 @@ +/*! + * \file main.c + * \author SBG Systems + * \date October 27, 2020 + * + * \brief Tool to perform REST GET/POST queries through sbgECom commands. + * + * \copyright Copyright (C) 2022, SBG Systems SAS. All rights reserved. + * \beginlicense Proprietary license + * + * This source code is intended for use only by SBG Systems SAS and + * those that have explicit written permission to use it from + * SBG Systems SAS. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + * KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + * PARTICULAR PURPOSE. + * + * \endlicense + */ + +// sbgCommonLib headers +#include +#include +#include +#include + +// sbgECom headers +#include +#include +#include + +// Argtable3 headers +#include + +//----------------------------------------------------------------------// +//- Constant definitions -// +//----------------------------------------------------------------------// + +/*! + * Program name. + */ +#define PROGRAM_NAME "sbgEComApi" + +/*! + * Default number of attempts per command execution. + */ +#define DEFAULT_CMD_NR_ATTEMPTS (3) + +/*! + * Default time-out per command execution attempt, in seconds. + */ +#define DEFAULT_CMD_TIMEOUT (5) + +//----------------------------------------------------------------------// +//- Structure definitions -// +//----------------------------------------------------------------------// + +/*! + * Exit code descriptor. + */ +typedef struct _ExitCodeDesc +{ + int exitCode; /*!< Exit code, on 7 bits. */ + uint16_t statusCode; /*!< Status code. */ + const char *pMessage; /*!< Message. */ +} ExitCodeDesc; + +//----------------------------------------------------------------------// +//- Private variables -// +//----------------------------------------------------------------------// + +/*! + * Table of exit code descriptors. + */ +static const ExitCodeDesc gExitCodeDescs[] = +{ + { .exitCode = 0, .statusCode = 200, .pMessage = "OK" }, + { .exitCode = 60, .statusCode = 400, .pMessage = "BAD REQUEST" }, + { .exitCode = 61, .statusCode = 401, .pMessage = "UNAUTHORIZED" }, + { .exitCode = 63, .statusCode = 403, .pMessage = "FORBIDDEN" }, + { .exitCode = 64, .statusCode = 404, .pMessage = "NOT FOUND" }, + { .exitCode = 69, .statusCode = 409, .pMessage = "CONFLICT" }, + { .exitCode = 82, .statusCode = 422, .pMessage = "UNPROCESSABLE ENTITY" }, + { .exitCode = 100, .statusCode = 200, .pMessage = "INTERNAL SERVER ERROR" }, +}; + +//----------------------------------------------------------------------// +//- Private functions -// +//----------------------------------------------------------------------// + +/*! + * Look up the exit code descriptor matching the given status code. + * + * \param[in] statusCode Status code. + * \return Exit code descriptor. + */ +static const ExitCodeDesc *lookupExitCodeDesc(uint16_t statusCode) +{ + const ExitCodeDesc *pDesc = NULL; + + for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) + { + const ExitCodeDesc *pTmpDesc; + + pTmpDesc = &gExitCodeDescs[i]; + + if (pTmpDesc->statusCode == statusCode) + { + pDesc = pTmpDesc; + break; + } + } + + return pDesc; +} + +/*! + * Convert a status code into an exit code. + * + * \param[in] statusCode Status code. + * \return Exit code. + */ +static int convertStatusCodeToExitCode(uint16_t statusCode) +{ + const ExitCodeDesc *pDesc; + int exitCode; + + pDesc = lookupExitCodeDesc(statusCode); + + if (pDesc) + { + exitCode = pDesc->exitCode; + } + else + { + exitCode = EXIT_FAILURE; + } + + return exitCode; +} + +/*! + * Print help about a single exit code / status code mapping. + * + * \param[in] pDesc Exit code descriptor. + */ +static void print_exit_code_mapping(const ExitCodeDesc *pDesc) +{ + printf(" %3u: %3u %s\n", pDesc->exitCode, pDesc->statusCode, pDesc->pMessage); +} + +/*! + * Print help about exit codes. + */ +static void print_exit_code_help(void) +{ + printf("Exit codes :\n"); + + for (size_t i = 0; i < SBG_ARRAY_SIZE(gExitCodeDescs); i++) + { + print_exit_code_mapping(&gExitCodeDescs[i]); + } + + printf("If an error occurs and is unrelated to the status code, or if the status code is unknown,\n"); + printf("return EXIT_FAILURE.\n"); +} + +/*! + * Callback definition used to route log error messages. + * + * \param[in] pFileName The file in which the log was triggered. + * \param[in] pFunctionName The function where the log was triggered. + * \param[in] line The line in the file where the log was triggered. + * \param[in] pCategory Category for this log or "None" if no category has been specified. + * \param[in] logType Associated log message level. + * \param[in] errorCode Associated error code or SBG_NO_ERROR for INFO & DEBUG level logs. + * \param[in] pMessage The message to log. + */ +static void onLogCallback(const char *pFileName, const char *pFunctionName, uint32_t line, const char *pCategory, SbgDebugLogType type, SbgErrorCode errorCode, const char *pMessage) +{ + const char *pTypeStr; + const char *pBaseName; + + assert(pFileName); + assert(pFunctionName); + assert(pCategory); + assert(pMessage); + + pTypeStr = sbgDebugLogTypeToStr(type); + pBaseName = strrchr(pFileName, '/'); + + if (!pBaseName) + { + pBaseName = pFileName; + } + else + { + // + // Skip the slash. + // + pBaseName++; + } + + if (errorCode == SBG_NO_ERROR) + { + fprintf(stderr, "%-7s %s (%s:%" PRIu32 ") %s\n", pTypeStr, pFunctionName, pBaseName, line, pMessage); + } + else + { + fprintf(stderr, "%-7s err:%s %s (%s:%" PRIu32 ") %s\n", pTypeStr, sbgErrorCodeToString(errorCode), pFunctionName, pBaseName, line, pMessage); + } +} + +/*! + * Read a string from a file. + * + * \param[out] pString String. + * \param[in] pPath File path. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode readStringFromFile(SbgString *pString, const char *pPath) +{ + SbgErrorCode errorCode; + FILE *pFile; + + assert(pString); + assert(pPath); + + pFile = fopen(pPath, "r"); + + if (pFile) + { + for (;;) + { + size_t size; + char buffer[4096]; + + size = fread(buffer, 1, sizeof(buffer) - 1, pFile); + + if (size != 0) + { + assert(size < sizeof(buffer)); + buffer[size] = '\0'; + + errorCode = sbgStringAppendCString(pString, buffer); + + if (errorCode == SBG_NO_ERROR) + { + char *pPtr; + + pPtr = memchr(buffer, '\0', size); + + if (pPtr) + { + break; + } + } + else + { + break; + } + } + else + { + if (ferror(pFile)) + { + errorCode = SBG_READ_ERROR; + SBG_LOG_ERROR(errorCode, "unable to read file %s", pPath); + } + else + { + errorCode = SBG_NO_ERROR; + } + + break; + } + } + + fclose(pFile); + } + else + { + errorCode = SBG_INVALID_PARAMETER; + SBG_LOG_ERROR(errorCode, "unable to open file %s", pPath); + } + + return errorCode; +} + +/*! + * Write a reply to an output file. + * + * \param[in] pReply Reply. + * \param[in] writeStatus If true, print the status to the output file. + * \param[out] pOutputFile Output file. + */ +static SbgErrorCode writeReply(const SbgEComCmdApiReply *pReply, bool writeStatus, FILE *pOutputFile) +{ + SbgErrorCode errorCode; + + if (writeStatus) + { + int nrChars; + + nrChars = fprintf(pOutputFile, "%"PRIu16"\n", pReply->statusCode); + + if (nrChars >= 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_WRITE_ERROR; + SBG_LOG_ERROR(errorCode, "unable to write status code"); + } + } + else + { + errorCode = SBG_NO_ERROR; + } + + if (errorCode == SBG_NO_ERROR) + { + int nrChars; + + nrChars = fprintf(pOutputFile, "%s", pReply->pContent); + + if (nrChars >= 0) + { + errorCode = SBG_NO_ERROR; + } + else + { + errorCode = SBG_WRITE_ERROR; + SBG_LOG_ERROR(errorCode, "unable to write content"); + } + } + + return errorCode; +} + +/*! + * Execute a GET or POST ECom command. + * + * \param[in] pInterface Interface. + * \param[in] methodIsGet The GET command is executed if true, the POST command otherwise. + * \param[in] pPath URI path component. + * \param[in] nrAttempts Number of attempts. + * \param[in] timeout Time-out per attempt, in seconds. + * \param[in] pQuery Query string, may be NULL. + * \param[in] pBody Body, may be NULL. + * \param[in] writeStatus If true, print the status to the output file. + * \param[out] pStatusCode Status code. + * \param[in] pOutputFile Output file. + * \return SBG_NO_ERROR if successful. + */ +static SbgErrorCode execute(SbgInterface *pInterface, bool methodIsGet, const char *pPath, uint32_t nrAttempts, uint32_t timeout, + const char *pQuery, const char *pBody, bool writeStatus, uint16_t *pStatusCode, FILE *pOutputFile) +{ + SbgErrorCode errorCode; + SbgEComHandle ecomHandle; + + assert(pPath); + assert(pOutputFile); + + errorCode = sbgEComInit(&ecomHandle, pInterface); + + if (errorCode == SBG_NO_ERROR) + { + SbgEComCmdApiReply reply; + + sbgEComSetCmdTrialsAndTimeOut(&ecomHandle, nrAttempts, timeout * 1000); + + sbgEComCmdApiReplyConstruct(&reply); + + if (methodIsGet) + { + errorCode = sbgEComCmdApiGet(&ecomHandle, pPath, pQuery, &reply); + } + else + { + errorCode = sbgEComCmdApiPost(&ecomHandle, pPath, pQuery, pBody, &reply); + } + + if (errorCode == SBG_NO_ERROR) + { + errorCode = writeReply(&reply, writeStatus, pOutputFile); + + if (errorCode == SBG_NO_ERROR) + { + *pStatusCode = reply.statusCode; + } + } + else + { + SBG_LOG_ERROR(errorCode, "unable to execute command"); + } + + sbgEComCmdApiReplyDestroy(&reply); + + if (errorCode == SBG_NO_ERROR) + { + errorCode = sbgEComClose(&ecomHandle); + } + else + { + sbgEComClose(&ecomHandle); + } + } + + return errorCode; +} + +//----------------------------------------------------------------------// +// Public functions // +//----------------------------------------------------------------------// + +/*! + * Program entry point. + * + * \param[in] argc Number of input arguments. + * \param[in] argv Input arguments as an array of strings. + * \return EXIT_SUCCESS if successful. + */ +int main(int argc, char **argv) +{ + int exitCode = EXIT_SUCCESS; + bool printHelp = false; + + struct arg_lit *pHelpArg; + struct arg_lit *pVersionArg; + struct arg_str *pSerialDeviceArg; + struct arg_int *pSerialBaudrateArg; + struct arg_int *pNrAttemptsArg; + struct arg_int *pTimeoutArg; + struct arg_lit *pGetMethodArg; + struct arg_lit *pPostMethodArg; + struct arg_str *pPathArg; + struct arg_str *pQueryArg; + struct arg_str *pBodyArg; + struct arg_file *pBodyFileArg; + struct arg_lit *pPrintStatus; + struct arg_file *pOutputFileArg; + struct arg_end *pEndArg; + + // + // TODO: add support for network interfaces. + // + void *argTable[] = + { + pHelpArg = arg_lit0( NULL, "help", "display this help and exit"), + pVersionArg = arg_lit0( NULL, "version", "display version info and exit"), + pSerialDeviceArg = arg_str1( "s", "serial-device", "SERIAL_DEVICE", "open a serial interface"), + pSerialBaudrateArg = arg_int1( "r", "serial-baudrate", "SERIAL_BAUDRATE", "serial baudrate"), + pNrAttemptsArg = arg_int0( "n", "nr-attempts", "NR_ATTEMPTS", "number of transaction attempts"), + pTimeoutArg = arg_int0( "t", "timeout", "TIMEOUT", "reply time-out, in seconds"), + pGetMethodArg = arg_lit0( "g", "method-get", "use the GET method (default)"), + pPostMethodArg = arg_lit0( "p", "method-post", "use the POST method"), + pQueryArg = arg_str0( "q", "query", "QUERY", "query string, format=pretty&delta=true, format and delta options are optionnal"), + pBodyArg = arg_str0( "b", "body", "BODY", "body (POST method only)"), + pBodyFileArg = arg_file0( "B", "body-file", "BODY_FILE", "file containing the body (POST method only)"), + pPrintStatus = arg_lit0( "S", "print-status", "print the status code on the output stream"), + pOutputFileArg = arg_file0( "o", "output-file", "OUTPUT_FILE", "output file"), + pPathArg = arg_str1( NULL, NULL, "PATH", "path"), + + pEndArg = arg_end(20), + }; + + sbgCommonLibSetLogCallback(onLogCallback); + + if (arg_nullcheck(argTable) == 0) + { + int argError; + + argError = arg_parse(argc, argv, argTable); + + if (pHelpArg->count != 0) + { + printf("Usage: %s", PROGRAM_NAME); + arg_print_syntax(stdout, argTable, "\n"); + printf("Access a RESTful SBG ECom server.\n\n"); + arg_print_glossary(stdout, argTable, " %-25s %s\n"); + + puts(""); + printf("BODY or BODY_FILE may only be provided when using the POST method.\n"); + + puts(""); + printf("If provided, BODY_FILE may not contain binary data.\n"); + + puts(""); + printf("PATH is a URI path component.\n"); + + puts(""); + print_exit_code_help(); + } + else if (pVersionArg->count != 0) + { + printf("%s\n", sbgEComGetVersionAsString()); + } + else if (argError == 0) + { + SbgInterface ecomInterface; + bool methodIsGet = true; + int nrAttempts = DEFAULT_CMD_NR_ATTEMPTS; + int timeout = DEFAULT_CMD_TIMEOUT; + bool writeStatus = false; + SbgString bodyStorage; + SbgString *pBody = NULL; + const char *pQuery = NULL; + const char *pPath = NULL; + + if (exitCode == EXIT_SUCCESS) + { + sbgStringConstructEmpty(&bodyStorage); + pBody = &bodyStorage; + } + + if (exitCode == EXIT_SUCCESS) + { + if ((pSerialDeviceArg->count != 0) && (pSerialBaudrateArg->count != 0)) + { + SbgErrorCode errorCode; + + errorCode = sbgInterfaceSerialCreate(&ecomInterface, pSerialDeviceArg->sval[0], pSerialBaudrateArg->ival[0]); + + if (errorCode == SBG_NO_ERROR) + { + FILE *pOutputFile = NULL; + + if ((pGetMethodArg->count != 0) && (pPostMethodArg->count == 0)) + { + methodIsGet = true; + } + else if ((pGetMethodArg->count == 0) && (pPostMethodArg->count != 0)) + { + methodIsGet = false; + } + else if ((pGetMethodArg->count != 0) && (pPostMethodArg->count != 0)) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + + if (exitCode == EXIT_SUCCESS) + { + if (pNrAttemptsArg->count != 0) + { + nrAttempts = pNrAttemptsArg->ival[0]; + + if (nrAttempts <= 0) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pTimeoutArg->count != 0) + { + timeout = pTimeoutArg->ival[0]; + + if (timeout <= 0) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pQueryArg->count != 0) + { + pQuery = pQueryArg->sval[0]; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (!methodIsGet && (pBodyArg->count != 0) && (pBodyFileArg->count == 0)) + { + errorCode = sbgStringAssignCString(pBody, pBodyArg->sval[0]); + + if (errorCode != SBG_NO_ERROR) + { + exitCode = EXIT_FAILURE; + } + } + else if (!methodIsGet && (pBodyArg->count == 0) && (pBodyFileArg->count != 0)) + { + errorCode = readStringFromFile(&bodyStorage, pBodyFileArg->filename[0]); + + if (errorCode != SBG_NO_ERROR) + { + exitCode = EXIT_FAILURE; + } + } + else if ((pBodyArg->count != 0) && (pBodyFileArg->count != 0)) + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (pPrintStatus->count != 0) + { + writeStatus = true; + } + } + + if (exitCode == EXIT_SUCCESS) + { + if (pOutputFileArg->count == 0) + { + pOutputFile = stdout; + } + else + { + pOutputFile = fopen(pOutputFileArg->filename[0], "w"); + + if (!pOutputFile) + { + SBG_LOG_ERROR(SBG_ERROR, "unable to open %s", pOutputFileArg->filename[0]); + exitCode = EXIT_FAILURE; + } + } + } + + if (exitCode == EXIT_SUCCESS) + { + assert(pPathArg->count != 0); + + pPath = pPathArg->sval[0]; + } + + if (exitCode == EXIT_SUCCESS) + { + const char *pBodyCString; + uint16_t statusCode; + + if (pBody) + { + pBodyCString = sbgStringGetCString(pBody); + } + else + { + pBodyCString = NULL; + } + + errorCode = execute(&ecomInterface, methodIsGet, pPath, nrAttempts, timeout, pQuery, pBodyCString, writeStatus, &statusCode, pOutputFile); + + if (errorCode == SBG_NO_ERROR) + { + exitCode = convertStatusCodeToExitCode(statusCode); + } + else + { + exitCode = EXIT_FAILURE; + } + } + + if (pOutputFile && (pOutputFile != stdout)) + { + int result; + + result = fclose(pOutputFile); + + if (result != 0) + { + SBG_LOG_ERROR(SBG_WRITE_ERROR, "unable to close %s", pOutputFileArg->filename[0]); + exitCode = EXIT_FAILURE; + } + } + + sbgInterfaceDestroy(&ecomInterface); + } + else + { + SBG_LOG_ERROR(errorCode, "unable to open serial interface"); + exitCode = EXIT_FAILURE; + } + } + else + { + exitCode = EXIT_FAILURE; + printHelp = true; + } + } + + if (pBody) + { + sbgStringDestroy(pBody); + } + } + else + { + printHelp = true; + } + + if (printHelp) + { + arg_print_errors(stderr, pEndArg, PROGRAM_NAME); + fprintf(stderr, "Try '%s --help' for more information.\n", PROGRAM_NAME); + exitCode = EXIT_FAILURE; + } + + arg_freetable(argTable, SBG_ARRAY_SIZE(argTable)); + } + else + { + SBG_LOG_ERROR(SBG_MALLOC_FAILED, "unable to allocate memory"); + exitCode = EXIT_FAILURE; + } + + return exitCode; +} diff --git a/crates/sbg-rs/src/data_conversion.rs b/crates/sbg-rs/src/data_conversion.rs new file mode 100644 index 0000000..ccc4a7d --- /dev/null +++ b/crates/sbg-rs/src/data_conversion.rs @@ -0,0 +1,147 @@ +use crate::bindings::{ + SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsPos, + SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, +}; +use bitflags::Flags; +use messages::sensor::*; +use messages::sensor_status::{ + AirFlags, AirStatus, EkfFlags, EkfStatus, GpsPositionStatus, GpsPositionStatusE, GpsVelStatus, + GpsVelStatusE, ImuFlags, ImuStatus, UtcStatus, UtcTimeStatus, +}; + +/// Simple helper function to work with the flags structure and set the fields as needed. +#[inline] +fn check(flags: &Option, test: F, value: T) -> Option +where + F: Flags, +{ + match flags { + Some(x) if x.contains(test) => Some(value), + _ => None, + } +} + +impl From for GpsPos { + fn from(value: SbgLogGpsPos) -> Self { + let status = GpsPositionStatus::new(value.status); + let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); + + Self { + latitude: valid.then_some(value.latitude), + longitude: valid.then_some(value.longitude), + time_of_week: valid.then_some(value.timeOfWeek), + undulation: valid.then_some(value.undulation), + altitude: valid.then_some(value.altitude), + time_stamp: value.timeStamp, + status, + latitude_accuracy: valid.then_some(value.latitudeAccuracy), + longitude_accuracy: valid.then_some(value.longitudeAccuracy), + altitude_accuracy: valid.then_some(value.altitudeAccuracy), + num_sv_used: valid.then_some(value.numSvUsed), + base_station_id: valid.then_some(value.baseStationId), + differential_age: valid.then_some(value.differentialAge), + } + } +} + +impl From for UtcTime { + fn from(value: SbgLogUtcData) -> Self { + let status = UtcTimeStatus::new(value.status); + let valid = matches!(status.get_utc_status(), Some(UtcStatus::Valid | UtcStatus::NoLeapSec)); + + Self { + time_stamp: value.timeStamp, // not convinced this is matched valid to the Utc Status bitmask. + status, + year: valid.then_some(value.year), + month: valid.then_some(value.month), + day: valid.then_some(value.day), + hour: valid.then_some(value.hour), + minute: valid.then_some(value.minute), + second: valid.then_some(value.second), + nano_second: valid.then_some(value.nanoSecond), + gps_time_of_week: valid.then_some(value.gpsTimeOfWeek), + } + } +} + +impl From for Air { + fn from(value: SbgLogAirData) -> Self { + let status = AirStatus::new(value.status); + let flags = status.get_flags(); + + Self { + time_stamp: value.timeStamp, // TODO: check if valid. + status, + pressure_abs: check(&flags, AirFlags::PressureAbsValid, value.pressureAbs), + altitude: check(&flags, AirFlags::AltitudeValid, value.altitude), + pressure_diff: check(&flags, AirFlags::PressureDiffValid, value.pressureDiff), + true_airspeed: check(&flags, AirFlags::AirpseedValid, value.trueAirspeed), + air_temperature: check(&flags, AirFlags::TemperatureValid, value.airTemperature), + } + } +} + +impl From for EkfQuat { + fn from(value: SbgLogEkfQuatData) -> Self { + let status = EkfStatus::new(value.status); + let flags = status.get_flags(); + + Self { + time_stamp: value.timeStamp, + quaternion: check(&flags, EkfFlags::HeadingValid, value.quaternion), + euler_std_dev: check(&flags, EkfFlags::HeadingValid, value.eulerStdDev), + status, + } + } +} + +impl From for EkfNav { + fn from(value: SbgLogEkfNavData) -> Self { + let status = EkfStatus::new(value.status); + let flags = status.get_flags(); + + Self { + status, + velocity_std_dev: check(&flags, EkfFlags::VelocityValid, value.velocityStdDev), + position_std_dev: check(&flags, EkfFlags::PositionValid, value.positionStdDev), + time_stamp: value.timeStamp, + velocity: check(&flags, EkfFlags::VelocityValid, value.velocity), + position: check(&flags, EkfFlags::PositionValid, value.position), + undulation: check(&flags, EkfFlags::AttitudeValid, value.undulation), + } + } +} + +impl From for Imu { + fn from(value: SbgLogImuData) -> Self { + let status = ImuStatus::new(value.status); + let flags = status.get_flags(); + + Self { + time_stamp: value.timeStamp, + status, + accelerometers: check(&flags, ImuFlags::AccelsInRange, value.accelerometers), + gyroscopes: check(&flags, ImuFlags::GyrosInRange, value.gyroscopes), + temperature: Some(value.temperature), // we cannot check since no flag exists. Keep in option for uniformity. + delta_velocity: check(&flags, ImuFlags::AccelsInRange, value.deltaVelocity), + delta_angle: check(&flags, ImuFlags::GyrosInRange, value.deltaAngle), + } + } +} + +impl From for GpsVel { + fn from(value: SbgLogGpsVel) -> Self { + let status = GpsVelStatus::new(value.status); + let valid = matches!(status.get_status(), Some(GpsVelStatusE::SolComputed)); + + Self { + time_of_week: valid.then_some(value.timeOfWeek), + time_stamp: value.timeStamp, + status, + velocity: valid.then_some(value.velocity), + course: valid.then_some(value.course), + course_acc: valid.then_some(value.courseAcc), + velocity_acc: valid.then_some(value.velocityAcc), + } + } +} diff --git a/crates/sbg-rs/src/lib.rs b/crates/sbg-rs/src/lib.rs new file mode 100644 index 0000000..068e185 --- /dev/null +++ b/crates/sbg-rs/src/lib.rs @@ -0,0 +1,17 @@ +#![no_std] + +// Disable specific format checks as C bindings may not conform. +#![allow(dead_code)] +#![allow(non_snake_case)] +#![allow(non_camel_case_types)] +#![allow(non_upper_case_globals)] + +#[allow(unused_imports)] // Auto-generated bindings import unused code. +mod bindings; +mod data_conversion; +#[allow(static_mut_refs)] // Supress warnings as these are safe in our context. +mod sbg; + +// Expose the Rust API wrapper with `pub use` not `pub mod` to hide the +// implementation details and simply expose the API as part of sbg-rs. +pub use sbg::*; diff --git a/crates/sbg-rs/src/sbg.rs b/crates/sbg-rs/src/sbg.rs new file mode 100644 index 0000000..6d4026d --- /dev/null +++ b/crates/sbg-rs/src/sbg.rs @@ -0,0 +1,501 @@ +use crate::bindings::{ + self, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR, + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO, _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING, + _SbgEComLog_SBG_ECOM_LOG_AIR_DATA, _SbgEComLog_SBG_ECOM_LOG_EKF_NAV, _SbgEComLog_SBG_ECOM_LOG_GPS1_POS, + _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL, _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT, _SbgEComLog_SBG_ECOM_LOG_UTC_TIME, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, _SbgErrorCode_SBG_NO_ERROR, _SbgErrorCode_SBG_NULL_POINTER, + _SbgErrorCode_SBG_WRITE_ERROR, sbgEComCmdOutputSetConf, sbgEComHandle, + _SbgBinaryLogData, _SbgDebugLogType, _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, _SbgEComHandle, + _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT, _SbgEComLog_SBG_ECOM_LOG_IMU_DATA, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, _SbgEComProtocol, _SbgErrorCode, _SbgInterface, +}; +use core::ffi::c_void; +use core::ptr::null_mut; +use core::slice::{from_raw_parts, from_raw_parts_mut}; +use core::sync::atomic::AtomicUsize; +use defmt::{debug, error, flush, info, warn}; +use heapless::{Deque, Vec}; +use core::ffi::CStr; +use messages::sensor::*; + +/** + * Max buffer size for SBG messages. + */ +pub const SBG_BUFFER_SIZE: usize = 1024; + +/** + * Represents the index of the buffer that is currently being used. + */ +static mut BUF_INDEX: AtomicUsize = AtomicUsize::new(0); +/** + * Points to the buffer that is currently being used. + */ +static mut BUF: &[u8; SBG_BUFFER_SIZE] = &[0; SBG_BUFFER_SIZE]; + +static mut DEQ: Deque = Deque::new(); + +static mut DATA_CALLBACK: Option = None; + +static mut SERIAL_WRITE_CALLBACK: Option)> = None; + +static mut RTC_GET_TIME: Option u32> = None; + +static mut SERIAL_FLUSH_CALLBACK: Option = None; + +pub enum CallbackData { + UtcTime(UtcTime), + Air(Air), + EkfQuat(EkfQuat), + EkfNav(EkfNav), + Imu(Imu), + GpsVel(GpsVel), + GpsPos(GpsPos), +} + +struct UARTSBGInterface { + interface: *mut bindings::SbgInterface, +} + +pub struct SBG { + uart_sbg_interface: UARTSBGInterface, + handle: _SbgEComHandle, + is_initialized: bool, +} + +impl SBG { + /** + * Creates a new SBG instance. + * Takes ownership of the serial device and RTC instance. + */ + pub fn new( + callback: fn(CallbackData), + serial_write_callback: fn(Vec), + rtc_get_time: fn() -> u32, + serial_flush_callback: fn(), + ) -> Self { + unsafe { + DATA_CALLBACK = Some(callback); + SERIAL_WRITE_CALLBACK = Some(serial_write_callback); + RTC_GET_TIME = Some(rtc_get_time); + SERIAL_FLUSH_CALLBACK = Some(serial_flush_callback); + } + // SAFETY: We are assigning the RTC instance to a static variable. + // This is safe because we are the only ones who have access to it. + let uart_sbg_interface = UARTSBGInterface { + interface: &mut _SbgInterface { + handle: null_mut(), // SAFETY: No idea what I just did. + type_: 0, + name: [0; 48], + pDestroyFunc: Some(SBG::sbg_destroy_func), + pWriteFunc: Some(SBG::sbg_interface_write_func), + pReadFunc: Some(SBG::sbg_interface_read_func), + pFlushFunc: Some(SBG::sbg_flush_func), + pSetSpeedFunc: Some(SBG::sbg_set_speed_func), + pGetSpeedFunc: Some(SBG::sbg_get_speed_func), + pDelayFunc: Some(SBG::sbg_delay_func), + }, + }; + let p_large_buffer: *mut u8 = null_mut(); + let protocol: _SbgEComProtocol = _SbgEComProtocol { + pLinkedInterface: uart_sbg_interface.interface, + rxBuffer: [0; 4096usize], + rxBufferSize: 0, + discardSize: 0, + nextLargeTxId: 0, + pLargeBuffer: p_large_buffer, + largeBufferSize: 0, + msgClass: 0, + msgId: 0, + transferId: 0, + pageIndex: 0, + nrPages: 0, + }; + let handle: _SbgEComHandle = _SbgEComHandle { + protocolHandle: protocol, + pReceiveLogCallback: Some(SBG::sbg_ecom_receive_log_func), + pUserArg: null_mut(), + numTrials: 3, + cmdDefaultTimeOut: 500, + }; + + let is_initialized = false; + + SBG { + uart_sbg_interface, + handle, + is_initialized, + } + } + + /** + * Returns true if the SBG is initialized. + */ + pub fn is_initialized(&self) -> bool { + self.is_initialized + } + + /** + * Reads SBG data frames for a buffer and returns the most recent data. + */ + pub fn read_data(&mut self, buffer: &[u8; SBG_BUFFER_SIZE]) { + // SAFETY: We are assigning a static mut variable. + // Buf can only be accessed from functions called by sbgEComHandle after this assignment. + // unsafe { BUF = buffer }; + for i in buffer { + unsafe { + match DEQ.push_back(*i) { + Ok(_) => (), + Err(_) => warn!("Deque SBG Error"), + } + }; + } + // SAFETY: We are assigning a static variable. + // This is safe because are the only thread reading since SBG is locked. + unsafe { + *BUF_INDEX.get_mut() = 0; + } + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + unsafe { + sbgEComHandle(&mut self.handle); + } + } + + /** + * Configures the SBG to output the following data + * Air data + * IMU data + * Extended Kalman Filter Euler data + * Extended Kalman Filter Quaternions + * Extended Kalman Filter Navigation data + */ + pub fn setup(&mut self) -> u32 { + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure UTC Time logs to 40 cycles"); + } + + let error_code: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure UTC Time logs to 40 cycles"); + } + + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_UTC_TIME as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure UTC Time logs to 40 cycles"); + } + + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code: _SbgErrorCode = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_AIR_DATA as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure Air Data logs to 40 cycles"); + } + + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure EKF Quat logs to 40 cycles"); + } + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_EKF_NAV as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure EKF Nav logs to 40 cycles"); + } + // SAFETY: We are calling a C function. + // This is safe because it is assumed the SBG library is safe. + let error_code = unsafe { + sbgEComCmdOutputSetConf( + &mut self.handle, + _SbgEComOutputPort_SBG_ECOM_OUTPUT_PORT_A, + _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0, + _SbgEComLog_SBG_ECOM_LOG_IMU_DATA as u8, + _SbgEComOutputMode_SBG_ECOM_OUTPUT_MODE_DIV_40, + ) + }; + if error_code != _SbgErrorCode_SBG_NO_ERROR { + warn!("Unable to configure IMU logs to 40 cycles"); + } else { + self.is_initialized = true; + }; + + error_code + } + + /** + * Allows the SBG interface to read data from the serial ports. + */ + pub unsafe extern "C" fn sbg_interface_read_func( + _p_interface: *mut _SbgInterface, + p_buffer: *mut c_void, + p_bytes_read: *mut usize, + bytes_to_read: usize, + ) -> _SbgErrorCode { + if p_buffer.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + if p_bytes_read.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + // SAFETY: We are casting a c_void pointer to a u8 pointer and then creating a slice from it. + // This is safe because we ensure p_buffer is valid, p_buffer is not accessed during the lifetime of this function, + // and the sbgECom library ensures the buffer given is of the correct size. + let array: &mut [u8] = unsafe { from_raw_parts_mut(p_buffer as *mut u8, bytes_to_read) }; + let mut read_bytes = 0; + for i in 0..(bytes_to_read) { + if let Some(front) = DEQ.pop_front() { + read_bytes += 1; + array[i] = front; + } else { + break; + } + } + unsafe { *p_bytes_read = read_bytes }; + + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Allows the SBG interface to write to the UART peripheral + */ + pub unsafe extern "C" fn sbg_interface_write_func( + p_interface: *mut _SbgInterface, + p_buffer: *const c_void, + bytes_to_write: usize, + ) -> _SbgErrorCode { + if p_interface.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + if p_buffer.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + + // SAFETY: We are casting a c_void pointer to a u8 pointer and then creating a slice from it. + // This is safe because we ensure p_buffer is valid, p_buffer is not accessed during the lifetime of this function, + // and the sbgECom library ensures the buffer given is of the correct size. + let array: &[u8] = unsafe { from_raw_parts(p_buffer as *const u8, bytes_to_write) }; + let vec = array.iter().copied().collect::>(); + match unsafe { SERIAL_WRITE_CALLBACK } { + Some(callback) => callback(vec), + None => return _SbgErrorCode_SBG_WRITE_ERROR, + } + + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Callback function for handling logs. + */ + pub unsafe extern "C" fn sbg_ecom_receive_log_func( + _p_handle: *mut _SbgEComHandle, + msg_class: u32, + msg: u8, + p_log_data: *const _SbgBinaryLogData, + _p_user_arg: *mut c_void, + ) -> _SbgErrorCode { + if p_log_data.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + + // SAFETY: DATA_CALLBACK is set once, before this function is called, + // so no race conditions can happen. + if let Some(callback) = unsafe { DATA_CALLBACK } { + if msg_class == _SbgEComClass_SBG_ECOM_CLASS_LOG_ECOM_0 { + // SAFETY: p_log_data is not null, and we are checking the union flag before accessing it + unsafe { + match msg as u32 { + _SbgEComLog_SBG_ECOM_LOG_AIR_DATA => { + callback(CallbackData::Air((*p_log_data).airData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_EKF_QUAT => { + callback(CallbackData::EkfQuat((*p_log_data).ekfQuatData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_IMU_DATA => { + callback(CallbackData::Imu((*p_log_data).imuData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_EKF_NAV => { + callback(CallbackData::EkfNav((*p_log_data).ekfNavData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_POS => { + callback(CallbackData::GpsPos((*p_log_data).gpsPosData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_VEL => { + callback(CallbackData::GpsVel((*p_log_data).gpsVelData.into())) + } + _SbgEComLog_SBG_ECOM_LOG_GPS1_HDT => {} + _ => {}, + } + } + } + } + + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * The SBG interface does not need to be destroyed. + */ + pub extern "C" fn sbg_destroy_func(_p_interface: *mut _SbgInterface) -> _SbgErrorCode { + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * Flushes the UART peripheral. + */ + pub unsafe extern "C" fn sbg_flush_func(p_interface: *mut _SbgInterface, _flags: u32) -> _SbgErrorCode { + if p_interface.is_null() { + return _SbgErrorCode_SBG_NULL_POINTER; + } + // SAFETY: We are casting a c_void pointer to a Uart peripheral pointer. + // This is safe because we only have one sbg object and we ensure that + // the peripheral pointer is not accessed during the lifetime of this function. + match unsafe { SERIAL_FLUSH_CALLBACK } { + Some(callback) => callback(), + None => return _SbgErrorCode_SBG_WRITE_ERROR, + } + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * The baud rate is fixed to 115200 and hence this function does nothing. + */ + pub extern "C" fn sbg_set_speed_func(_p_interface: *mut _SbgInterface, _speed: u32) -> _SbgErrorCode { + _SbgErrorCode_SBG_NO_ERROR + } + + /** + * The baud rate is fixed to 115200 + */ + pub extern "C" fn sbg_get_speed_func(_p_interface: *const _SbgInterface) -> u32 { + 115200 + } + + /** + * Optional method used to compute an expected delay to transmit/receive X bytes + */ + pub extern "C" fn sbg_delay_func(_p_interface: *const _SbgInterface, _num_bytes: usize) -> u32 { + 501 + } +} + +// SAFETY: No one besides us has the raw pointer to the SBG struct. +// We can safely transfer the SBG struct between threads. +unsafe impl Send for SBG {} + +/** + * Logs the message to the console. + * Needs to be updated to handle the Variadic arguments. + */ +#[no_mangle] +pub unsafe extern "C" fn sbgPlatformDebugLogMsg( + p_file_name: *const ::core::ffi::c_char, + p_function_name: *const ::core::ffi::c_char, + line: u32, + p_category: *const ::core::ffi::c_char, + log_type: _SbgDebugLogType, + error_code: _SbgErrorCode, + p_format: *const ::core::ffi::c_char, +) { + if p_file_name.is_null() || p_function_name.is_null() || p_category.is_null() || p_format.is_null() { + return; + } + // SAFETY: We are converting a raw pointer to a CStr and then to a str. + // This is safe because we check if the pointers are null and + // the pointers can only be accessed during the lifetime of this function. + let file = unsafe { CStr::from_ptr(p_file_name).to_str().unwrap() }; + let function = unsafe { CStr::from_ptr(p_function_name).to_str().unwrap() }; + let category = unsafe { CStr::from_ptr(p_category).to_str().unwrap() }; + let format = unsafe { CStr::from_ptr(p_format).to_str().unwrap() }; + + info!("{}:{}:{}:{}:{}:{}", file, function, line, category, error_code, format); + + match log_type { + // silently handle errors + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_ERROR => error!("SBG Error"), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_WARNING => warn!("SBG Warning"), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_INFO => info!("SBG Info "), + _SbgDebugLogType_SBG_DEBUG_LOG_TYPE_DEBUG => debug!("SBG Debug "), + _ => (), + }; + flush(); +} + +/** + * Returns the number of milliseconds that have passed. + */ +#[no_mangle] +pub extern "C" fn sbgGetTime() -> u32 { + // SAFETY: We are accessing a static mut variable. + // This is safe because this is the only place where we access the RTC. + match unsafe { RTC_GET_TIME } { + Some(get_time) => { + get_time() + } + None => 0, + } +} + +/** + * Sleeps the sbg execution + */ +#[no_mangle] +pub extern "C" fn sbgSleep(ms: u32) { + let start_time = sbgGetTime(); + while (sbgGetTime() - start_time) < ms { + // do nothing + } +} diff --git a/crates/sbg-rs/toolchain.cmake b/crates/sbg-rs/toolchain.cmake new file mode 100644 index 0000000..7112d9c --- /dev/null +++ b/crates/sbg-rs/toolchain.cmake @@ -0,0 +1,11 @@ +# Configure the appropriate target for cross-compilation +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) + +# Compile targets as static libraries. +# Important for cross-compilation as shared libraries may not be supported. +set(CMAKE_TRY_COMPILE_TARGET_TYPE "STATIC_LIBRARY") + +# Set the appropriate cross-compiler binaries +set(CMAKE_C_COMPILER arm-none-eabi-gcc) +set(CMAKE_CXX_COMPILER arm-none-eabi-g++) diff --git a/crates/sbg-rs/wrapper.h b/crates/sbg-rs/wrapper.h new file mode 100644 index 0000000..0401e2e --- /dev/null +++ b/crates/sbg-rs/wrapper.h @@ -0,0 +1,2 @@ +#include "sbgECom/common/sbgDefines.h" +#include "sbgECom/src/sbgEComLib.h" diff --git a/examples/simple-playground/src/main.rs b/examples/simple-playground/src/main.rs index 7f73a96..5137334 100644 --- a/examples/simple-playground/src/main.rs +++ b/examples/simple-playground/src/main.rs @@ -5,8 +5,7 @@ use common_arm as _; use cortex_m_rt::entry; use defmt::info; use panic_probe as _; -use stm32h7xx_hal::pac; -use stm32h7xx_hal::prelude::*; +use stm32h7xx_hal::{adc, delay::Delay, pac, prelude::*, rcc::rec::AdcClkSel}; #[inline(never)] #[defmt::panic_handler] @@ -29,13 +28,77 @@ fn main() -> ! { info!("Reset reason: {:?}", reset); - let _ccdr = rcc + let mut ccdr = rcc .use_hse(48.MHz()) // check the clock hardware .sys_ck(200.MHz()) .freeze(pwrcfg, &dp.SYSCFG); info!("RCC configured"); + // Configure ADC + ccdr.peripheral.kernel_adc_clk_mux(AdcClkSel::Per); + let mut delay = Delay::new(_cp.SYST, ccdr.clocks); + let mut adc1 = adc::Adc::adc1(dp.ADC1, 4.MHz(), &mut delay, ccdr.peripheral.ADC12, &ccdr.clocks).enable(); + adc1.set_resolution(adc::Resolution::SixteenBit); + + let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA); + let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB); + let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC); + let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD); + + let mut main_a_arm = gpiod.pd6.into_push_pull_output(); + let mut main_a_fire = gpiod.pd5.into_push_pull_output(); + let mut main_a_sense = gpioa.pa2.into_analog(); + + let mut main_b_arm = gpiod.pd14.into_push_pull_output(); + let mut main_b_fire = gpiod.pd13.into_push_pull_output(); + let mut main_b_sense = gpiob.pb0.into_analog(); + + let mut drogue_a_arm = gpioc.pc11.into_push_pull_output(); + let mut drogue_a_fire = gpioc.pc12.into_push_pull_output(); + let mut drogue_a_sense = gpioa.pa3.into_analog(); + + let mut drogue_b_arm = gpiod.pd2.into_push_pull_output(); + let mut drogue_b_fire = gpiod.pd1.into_push_pull_output(); + let mut drogue_b_sense = gpioc.pc5.into_analog(); + + cortex_m::asm::delay(1_000_000); + + main_a_arm.set_low(); + main_a_fire.set_low(); + main_b_arm.set_low(); + main_b_fire.set_low(); + drogue_a_arm.set_low(); + drogue_a_fire.set_low(); + drogue_b_arm.set_low(); + drogue_b_fire.set_low(); + + cortex_m::asm::delay(1_000); + + main_a_arm.set_high(); + main_b_arm.set_high(); + drogue_a_arm.set_high(); + drogue_b_arm.set_high(); + loop { - info!("Hello, world!"); + const VREF: f32 = 3.0; // In volts + let res: f32 = adc1.slope() as f32; + + let main_a_sense: u32 = adc1.read(&mut main_a_sense).unwrap(); + let main_b_sense: u32 = adc1.read(&mut main_b_sense).unwrap(); + let drogue_a_sense: u32 = adc1.read(&mut drogue_a_sense).unwrap(); + let drogue_b_sense: u32 = adc1.read(&mut drogue_b_sense).unwrap(); + + let main_a_data: f32 = main_a_sense as f32 * (VREF / res); + let main_b_data: f32 = main_b_sense as f32 * (VREF / res); + let drogue_a_data: f32 = drogue_a_sense as f32 * (VREF / res); + let drogue_b_data: f32 = drogue_b_sense as f32 * (VREF / res); + + info!("Main A: {:?}", main_a_data); + info!("Main B: {:?}", main_b_data); + info!("Drogue A: {:?}", drogue_a_data); + info!("Drogue B: {:?}", drogue_b_data); + info!("\n"); + + cortex_m::asm::delay(100_000_000); } } diff --git a/phoenix/Cargo.toml b/phoenix/Cargo.toml index ee9bced..0392b48 100644 --- a/phoenix/Cargo.toml +++ b/phoenix/Cargo.toml @@ -15,6 +15,7 @@ stm32h7xx-hal = { workspace = true } postcard = { workspace = true } defmt = { workspace = true} fdcan = { workspace = true } +sbg-rs = { path = "../crates/sbg-rs" } embedded-alloc = {workspace = true} heapless = {workspace = true} rtic-sync = { workspace = true } diff --git a/phoenix/src/communication.rs b/phoenix/src/communication.rs index 9a3448c..d05e5f8 100644 --- a/phoenix/src/communication.rs +++ b/phoenix/src/communication.rs @@ -7,9 +7,8 @@ use fdcan::{ id::StandardId, }; use mavlink::peek_reader::PeekReader; -use messages::mavlink::uorocketry::MavMessage; +use messages::{mavlink::uorocketry::MavMessage, CanMessage, RadioMessage}; use messages::mavlink::{self}; -use messages::Message; use postcard::from_bytes; /// Clock configuration is out of scope for this builder @@ -30,7 +29,7 @@ impl CanCommandManager { ) -> Self { Self { can } } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + pub fn send_message(&mut self, m: CanMessage) -> Result<(), HydraError> { let mut buf = [0u8; 64]; let payload = postcard::to_slice(&m, &mut buf)?; let header = TxFrameHeader { @@ -46,11 +45,11 @@ impl CanCommandManager { pub fn process_data(&mut self, data_manager: &mut DataManager) -> Result<(), HydraError> { let mut buf = [0u8; 64]; while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { + if let Ok(data) = from_bytes::(&buf) { info!("Received message {}", data.clone()); data_manager.handle_command(data)?; } else { - info!("Error: {:?}", from_bytes::(&buf).unwrap_err()); + info!("Error: {:?}", from_bytes::(&buf).unwrap_err()); } } Ok(()) @@ -75,7 +74,7 @@ impl CanDataManager { ) -> Self { Self { can } } - pub fn send_message(&mut self, m: Message) -> Result<(), HydraError> { + pub fn send_message(&mut self, m: CanMessage) -> Result<(), HydraError> { let mut buf = [0u8; 64]; let payload = postcard::to_slice(&m, &mut buf)?; let header = TxFrameHeader { @@ -94,10 +93,10 @@ impl CanDataManager { pub fn process_data(&mut self) -> Result<(), HydraError> { let mut buf = [0u8; 64]; while self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { + if let Ok(data) = from_bytes::(&buf) { info!("Received message {}", data.clone()); crate::app::send_gs::spawn(data).ok(); - } else if let Err(e) = from_bytes::(&buf) { + } else if let Err(e) = from_bytes::(&buf) { info!("Error: {:?}", e); } } @@ -105,10 +104,10 @@ impl CanDataManager { .clear_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg); Ok(()) } - pub fn receive_message(&mut self) -> Result, HydraError> { + pub fn receive_message(&mut self) -> Result, HydraError> { let mut buf = [0u8; 64]; if self.can.receive0(&mut buf).is_ok() { - if let Ok(data) = from_bytes::(&buf) { + if let Ok(data) = from_bytes::(&buf) { return Ok(Some(data)); } } @@ -117,12 +116,12 @@ impl CanDataManager { } pub struct RadioDevice { - transmitter: stm32h7xx_hal::serial::Tx, - pub receiver: PeekReader>, + transmitter: stm32h7xx_hal::serial::Tx, + pub receiver: PeekReader>, } impl RadioDevice { - pub fn new(uart: stm32h7xx_hal::serial::Serial) -> Self { + pub fn new(uart: stm32h7xx_hal::serial::Serial) -> Self { let (tx, mut rx) = uart.split(); rx.listen(); @@ -175,7 +174,7 @@ impl RadioManager { self.mav_sequence = self.mav_sequence.wrapping_add(1); self.mav_sequence } - pub fn receive_message(&mut self) -> Result { + pub fn receive_message(&mut self) -> Result { let (_header, msg): (_, MavMessage) = mavlink::read_versioned_msg(&mut self.radio.receiver, mavlink::MavlinkVersion::V2)?; @@ -183,12 +182,12 @@ impl RadioManager { // Do we need the header? match msg { mavlink::uorocketry::MavMessage::POSTCARD_MESSAGE(msg) => { - Ok(postcard::from_bytes::(&msg.message)?) + Ok(from_bytes::(&msg.message)?) // weird Ok syntax to coerce to hydra error type. } mavlink::uorocketry::MavMessage::COMMAND_MESSAGE(command) => { info!("{}", command.command); - Ok(postcard::from_bytes::(&command.command)?) + Ok(from_bytes::(&command.command)?) } mavlink::uorocketry::MavMessage::HEARTBEAT(_) => { info!("Heartbeat"); diff --git a/phoenix/src/data_manager.rs b/phoenix/src/data_manager.rs index a396105..026156e 100644 --- a/phoenix/src/data_manager.rs +++ b/phoenix/src/data_manager.rs @@ -1,48 +1,38 @@ use common_arm::HydraError; use messages::command::RadioRate; -use messages::state::StateData; -use messages::Message; +use messages::sensor::{Gps, SbgData}; +use messages::state::State; +use messages::{CanData, CanMessage, Common, RadioData, RadioMessage}; use stm32h7xx_hal::rcc::ResetReason; + #[derive(Clone)] -pub struct DataManager { - pub air: Option, - pub ekf_nav_1: Option, - pub ekf_nav_2: Option, - pub ekf_nav_acc: Option, - pub ekf_quat: Option, - pub madgwick_quat: Option, - pub imu_1: Option, - pub imu_2: Option, - pub utc_time: Option, - pub gps_vel: Option, - pub gps_vel_acc: Option, - pub gps_pos_1: Option, - pub gps_pos_2: Option, - pub gps_pos_acc: Option, - pub state: Option, +pub struct DataManager<'a> { + pub air: Option>, + pub ekf_nav: Option>, + pub ekf_quat: Option>, + pub madgwick_quat: Option>, + pub imu: Option>, + pub utc_time: Option>, + pub gps_vel: Option>, + pub gps_pos: Option>, + pub recovery_sensing: Option>, + pub nav_pos_l1h: Option>, + pub state: Option, pub reset_reason: Option, pub logging_rate: Option, - pub recovery_sensing: Option, - pub nav_pos_l1h: Option, } -impl DataManager { +impl <'a> DataManager<'a> { pub fn new() -> Self { Self { air: None, - ekf_nav_1: None, - ekf_nav_2: None, - ekf_nav_acc: None, + ekf_nav: None, ekf_quat: None, madgwick_quat: None, - imu_1: None, - imu_2: None, + imu: None, utc_time: None, gps_vel: None, - gps_vel_acc: None, - gps_pos_1: None, - gps_pos_2: None, - gps_pos_acc: None, + gps_pos: None, state: None, reset_reason: None, logging_rate: Some(RadioRate::Slow), // start slow. @@ -62,28 +52,22 @@ impl DataManager { } /// Do not clone instead take to reduce CPU load. - pub fn take_sensors(&mut self) -> [Option; 16] { + pub fn take_sensors(&mut self) -> [Option; 10] { [ self.air.take(), - self.ekf_nav_1.take(), - self.ekf_nav_2.take(), - self.ekf_nav_acc.take(), + self.ekf_nav.take(), self.ekf_quat.take(), self.madgwick_quat.take(), - self.imu_1.take(), - self.imu_2.take(), + self.imu.take(), self.utc_time.take(), self.gps_vel.take(), - self.gps_vel_acc.take(), - self.gps_pos_1.take(), - self.gps_pos_2.take(), - self.gps_pos_acc.take(), + self.gps_pos.take(), self.nav_pos_l1h.take(), self.recovery_sensing.take(), ] } - pub fn clone_states(&self) -> [Option; 1] { + pub fn clone_states(&self) -> [Option; 1] { [self.state.clone()] } @@ -95,97 +79,69 @@ impl DataManager { self.reset_reason = Some(reset); } - pub fn handle_command(&mut self, data: Message) -> Result<(), HydraError> { - match data.data { - messages::Data::Command(command) => match command.data { - messages::command::CommandData::PowerDown(_) => { + pub fn handle_command(&mut self, data: CanMessage) -> Result<(), HydraError> { + if let CanData::Common(Common::Command(command)) = data.data { + match command { + messages::command::Command::PowerDown(_) => { crate::app::sleep_system::spawn().ok(); } - messages::command::CommandData::RadioRateChange(command_data) => { + messages::command::Command::RadioRateChange(command_data) => { self.logging_rate = Some(command_data.rate); } - _ => { - // We don't care atm about these other commands. - } - }, - _ => { - // we can disregard all other messages for now. + _ => {} // We don't care atm about these other commands. } } + Ok(()) } - pub fn handle_data(&mut self, data: Message) { + + pub fn handle_data(&mut self, data: RadioMessage) { match data.data { - messages::Data::Sensor(ref sensor) => match sensor.data { - messages::sensor::SensorData::SbgData(ref sbg_data) => match sbg_data { - messages::sensor::SbgData::EkfNavAcc(_) => { - self.ekf_nav_acc = Some(data); - } - messages::sensor::SbgData::GpsPosAcc(_) => { - self.gps_pos_acc = Some(data); - } - messages::sensor::SbgData::Air(_) => { - self.air = Some(data); - } - messages::sensor::SbgData::EkfNav1(_) => { - self.ekf_nav_1 = Some(data); - } - messages::sensor::SbgData::EkfNav2(_) => { - self.ekf_nav_2 = Some(data); - } - messages::sensor::SbgData::EkfQuat(_) => { - self.ekf_quat = Some(data); - } - messages::sensor::SbgData::GpsVel(_) => { - self.gps_vel = Some(data); - } - messages::sensor::SbgData::GpsVelAcc(_) => { - self.gps_vel_acc = Some(data); - } - messages::sensor::SbgData::Imu1(_) => { - self.imu_1 = Some(data); - } - messages::sensor::SbgData::Imu2(_) => { - self.imu_2 = Some(data); - } - messages::sensor::SbgData::UtcTime(_) => { - self.utc_time = Some(data); - } - messages::sensor::SbgData::GpsPos1(_) => { - self.gps_pos_1 = Some(data); - } - messages::sensor::SbgData::GpsPos2(_) => { - self.gps_pos_2 = Some(data); - } + RadioData::Sbg(ref sbg_data) => match sbg_data { + SbgData::UtcTime(utc_time) => { + self.utc_time = Some(data); + }, + SbgData::Air(_) => { + self.air = Some(data); + }, + SbgData::EkfQuat(ekf_quat) => { + self.ekf_quat = Some(data); + }, + SbgData::EkfNav(ekf_nav) => { + self.ekf_nav = Some(data); + }, + SbgData::Imu(imu) => { + self.imu = Some(data); }, + SbgData::GpsVel(gps_vel) => { + self.gps_vel = Some(data); + }, + SbgData::GpsPos(gps_pos) => { + self.gps_pos = Some(data); + }, + }, + RadioData::Gps(Gps::NavPosLlh(_)) => { + self.gps_pos = Some(data); + }, + messages::Data::Sensor(ref sensor) => match sensor.data { messages::sensor::SensorData::RecoverySensing(_) => { self.recovery_sensing = Some(data); } - messages::sensor::SensorData::NavPosLlh(_) => { - self.nav_pos_l1h = Some(data); - } messages::sensor::SensorData::ResetReason(_) => {} }, - messages::Data::State(state) => { - self.state = Some(state.data); - } - // messages::Data::Command(command) => match command.data { - // messages::command::CommandData::RadioRateChange(command_data) => { - // self.logging_rate = Some(command_data.rate); - // } - // messages::command::CommandData::DeployDrogue(_) => {} - // messages::command::CommandData::DeployMain(_) => {} - // messages::command::CommandData::PowerDown(_) => {} - // }, + RadioData::Common(Common::State(state)) => { + self.state = Some(state); + }, _ => {} } } - pub fn store_madgwick_result(&mut self, result: Message) { + + pub fn store_madgwick_result(&mut self, result: RadioMessage<'a>) { self.madgwick_quat = Some(result); } } -impl Default for DataManager { +impl Default for DataManager<'_> { fn default() -> Self { Self::new() } diff --git a/phoenix/src/madgwick_service.rs b/phoenix/src/madgwick_service.rs index 70c68bd..85f6eb0 100644 --- a/phoenix/src/madgwick_service.rs +++ b/phoenix/src/madgwick_service.rs @@ -1,6 +1,6 @@ use madgwick::Marg; -use messages::{Message, sensor::{self, SbgData, EkfQuat}}; -use messages::sensor::Sensor; +use messages::{RadioData, RadioMessage}; +use messages::sensor::{EkfQuat, SbgData}; use messages::sensor_status::EkfStatus; /// Service that implements the Madgwick sensor fusion algorithim for orientation @@ -68,55 +68,44 @@ impl MadgwickService { } } - /// Method for processing incoming IMU data; returns a new Message with an updated quaternion from the filter - pub fn process_imu_data(&mut self, data: &Message) -> Option { + /// Method for processing incoming IMU data. Returns a new RadioMessage with an updated quaternion from the filter. + pub fn process_imu_data(&mut self, data: &RadioMessage) -> Option { match &data.data { - messages::Data::Sensor(sensor) => match &sensor.data { - messages::sensor::SensorData::SbgData(ref sbg_data) => match sbg_data { - SbgData::Imu1(imu_data) => { - if let (Some(accel), Some(gyro)) = (imu_data.accelerometers, imu_data.gyroscopes) { - let mag = madgwick::F32x3 { x: 0.0, y: 0.0, z: 0.0 }; - let gyro = madgwick::F32x3 { - x: gyro[0], - y: gyro[1], - z: gyro[2], - }; - - let accel = madgwick::F32x3 { - x: accel[0], - y: accel[1], - z: accel[2], - }; + RadioData::Sbg(SbgData::Imu(imu_data)) + if imu_data.accelerometers.is_some() && imu_data.gyroscopes.is_some() => + { + // Unwrap cannot panic here because we checked is_some() above + let accel = imu_data.accelerometers.unwrap(); + let gyro = imu_data.gyroscopes.unwrap(); - let quat = self.madgwick.update(mag, gyro, accel); - - // Store the latest quaternion - self.latest_quat = (quat.0, quat.1, quat.2, quat.3); - - Some(Message::new( - data.timestamp.clone(), - data.node.clone(), - Sensor::new( - sensor::SensorData::SbgData( - SbgData::EkfQuat( - EkfQuat { - time_stamp: imu_data.time_stamp, - quaternion: Some([quat.0, quat.1, quat.2, quat.3]), - euler_std_dev: None, - status: EkfStatus::new(0), - } - ) - ) - ) - )) - } else { - None - } - }, - _ => None, - }, - _ => None, - }, + let mag = madgwick::F32x3 { x: 0.0, y: 0.0, z: 0.0 }; + let gyro = madgwick::F32x3 { + x: gyro[0], + y: gyro[1], + z: gyro[2], + }; + let accel = madgwick::F32x3 { + x: accel[0], + y: accel[1], + z: accel[2], + }; + + let quat = self.madgwick.update(mag, gyro, accel); + + // Store the latest quaternion + self.latest_quat = (quat.0, quat.1, quat.2, quat.3); + + Some(RadioMessage::new( + data.timestamp.clone(), + data.node.clone(), + RadioData::Sbg(SbgData::EkfQuat(EkfQuat { + time_stamp: imu_data.time_stamp, + quaternion: Some([quat.0, quat.1, quat.2, quat.3]), + euler_std_dev: None, + status: EkfStatus::new(0), + })), + )) + } _ => None, } } @@ -153,4 +142,4 @@ impl MadgwickService { pub fn get_sample_period(&self) -> f32 { self.sample_period } -} \ No newline at end of file +} diff --git a/phoenix/src/main.rs b/phoenix/src/main.rs index dda1bce..15e9712 100644 --- a/phoenix/src/main.rs +++ b/phoenix/src/main.rs @@ -4,6 +4,7 @@ mod communication; mod data_manager; mod madgwick_service; +mod sbg_manager; mod types; use chrono::NaiveDate; @@ -17,20 +18,26 @@ use fdcan::{ config::NominalBitTiming, filter::{StandardFilter, StandardFilterSlot}, }; +use heapless::Vec; +use messages::{CanMessage, RadioData, RadioMessage}; use messages::command::RadioRate; -use messages::{sensor, Data}; use panic_probe as _; use rtic_monotonics::systick::prelude::*; use rtic_sync::{channel::*, make_channel}; -use stm32h7xx_hal::gpio::gpioa::{PA2, PA3}; +use sbg_manager::{SBGManager, sbg_dma, sbg_handle_data, sbg_flush, sbg_write_data, sbg_sd_task, sbg_get_time}; +use sbg_rs::{CallbackData, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamsTuple; +use stm32h7xx_hal::gpio::gpioa::{PA2, PA3, PA4}; use stm32h7xx_hal::gpio::gpiob::PB4; use stm32h7xx_hal::gpio::Speed; use stm32h7xx_hal::gpio::{Output, PushPull}; use stm32h7xx_hal::prelude::*; use stm32h7xx_hal::rtc; use stm32h7xx_hal::{rcc, rcc::rec}; +use stm32h7xx_hal::spi; use types::COM_ID; // global logger + const DATA_CHANNEL_CAPACITY: usize = 10; systick_monotonic!(Mono, 500); @@ -45,9 +52,6 @@ fn panic() -> ! { #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, dispatchers = [EXTI0, EXTI1, EXTI2, SPI3, SPI2])] mod app { - use messages::Message; - use stm32h7xx_hal::gpio::{Alternate, Pin}; - use super::*; #[shared] @@ -55,11 +59,12 @@ mod app { data_manager: DataManager, madgwick_service: madgwick_service::MadgwickService, em: ErrorManager, - // sd_manager: SdManager< - // stm32h7xx_hal::spi::Spi, - // PA4>, - // >, + sd_manager: SdManager< + stm32h7xx_hal::spi::Spi, + PA4>, + >, radio_manager: RadioManager, + sbg_manager: SBGManager, can_command_manager: CanCommandManager, can_data_manager: CanDataManager, sbg_power: PB4>, @@ -79,7 +84,7 @@ mod app { #[init] fn init(ctx: init::Context) -> (SharedResources, LocalResources) { // channel setup - let (_s, r) = make_channel!(Message, DATA_CHANNEL_CAPACITY); + let (_s, r) = make_channel!(CanMessage, DATA_CHANNEL_CAPACITY); let core = ctx.core; @@ -133,6 +138,7 @@ mod app { let gpioa = ctx.device.GPIOA.split(ccdr.peripheral.GPIOA); let gpiod = ctx.device.GPIOD.split(ccdr.peripheral.GPIOD); let gpiob = ctx.device.GPIOB.split(ccdr.peripheral.GPIOB); + let gpioe = ctx.device.GPIOE.split(ccdr.peripheral.GPIOE); let pins = gpiob.pb14.into_alternate(); let mut c0 = ctx @@ -232,25 +238,25 @@ mod app { let can_command_manager = CanCommandManager::new(can_command.into_normal()); - // let spi_sd: stm32h7xx_hal::spi::Spi< - // stm32h7xx_hal::stm32::SPI1, - // stm32h7xx_hal::spi::Enabled, - // u8, - // > = ctx.device.SPI1.spi( - // ( - // gpioa.pa5.into_alternate::<5>(), - // gpioa.pa6.into_alternate(), - // gpioa.pa7.into_alternate(), - // ), - // spi::Config::new(spi::MODE_0), - // 16.MHz(), - // ccdr.peripheral.SPI1, - // &ccdr.clocks, - // ); - - // let cs_sd = gpioa.pa4.into_push_pull_output(); - - // let sd_manager = SdManager::new(spi_sd, cs_sd); + let spi_sd: stm32h7xx_hal::spi::Spi< + stm32h7xx_hal::stm32::SPI1, + stm32h7xx_hal::spi::Enabled, + u8, + > = ctx.device.SPI1.spi( + ( + gpioa.pa5.into_alternate::<5>(), + gpioa.pa6.into_alternate(), + gpioa.pa7.into_alternate(), + ), + spi::Config::new(spi::MODE_0), + 16.MHz(), + ccdr.peripheral.SPI1, + &ccdr.clocks, + ); + + let cs_sd = gpioa.pa4.into_push_pull_output(); + + let sd_manager = SdManager::new(spi_sd, cs_sd); // leds let led_red = gpioa.pa2.into_push_pull_output(); @@ -261,19 +267,25 @@ mod app { sbg_power.set_high(); // UART for sbg - let tx: Pin<'D', 1, Alternate<8>> = gpiod.pd1.into_alternate(); - let rx: Pin<'D', 0, Alternate<8>> = gpiod.pd0.into_alternate(); - - // let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); - let uart_radio = ctx + let tx = gpioa.pa0.into_alternate(); + let rx = gpioa.pa1.into_alternate(); + let stream_tuple = StreamsTuple::new(ctx.device.DMA1, ccdr.peripheral.DMA1); + let uart_sbg = ctx .device .UART4 - .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .serial((tx, rx), 115_200.bps(), ccdr.peripheral.UART4, &ccdr.clocks) + .unwrap(); + let sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); + + // UART for radio + let tx = gpioe.pe8.into_alternate(); + let rx = gpioe.pe7.into_alternate(); + let uart_radio = ctx + .device + .UART7 + .serial((tx, rx), 57600.bps(), ccdr.peripheral.UART7, &ccdr.clocks) .unwrap(); - // let mut sbg_manager = sbg_manager::SBGManager::new(uart_sbg, stream_tuple); - let radio = RadioDevice::new(uart_radio); - let radio_manager = RadioManager::new(radio); let mut rtc = stm32h7xx_hal::rtc::Rtc::open_or_init( @@ -312,8 +324,9 @@ mod app { data_manager, madgwick_service, em, - // sd_manager, + sd_manager, radio_manager, + sbg_manager, can_command_manager, can_data_manager, sbg_power, @@ -331,12 +344,12 @@ mod app { async fn generate_random_messages(mut cx: generate_random_messages::Context) { loop { cx.shared.em.run(|| { - let message = Message::new( + let message = RadioMessage::new( cx.shared .rtc .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), COM_ID, - messages::state::State::new(messages::state::StateData::Initializing), + messages::Common::State(messages::state::State::Initializing), ); spawn!(send_gs, message.clone())?; // spawn!(send_data_internal, message)?; @@ -368,7 +381,7 @@ mod app { stm32h7xx_hal::rcc::ResetReason::Unknown { rcc_rsr } => sensor::ResetReason::Unknown { rcc_rsr }, stm32h7xx_hal::rcc::ResetReason::WindowWatchdogReset => sensor::ResetReason::WindowWatchdogReset, }; - let message = messages::Message::new( + let message = RadioMessage::new( cx.shared .rtc .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), @@ -393,12 +406,12 @@ mod app { .lock(|data_manager| data_manager.state.clone()); cx.shared.em.run(|| { if let Some(x) = state_data { - let message = Message::new( + let message = RadioMessage::new( cx.shared .rtc .lock(|rtc| messages::FormattedNaiveDateTime(rtc.date_time().unwrap())), COM_ID, - messages::state::State::new(x), + messages::Common::State(x), ); spawn!(send_gs, message)?; } // if there is none we still return since we simply don't have data yet. @@ -447,16 +460,16 @@ mod app { } /// Receives a log message from the custom logger so that it can be sent over the radio. - pub fn queue_gs_message(d: impl Into) { + pub fn queue_gs_message<'a>(d: impl Into>) { info!("Queueing message"); send_gs_intermediate::spawn(d.into()).ok(); } #[task(priority = 3, shared = [rtc, &em])] - async fn send_gs_intermediate(mut cx: send_gs_intermediate::Context, m: Data) { + async fn send_gs_intermediate(mut cx: send_gs_intermediate::Context, m: RadioData<'_>) { cx.shared.em.run(|| { cx.shared.rtc.lock(|rtc| { - let message = messages::Message::new( + let message = RadioMessage::new( messages::FormattedNaiveDateTime(rtc.date_time().unwrap()), COM_ID, m, @@ -491,7 +504,7 @@ mod app { * Sends a message to the radio over UART. */ #[task(priority = 3, shared = [&em, radio_manager])] - async fn send_gs(mut cx: send_gs::Context, m: Message) { + async fn send_gs(mut cx: send_gs::Context, m: RadioMessage<'_>) { // info!("{}", m.clone()); cx.shared.radio_manager.lock(|radio_manager| { @@ -525,7 +538,7 @@ mod app { #[task(priority = 2, shared = [&em, can_data_manager, data_manager])] async fn send_data_internal( mut cx: send_data_internal::Context, - mut receiver: Receiver<'static, Message, DATA_CHANNEL_CAPACITY>, + mut receiver: Receiver<'static, CanMessage, DATA_CHANNEL_CAPACITY>, ) { loop { if let Ok(m) = receiver.recv().await { @@ -540,7 +553,7 @@ mod app { } #[task(priority = 2, shared = [&em, can_command_manager, data_manager])] - async fn send_command_internal(mut cx: send_command_internal::Context, m: Message) { + async fn send_command_internal(mut cx: send_command_internal::Context, m: CanMessage) { // while let Ok(m) = receiver.recv().await { cx.shared.can_command_manager.lock(|can| { cx.shared.em.run(|| { @@ -587,4 +600,26 @@ mod app { sbg.set_low(); }); } + + // These tasks are defined in sbg_manager but RTIC can only apply macros to definitions in the same module. + // Declaring these as extern allows us to decorate them with the appropriate macros. + extern "Rust" { + #[task(priority = 1, shared = [&em, sd_manager])] + async fn sbg_sd_task(context: sbg_sd_task::Context, data: [u8; SBG_BUFFER_SIZE]); + + #[task(priority = 3, binds = DMA1_STR1, shared = [&em, sbg_manager])] + fn sbg_dma(mut context: sbg_dma::Context); + + #[task(priority = 2, shared = [data_manager])] + async fn sbg_handle_data(context: sbg_handle_data::Context, data: CallbackData); + + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_flush(context: sbg_flush::Context); + + #[task(priority = 1, shared = [&em, sbg_manager])] + async fn sbg_write_data(context: sbg_write_data::Context, data: Vec); + + #[task(priority = 1, shared = [&em, rtc])] + async fn sbg_get_time(context: sbg_get_time::Context, time: &mut u32); + } } diff --git a/phoenix/src/sbg_manager.rs b/phoenix/src/sbg_manager.rs new file mode 100644 index 0000000..5ce4522 --- /dev/null +++ b/phoenix/src/sbg_manager.rs @@ -0,0 +1,276 @@ +use core::alloc::{GlobalAlloc, Layout}; +use core::ffi::c_void; +use core::mem::size_of; +use core::ptr; +use crate::app::{sbg_flush, sbg_get_time}; +use crate::app::sbg_handle_data; +use crate::app::sbg_write_data; +use chrono::{NaiveDate, NaiveDateTime, NaiveTime}; +use core::mem::MaybeUninit; +use defmt::info; +use embedded_alloc::Heap; +use heapless::Vec; +use rtic::Mutex; +use sbg_rs as sbg; +use sbg::{CallbackData, SBG, SBG_BUFFER_SIZE}; +use stm32h7xx_hal::dma::dma::StreamX; +use stm32h7xx_hal::dma::{ + dma::{DmaConfig, StreamsTuple}, + PeripheralToMemory, Transfer, +}; +use stm32h7xx_hal::pac::UART4; +use stm32h7xx_hal::serial::{Rx, Tx}; +use messages::mavlink::embedded::Write; + +const SBG_LOG_FILE_NAME: &str = "lc24.txt"; + +// must have this link section. +#[link_section = ".axisram.buffers"] +pub static mut SBG_BUFFER: MaybeUninit<[u8; SBG_BUFFER_SIZE]> = MaybeUninit::uninit(); + +// Simple heap required by the SBG library +static HEAP: Heap = Heap::empty(); + +pub struct SBGManager { + pub sbg_device: SBG, + xfer: Option< + Transfer< + StreamX, + Rx, + stm32h7xx_hal::dma::PeripheralToMemory, + &'static mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + >, + >, + sbg_tx: Tx, +} + +impl SBGManager { + pub fn new( + serial: stm32h7xx_hal::serial::Serial, + stream_tuple: StreamsTuple, + ) -> Self { + /* Initialize the Heap */ + { + use core::mem::MaybeUninit; + const HEAP_SIZE: usize = 1024; + // TODO: Could add a link section here to memory. + static mut HEAP_MEM: [MaybeUninit; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE]; + unsafe { HEAP.init(HEAP_MEM.as_ptr() as usize, HEAP_SIZE) } + } + + let (sbg_tx, sbg_rx) = serial.split(); + + // TODO: This could be wrong. It's a bit of a guess. + // let sbg_buffer: &'static mut [u8; SBG_BUFFER_SIZE] = { + // let buf: &mut [MaybeUninit; SBG_BUFFER_SIZE] = + // unsafe { &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _) }; + // for (i, value) in buf.iter_mut().enumerate() { + // unsafe { value.as_mut_ptr().write(i as u8) }; + // } + // unsafe { SBG_BUFFER.assume_init_mut() } + // }; + unsafe { + // Convert an uninitialised array into an array of uninitialised + let buf: &mut [core::mem::MaybeUninit; SBG_BUFFER_SIZE] = + &mut *(core::ptr::addr_of_mut!(SBG_BUFFER) as *mut _); + buf.iter_mut().for_each(|x| x.as_mut_ptr().write(0)); + } + + let config = DmaConfig::default() + .memory_increment(true) + .transfer_complete_interrupt(true); + let mut transfer: Transfer< + StreamX, + Rx, + PeripheralToMemory, + &mut [u8; SBG_BUFFER_SIZE], + stm32h7xx_hal::dma::DBTransfer, + > = Transfer::init( + stream_tuple.1, + sbg_rx, + unsafe { SBG_BUFFER.assume_init_mut() }, // Uninitialised memory + None, + config, + ); + + info!("Starting transfer"); + + // Could this be unsafe because what happens if the interrupt fires before the object is created which is used in the interrupt handler. + + transfer.start(|serial| { + serial.enable_dma_rx(); + }); + info!("Transfer started"); + + // while !transfer.get_transfer_complete_flag() {} + // info!("Transfer complete"); + // info!("{}", unsafe { SBG_BUFFER.assume_init_read() }); + + let sbg: sbg::SBG = sbg::SBG::new( + |data| { + sbg_handle_data::spawn(data).ok(); + }, + |data| { + sbg_write_data::spawn(data).ok(); + }, + || { + let mut time: u32 = 0; + sbg_get_time::spawn(&mut time).ok(); + time + }, + || { + sbg_flush::spawn().ok(); + }, + ); + // sbg.setup(); + // sbg.read_data(&unsafe { SBG_BUFFER.assume_init_read() }); + + SBGManager { + sbg_device: sbg, + xfer: Some(transfer), + sbg_tx, + } + } +} + +pub async fn sbg_flush(mut _cx: sbg_flush::Context<'_>) { + // cx.shared.sbg_manager.lock(|sbg| { + // sbg.sbg_tx + // }); +} + +pub async fn sbg_write_data(mut cx: sbg_write_data::Context<'_>, data: Vec) { + cx.shared.sbg_manager.lock(|sbg| { + sbg.sbg_tx.write_all(data.as_slice()); + }); +} + +pub fn sbg_get_time(mut cx: sbg_get_time::Context, time: &mut u32) { + *time = cx.shared.rtc.lock(|rtc| { + rtc.date_time() + .unwrap_or(NaiveDateTime::new( + NaiveDate::from_ymd_opt(2024, 1, 1).unwrap(), + NaiveTime::from_hms_milli_opt(0, 0, 0, 0).unwrap(), + )) + .and_utc() + .timestamp_subsec_millis() + }); +} + +pub async fn sbg_handle_data(mut cx: sbg_handle_data::Context<'_>, data: CallbackData) { + cx.shared.data_manager.lock(|manager| match data { + CallbackData::UtcTime(x) => manager.utc_time = Some(x), + CallbackData::Air(x) => manager.air = Some(x), + CallbackData::EkfQuat(x) => manager.ekf_quat = Some(x), + CallbackData::EkfNav(x) => manager.ekf_nav = Some(x), + CallbackData::Imu(x) => manager.imu = Some(x), + CallbackData::GpsVel(x) => manager.gps_vel = Some(x), + CallbackData::GpsPos(x) => manager.gps_pos = Some(x), + }); +} + +pub async fn sbg_sd_task( + mut cx: crate::app::sbg_sd_task::Context<'_>, + data: [u8; SBG_BUFFER_SIZE], +) { + cx.shared.sd_manager.lock(|manager| { + if let Some(mut file) = manager.file.take() { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); // give the file back after use + } else if let Ok(mut file) = manager.open_file(SBG_LOG_FILE_NAME) { + cx.shared.em.run(|| { + manager.write(&mut file, &data)?; + Ok(()) + }); + manager.file = Some(file); + } + }); +} + +/** + * Handles the DMA interrupt. + * Handles the SBG data. + */ +pub fn sbg_dma(mut cx: crate::app::sbg_dma::Context) { + cx.shared.sbg_manager.lock(|sbg| { + match &mut sbg.xfer { + Some(xfer) => { + if xfer.get_transfer_complete_flag() { + let data = unsafe { SBG_BUFFER.assume_init_read() }.clone(); + xfer.next_transfer( + unsafe { (*core::ptr::addr_of_mut!(SBG_BUFFER)).assume_init_mut() }, // Uninitialised memory + ); + // info!("{}", data); + xfer.clear_transfer_complete_interrupt(); + sbg.sbg_device.read_data(&data); + crate::app::sbg_sd_task::spawn(data).ok(); + } + } + None => { + // it should be impossible to reach here. + info!("None"); + } + } + }); +} + +/// Stored right before an allocation. Stores information that is needed to deallocate memory. +#[derive(Copy, Clone)] +struct AllocInfo { + layout: Layout, + ptr: *mut u8, +} + +/// Custom malloc for the SBG library. This uses the HEAP object initialized at the start of the +/// [`SBGManager`]. The [`Layout`] of the allocation is stored right before the returned pointed, +/// which makes it possible to implement [`free`] without any other data structures. +#[no_mangle] +pub extern "C" fn malloc(size: usize) -> *mut c_void { + if size == 0 { + return ptr::null_mut(); + } + + // Get a layout for both the requested size + let header_layout = Layout::new::(); + let requested_layout = Layout::from_size_align(size, 8).unwrap(); + let (layout, offset) = header_layout.extend(requested_layout).unwrap(); + + // Ask the allocator for memory + let orig_ptr = unsafe { HEAP.alloc(layout) }; + if orig_ptr.is_null() { + return orig_ptr as *mut c_void; + } + + // Compute the pointer that we will return + let result_ptr = unsafe { orig_ptr.add(offset) }; + + // Store the allocation information right before the returned pointer + let info_ptr = unsafe { result_ptr.sub(size_of::()) as *mut AllocInfo }; + unsafe { + info_ptr.write_unaligned(AllocInfo { + layout, + ptr: orig_ptr, + }); + } + + result_ptr as *mut c_void +} + +/// Custom free implementation for the SBG library. This uses the stored allocation information +/// right before the pointer to free up the resources. +/// +/// SAFETY: The value passed to ptr must have been obtained from a previous call to [`malloc`]. +#[no_mangle] +pub unsafe extern "C" fn free(ptr: *mut c_void) { + assert!(!ptr.is_null()); + + let info_ptr = unsafe { ptr.sub(size_of::()) as *const AllocInfo }; + let info = unsafe { info_ptr.read_unaligned() }; + unsafe { + HEAP.dealloc(info.ptr, info.layout); + } +}