diff --git a/.clang-format b/.clang-format index 9cbe5f7..33bf2a3 100644 --- a/.clang-format +++ b/.clang-format @@ -1,59 +1,137 @@ --- -BasedOnStyle: Google +Language: Cpp +# BasedOnStyle: LLVM AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Right +AlignOperands: true AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: WithoutElse +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AlwaysBreakTemplateDeclarations: Yes +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: None -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true +AlwaysBreakTemplateDeclarations: MultiLine +BinPackArguments: true BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 80 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: false +DisableFormat: false ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + - Regex: '.*' + Priority: 1 + SortPriority: 0 +IncludeIsMainRegex: '(Test)?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: false +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' MaxEmptyLinesToKeep: 1 NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -SpacesInParentheses: false -SpacesInAngles: false +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 60 +PointerAlignment: Right +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true SpacesInCStyleCastParentheses: false -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 2 -SortIncludes: false -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: - AfterClass: true - AfterControlStatement: true - AfterEnum : true - AfterFunction : true - AfterNamespace : true - AfterStruct : true - AfterUnion : true - BeforeCatch : true - BeforeElse : true - IndentBraces : false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Latest +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never ... + diff --git a/.dockerignore b/.dockerignore index 0b5f973..3e4e48b 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,2 +1 @@ -.travis.yml .gitignore \ No newline at end of file diff --git a/.github/workflows/clang-format-check.yml b/.github/workflows/clang-format-check.yml new file mode 100644 index 0000000..8d05533 --- /dev/null +++ b/.github/workflows/clang-format-check.yml @@ -0,0 +1,14 @@ +name: clang-format check +on: [push] +jobs: + formatting-check: + name: C/C++ LLVM formatting check + runs-on: ubuntu-20.04 + steps: + - uses: actions/checkout@v2 + - name: Run clang-format style check for C/C++ packages + uses: jidicula/clang-format-action@v4.5.0 + with: + clang-format-version: '13' + check-path: '.' + fallback-style: 'llvm' # for when the .clang-format file does not exist diff --git a/.github/workflows/docker-publish.yml b/.github/workflows/docker-publish.yml new file mode 100644 index 0000000..5281624 --- /dev/null +++ b/.github/workflows/docker-publish.yml @@ -0,0 +1,42 @@ +name: Publish Docker image to ghcr + +on: + push: + branches: [ master ] + + +env: + REGISTRY: ghcr.io + IMAGE_NAME: ${{ github.repository }} + +jobs: + build-and-push-image: + runs-on: ubuntu-20.04 + permissions: + contents: read + packages: write + + steps: + - name: Checkout repository + uses: actions/checkout@v2 + + - name: Log in to the Container registry + uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9 + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) for Docker + id: meta + uses: docker/metadata-action@98669ae865ea3cffbcbaa878cf57c20bbf1c6c38 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + + - name: Build and push Docker image + uses: docker/build-push-action@ad44023a93711e3deb337508980b4b5e9bcdc5dc + with: + context: . + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 048e1d5..0000000 --- a/.travis.yml +++ /dev/null @@ -1,10 +0,0 @@ -language: generic - -services: - - docker - -before_script: - - docker build --tag stim300 . - -script: - - docker run stim300 catkin_test_results /catkin_ws/build/driver_stim300 diff --git a/Dockerfile b/Dockerfile index 41cffae..59cedd9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,10 +1,11 @@ -FROM ros:kinetic-ros-core-xenial -WORKDIR /catkin_ws -RUN apt-get update -RUN apt-get install --yes python-catkin-tools -COPY . ./src/stim300 - -# See https://stackoverflow.com/questions/20635472/using-the-run-instruction-in-a-dockerfile-with-source-does-not-work -RUN ["/bin/bash", "-c", "source /opt/ros/kinetic/setup.bash && \ - catkin build && \ - catkin run_tests driver_stim300 --no-deps"] +FROM ros:noetic-ros-core-focal + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install --yes --no-install-recommends python3-catkin-tools build-essential + +COPY . ./imu_ws/src +RUN source /opt/ros/noetic/setup.bash && cd /imu_ws && catkin build + +COPY ./entrypoint.sh / +CMD ["/entrypoint.sh"] diff --git a/README.md b/README.md index 39e3db1..e35f035 100644 --- a/README.md +++ b/README.md @@ -77,3 +77,28 @@ This will show a list on available commands including how to go back to normal m c will perform a system check. + + ## Calibration + +The following commands are used to calibrate the stim300 driver with the ESKF (Error-state Kalman filter) + +First launch the stim300 driver + +```bash +roslaunch driver_stim300 stim300_driver.launch +``` + +Then move to another terminal and do the following + +```bash +rosservice call /IMU_calibration +``` + +Now head over to the stim300-driver.launch terminal window and take note of the roll and pitch alignment errors. + +Find the parameter list for the ESKF and put in the roll and pitch errors in the following parameters + +```bash +sr_accelerometer_alignment +sr_gyro_alignment +``` diff --git a/entrypoint.sh b/entrypoint.sh new file mode 100755 index 0000000..1f0024f --- /dev/null +++ b/entrypoint.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +source /imu_ws/devel/setup.bash +roslaunch driver_stim300 stim300_driver.launch \ No newline at end of file diff --git a/include/driver_stim300.h b/include/driver_stim300.h index 662f22b..04d513f 100644 --- a/include/driver_stim300.h +++ b/include/driver_stim300.h @@ -2,14 +2,13 @@ #ifndef DRIVER_STIM300_DRIVER_STIM300_H #define DRIVER_STIM300_DRIVER_STIM300_H -#include -#include "../src/stim300_constants.h" #include "../src/datagram_parser.h" #include "../src/serial_unix.h" +#include "../src/stim300_constants.h" +#include #include -enum class Stim300Status -{ +enum class Stim300Status { NORMAL, NEW_MEASURMENT, CONFIG_CHANGED, @@ -21,19 +20,20 @@ enum class Stim300Status ERROR }; -class DriverStim300 -{ +class DriverStim300 { public: - DriverStim300(SerialDriver& serial_driver, DatagramIdentifier datagram_id, GyroOutputUnit gyro_output_unit, - AccOutputUnit acc_output_unit, InclOutputUnit incl_output_unit, AccRange acc_range, SampleFreq freq); - explicit DriverStim300(SerialDriver& serial_driver); + DriverStim300(SerialDriver &serial_driver, DatagramIdentifier datagram_id, + GyroOutputUnit gyro_output_unit, AccOutputUnit acc_output_unit, + InclOutputUnit incl_output_unit, AccRange acc_range, + SampleFreq freq); + explicit DriverStim300(SerialDriver &serial_driver); ~DriverStim300() = default; // The class is Non-Copyable - DriverStim300(const DriverStim300& a) = delete; - DriverStim300& operator=(const DriverStim300& a) = delete; + DriverStim300(const DriverStim300 &a) = delete; + DriverStim300 &operator=(const DriverStim300 &a) = delete; // The class is non-movable - DriverStim300(DriverStim300&& a) = delete; - DriverStim300& operator=(DriverStim300&& a) = delete; + DriverStim300(DriverStim300 &&a) = delete; + DriverStim300 &operator=(DriverStim300 &&a) = delete; double getAccX() const noexcept; double getAccY() const noexcept; @@ -54,42 +54,37 @@ class DriverStim300 Stim300Status update() noexcept; private: - enum class Mode : uint8_t - { - Init, - Normal, - Service - }; - Mode mode_{ Mode::Init }; - enum class ReadingMode - { + enum class Mode : uint8_t { Init, Normal, Service }; + Mode mode_{Mode::Init}; + enum class ReadingMode { IdentifyingDatagram, ReadingDatagram, VerifyingDatagramCR, VerifyingDatagramLF }; - ReadingMode reading_mode_{ ReadingMode::IdentifyingDatagram }; + ReadingMode reading_mode_{ReadingMode::IdentifyingDatagram}; - SerialDriver& serial_driver_; + SerialDriver &serial_driver_; stim_300::DatagramParser datagram_parser_; std::vector buffer_{}; - size_t n_new_bytes_{ 0 }; - size_t n_checked_bytes{ 0 }; + size_t n_new_bytes_{0}; + size_t n_checked_bytes{0}; uint8_t datagram_id_; uint8_t crc_dummy_bytes_; uint8_t datagram_size_; stim_300::SensorConfig sensor_config_; - bool read_config_from_sensor_{ true }; + bool read_config_from_sensor_{true}; stim_300::SensorData sensor_data_{}; - uint8_t sensor_status_{ 0 }; + uint8_t sensor_status_{0}; Stim300Status readDataStream(); bool setDatagramFormat(DatagramIdentifier id); - static bool verifyChecksum(const std::vector::const_iterator& begin, - const std::vector::const_iterator& end, const uint8_t& crc_dummy_bytes); + static bool verifyChecksum(const std::vector::const_iterator &begin, + const std::vector::const_iterator &end, + const uint8_t &crc_dummy_bytes); void askForConfigDatagram(); }; -#endif // DRIVER_STIM300_DRIVER_STIM300_H +#endif // DRIVER_STIM300_DRIVER_STIM300_H diff --git a/src/datagram_parser.cpp b/src/datagram_parser.cpp index ada1129..c593d03 100644 --- a/src/datagram_parser.cpp +++ b/src/datagram_parser.cpp @@ -4,107 +4,102 @@ using namespace stim_const; using namespace stim_300; -DatagramParser::DatagramParser(DatagramIdentifier dg_id, GyroOutputUnit gyro_o, AccOutputUnit acc_o, - InclOutputUnit incl_o, AccRange acc_range) - : is_included_(isIncluded(dg_id)), temp_scale_(tempScale()), aux_scale_(auxScale()) -{ +DatagramParser::DatagramParser(DatagramIdentifier dg_id, GyroOutputUnit gyro_o, + AccOutputUnit acc_o, InclOutputUnit incl_o, + AccRange acc_range) + : is_included_(isIncluded(dg_id)), temp_scale_(tempScale()), + aux_scale_(auxScale()) { setDataScales(gyro_o, acc_o, incl_o, acc_range); } -void DatagramParser::setDataParameters(SensorConfig sensor_config) -{ +void DatagramParser::setDataParameters(SensorConfig sensor_config) { is_included_ = isIncluded(sensor_config.datagram_id); - setDataScales(sensor_config.gyro_output_unit, sensor_config.acc_output_unit, sensor_config.incl_output_unit, - sensor_config.acc_range); + setDataScales(sensor_config.gyro_output_unit, sensor_config.acc_output_unit, + sensor_config.incl_output_unit, sensor_config.acc_range); } -void DatagramParser::setDataScales(GyroOutputUnit gyro_o, AccOutputUnit acc_o, InclOutputUnit incl_o, - AccRange acc_range) -{ - switch (gyro_o) - { - case GyroOutputUnit::ANGULAR_RATE: // units are in rad/s - case GyroOutputUnit::AVERAGE_ANGULAR_RATE: // units are in rad/s - gyro_scale_ = gyroScale(); - break; - case GyroOutputUnit::INCREMENTAL_ANGLE: // units are in rad/sample - case GyroOutputUnit::INTEGRATED_ANGLE: // units are in rad - gyro_scale_ = gyroIncrScale(); - break; +void DatagramParser::setDataScales(GyroOutputUnit gyro_o, AccOutputUnit acc_o, + InclOutputUnit incl_o, AccRange acc_range) { + switch (gyro_o) { + case GyroOutputUnit::ANGULAR_RATE: // units are in rad/s + case GyroOutputUnit::AVERAGE_ANGULAR_RATE: // units are in rad/s + gyro_scale_ = gyroScale(); + break; + case GyroOutputUnit::INCREMENTAL_ANGLE: // units are in rad/sample + case GyroOutputUnit::INTEGRATED_ANGLE: // units are in rad + gyro_scale_ = gyroIncrScale(); + break; } - switch (acc_o) - { - case AccOutputUnit::ACCELERATION: // units are in g - case AccOutputUnit::AVERAGE_ACCELERATION: // units are in g - acc_scale_ = accScale(acc_range); - break; - case AccOutputUnit::INCREMENTAL_VELOCITY: // units are in m/s/sample - case AccOutputUnit::INTEGRATED_VELOCITY: - acc_scale_ = accIncrScale(acc_range); - break; + switch (acc_o) { + case AccOutputUnit::ACCELERATION: // units are in g + case AccOutputUnit::AVERAGE_ACCELERATION: // units are in g + acc_scale_ = accScale(acc_range); + break; + case AccOutputUnit::INCREMENTAL_VELOCITY: // units are in m/s/sample + case AccOutputUnit::INTEGRATED_VELOCITY: + acc_scale_ = accIncrScale(acc_range); + break; } - switch (incl_o) - { - case InclOutputUnit::ACCELERATION: // units are in g - case InclOutputUnit::AVERAGE_ACCELERATION: // units are in g - incl_scale_ = inclScale(); - break; - case InclOutputUnit::INCREMENTAL_VELOCITY: // units are in m/s/sample - case InclOutputUnit::INTEGRATED_VELOCITY: - incl_scale_ = inclIncrScale(); - break; + switch (incl_o) { + case InclOutputUnit::ACCELERATION: // units are in g + case InclOutputUnit::AVERAGE_ACCELERATION: // units are in g + incl_scale_ = inclScale(); + break; + case InclOutputUnit::INCREMENTAL_VELOCITY: // units are in m/s/sample + case InclOutputUnit::INTEGRATED_VELOCITY: + incl_scale_ = inclIncrScale(); + break; } } -uint32_t DatagramParser::parseCRC(std::vector::const_iterator&& itr) -{ +uint32_t DatagramParser::parseCRC(std::vector::const_iterator &&itr) { return parseUnsigned(itr, N_BYTES_CRC); } -uint8_t DatagramParser::parseData(std::vector::const_iterator&& buffer_itr, SensorData& sensor_data) const -{ - uint8_t status{ 0 }; - buffer_itr++; // Skip datagram identifier - if (is_included_[SensorIndx::GYRO]) - { - for (auto& gyro : sensor_data.gyro) - gyro = gyro_scale_ * parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); +uint8_t +DatagramParser::parseData(std::vector::const_iterator &&buffer_itr, + SensorData &sensor_data) const { + uint8_t status{0}; + buffer_itr++; // Skip datagram identifier + if (is_included_[SensorIndx::GYRO]) { + for (auto &gyro : sensor_data.gyro) + gyro = gyro_scale_ * + parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } - if (is_included_[SensorIndx::ACC]) - { - for (auto& acc : sensor_data.acc) - acc = acc_scale_ * parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); + if (is_included_[SensorIndx::ACC]) { + for (auto &acc : sensor_data.acc) + acc = + acc_scale_ * parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } - if (is_included_[SensorIndx::INCL]) - { - for (auto& incl : sensor_data.incl) - incl = incl_scale_ * parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); + if (is_included_[SensorIndx::INCL]) { + for (auto &incl : sensor_data.incl) + incl = incl_scale_ * + parseTwosComplement(buffer_itr, N_BYTES_INERTIAL_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } - if (is_included_[SensorIndx::TEMP]) - { - if (is_included_[SensorIndx::GYRO]) - { - for (auto& temp : sensor_data.temp_gyro) - temp = temp_scale_ * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); + if (is_included_[SensorIndx::TEMP]) { + if (is_included_[SensorIndx::GYRO]) { + for (auto &temp : sensor_data.temp_gyro) + temp = + temp_scale_ * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } - if (is_included_[SensorIndx::ACC]) - { - for (auto& temp : sensor_data.temp_acc) - temp = temp_scale_ * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); + if (is_included_[SensorIndx::ACC]) { + for (auto &temp : sensor_data.temp_acc) + temp = + temp_scale_ * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } - if (is_included_[SensorIndx::INCL]) - { - for (auto& temp : sensor_data.temp_incl) - temp = tempScale() * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); + if (is_included_[SensorIndx::INCL]) { + for (auto &temp : sensor_data.temp_incl) + temp = + tempScale() * parseTwosComplement(buffer_itr, N_BYTES_TEMP_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } } - if (is_included_[SensorIndx::AUX]) - { - sensor_data.aux = aux_scale_ * parseTwosComplement(buffer_itr, N_BYTES_AUX_SENSOR); + if (is_included_[SensorIndx::AUX]) { + sensor_data.aux = + aux_scale_ * parseTwosComplement(buffer_itr, N_BYTES_AUX_SENSOR); status |= parseUnsigned(buffer_itr, N_BYTES_STATUS); } sensor_data.counter = parseUnsigned(buffer_itr, N_BYTES_COUNTER); @@ -114,31 +109,30 @@ uint8_t DatagramParser::parseData(std::vector::const_iterator&& buffer_ return status; } -SensorConfig DatagramParser::parseConfig(std::vector::const_iterator&& buffer_itr) const -{ +SensorConfig DatagramParser::parseConfig( + std::vector::const_iterator &&buffer_itr) const { SensorConfig sensor_config{}; sensor_config.revision = buffer_itr[1]; sensor_config.firmvare_version = buffer_itr[2]; - switch (buffer_itr[3] >> 5u) - { - case 0u: - sensor_config.sample_freq = SampleFreq::S125; - break; - case 1u: - sensor_config.sample_freq = SampleFreq::S250; - break; - case 2u: - sensor_config.sample_freq = SampleFreq::S500; - break; - case 3u: - sensor_config.sample_freq = SampleFreq::S1000; - break; - case 4u: - sensor_config.sample_freq = SampleFreq::S2000; - break; - case 5u: - sensor_config.sample_freq = SampleFreq::TRG; - break; + switch (buffer_itr[3] >> 5u) { + case 0u: + sensor_config.sample_freq = SampleFreq::S125; + break; + case 1u: + sensor_config.sample_freq = SampleFreq::S250; + break; + case 2u: + sensor_config.sample_freq = SampleFreq::S500; + break; + case 3u: + sensor_config.sample_freq = SampleFreq::S1000; + break; + case 4u: + sensor_config.sample_freq = SampleFreq::S2000; + break; + case 5u: + sensor_config.sample_freq = SampleFreq::TRG; + break; } sensor_config.normal_datagram_CRLF = (buffer_itr[3] & (1 << 0)); @@ -150,83 +144,79 @@ SensorConfig DatagramParser::parseConfig(std::vector::const_iterator&& included_sensors[SensorIndx::AUX] = (buffer_itr[3] & (1 << 4)); sensor_config.datagram_id = toDatagramID(included_sensors); - switch (buffer_itr[5] & 0b00001111) - { - case 0: - sensor_config.gyro_output_unit = GyroOutputUnit::ANGULAR_RATE; - break; - case 1: - sensor_config.gyro_output_unit = GyroOutputUnit::INCREMENTAL_ANGLE; - break; - case 2: - sensor_config.gyro_output_unit = GyroOutputUnit::AVERAGE_ANGULAR_RATE; - break; - case 3: - sensor_config.gyro_output_unit = GyroOutputUnit::INTEGRATED_ANGLE; - break; - case 4: - sensor_config.gyro_output_unit = GyroOutputUnit::ANGULAR_RATE; - break; - case 5: - sensor_config.gyro_output_unit = GyroOutputUnit::INCREMENTAL_ANGLE; - break; - case 6: - sensor_config.gyro_output_unit = GyroOutputUnit::AVERAGE_ANGULAR_RATE; - break; - case 7: - sensor_config.gyro_output_unit = GyroOutputUnit::INTEGRATED_ANGLE; - break; + switch (buffer_itr[5] & 0b00001111) { + case 0: + sensor_config.gyro_output_unit = GyroOutputUnit::ANGULAR_RATE; + break; + case 1: + sensor_config.gyro_output_unit = GyroOutputUnit::INCREMENTAL_ANGLE; + break; + case 2: + sensor_config.gyro_output_unit = GyroOutputUnit::AVERAGE_ANGULAR_RATE; + break; + case 3: + sensor_config.gyro_output_unit = GyroOutputUnit::INTEGRATED_ANGLE; + break; + case 4: + sensor_config.gyro_output_unit = GyroOutputUnit::ANGULAR_RATE; + break; + case 5: + sensor_config.gyro_output_unit = GyroOutputUnit::INCREMENTAL_ANGLE; + break; + case 6: + sensor_config.gyro_output_unit = GyroOutputUnit::AVERAGE_ANGULAR_RATE; + break; + case 7: + sensor_config.gyro_output_unit = GyroOutputUnit::INTEGRATED_ANGLE; + break; } - switch (buffer_itr[8] & 0b00001111) - { - case 0: - sensor_config.acc_output_unit = AccOutputUnit::ACCELERATION; - break; - case 1: - sensor_config.acc_output_unit = AccOutputUnit::INCREMENTAL_VELOCITY; - break; - case 2: - sensor_config.acc_output_unit = AccOutputUnit::AVERAGE_ACCELERATION; - break; - case 3: - sensor_config.acc_output_unit = AccOutputUnit::INTEGRATED_VELOCITY; - break; + switch (buffer_itr[8] & 0b00001111) { + case 0: + sensor_config.acc_output_unit = AccOutputUnit::ACCELERATION; + break; + case 1: + sensor_config.acc_output_unit = AccOutputUnit::INCREMENTAL_VELOCITY; + break; + case 2: + sensor_config.acc_output_unit = AccOutputUnit::AVERAGE_ACCELERATION; + break; + case 3: + sensor_config.acc_output_unit = AccOutputUnit::INTEGRATED_VELOCITY; + break; } - switch (buffer_itr[11] & 0b00001111) - { - case 0: - sensor_config.incl_output_unit = InclOutputUnit::ACCELERATION; - break; - case 1: - sensor_config.incl_output_unit = InclOutputUnit::INCREMENTAL_VELOCITY; - break; - case 2: - sensor_config.incl_output_unit = InclOutputUnit::AVERAGE_ACCELERATION; - break; - case 3: - sensor_config.incl_output_unit = InclOutputUnit::INTEGRATED_VELOCITY; - break; + switch (buffer_itr[11] & 0b00001111) { + case 0: + sensor_config.incl_output_unit = InclOutputUnit::ACCELERATION; + break; + case 1: + sensor_config.incl_output_unit = InclOutputUnit::INCREMENTAL_VELOCITY; + break; + case 2: + sensor_config.incl_output_unit = InclOutputUnit::AVERAGE_ACCELERATION; + break; + case 3: + sensor_config.incl_output_unit = InclOutputUnit::INTEGRATED_VELOCITY; + break; } // Assuming all the axes has the same range - switch (buffer_itr[17] >> 4) - { - case 0: - sensor_config.acc_range = AccRange::G10; - break; - case 2: - sensor_config.acc_range = AccRange::G2; - break; - case 3: - sensor_config.acc_range = AccRange::G5; - break; - case 4: - sensor_config.acc_range = AccRange::G30; - break; - case 6: - sensor_config.acc_range = AccRange::G80; - break; + switch (buffer_itr[17] >> 4) { + case 0: + sensor_config.acc_range = AccRange::G10; + break; + case 2: + sensor_config.acc_range = AccRange::G2; + break; + case 3: + sensor_config.acc_range = AccRange::G5; + break; + case 4: + sensor_config.acc_range = AccRange::G30; + break; + case 6: + sensor_config.acc_range = AccRange::G80; + break; } return sensor_config; } diff --git a/src/datagram_parser.h b/src/datagram_parser.h index c9b35bd..2531c91 100644 --- a/src/datagram_parser.h +++ b/src/datagram_parser.h @@ -2,18 +2,16 @@ #ifndef DRIVER_STIM300_DATAGRAM_PARSER_H #define DRIVER_STIM300_DATAGRAM_PARSER_H -#include -#include -#include #include "stim300_constants.h" -#include +#include #include +#include +#include +#include using namespace stim_const; -namespace stim_300 -{ -struct SensorConfig -{ +namespace stim_300 { +struct SensorConfig { char revision; uint8_t firmvare_version; SampleFreq sample_freq; @@ -24,17 +22,18 @@ struct SensorConfig InclOutputUnit incl_output_unit; AccRange acc_range; - inline bool operator!=(const SensorConfig& rhs) - { - return this->sample_freq != rhs.sample_freq or this->datagram_id != rhs.datagram_id or - this->normal_datagram_CRLF != rhs.normal_datagram_CRLF or this->gyro_output_unit != rhs.gyro_output_unit or - this->acc_output_unit != rhs.acc_output_unit or this->incl_output_unit != rhs.incl_output_unit or + inline bool operator!=(const SensorConfig &rhs) { + return this->sample_freq != rhs.sample_freq or + this->datagram_id != rhs.datagram_id or + this->normal_datagram_CRLF != rhs.normal_datagram_CRLF or + this->gyro_output_unit != rhs.gyro_output_unit or + this->acc_output_unit != rhs.acc_output_unit or + this->incl_output_unit != rhs.incl_output_unit or this->acc_range != rhs.acc_range; } }; -struct SensorData -{ +struct SensorData { std::array gyro; std::array acc; std::array incl; @@ -46,14 +45,16 @@ struct SensorData uint16_t latency_us; }; -struct DatagramParser -{ - DatagramParser(DatagramIdentifier dg_id, GyroOutputUnit gyro_o, AccOutputUnit acc_o, InclOutputUnit incl_o, +struct DatagramParser { + DatagramParser(DatagramIdentifier dg_id, GyroOutputUnit gyro_o, + AccOutputUnit acc_o, InclOutputUnit incl_o, AccRange acc_range); void setDataParameters(SensorConfig sensor_config); - static uint32_t parseCRC(std::vector::const_iterator&& itr); - uint8_t parseData(std::vector::const_iterator&& buffer_itr, SensorData& sensor_data) const; - SensorConfig parseConfig(std::vector::const_iterator&& buffer_itr) const; + static uint32_t parseCRC(std::vector::const_iterator &&itr); + uint8_t parseData(std::vector::const_iterator &&buffer_itr, + SensorData &sensor_data) const; + SensorConfig + parseConfig(std::vector::const_iterator &&buffer_itr) const; private: std::array is_included_; @@ -62,36 +63,35 @@ struct DatagramParser double gyro_scale_; double acc_scale_; double incl_scale_; - void setDataScales(GyroOutputUnit gyro_o, AccOutputUnit acc_o, InclOutputUnit incl_o, AccRange acc_range); - // Meta data is stored as "unsigned word", we simply combine the bytes into the right - // sized uint by left shifting them. Note the biggest is the CRC which is 32 bits. - static const uint32_t parseUnsigned(std::vector::const_iterator& it, uint8_t size) - { - uint32_t tmp{ 0 }; - for (int i = 0; i < size; ++i) - { + void setDataScales(GyroOutputUnit gyro_o, AccOutputUnit acc_o, + InclOutputUnit incl_o, AccRange acc_range); + // Meta data is stored as "unsigned word", we simply combine the bytes into + // the right sized uint by left shifting them. Note the biggest is the CRC + // which is 32 bits. + static const uint32_t parseUnsigned(std::vector::const_iterator &it, + uint8_t size) { + uint32_t tmp{0}; + for (int i = 0; i < size; ++i) { tmp = (tmp << 8u) | *(it++); } return tmp; } - // Sensor data is stored as two`s complement, we shift the bytes according to the datasheet, - // bu we shift them to fill up the int32_t type then we shift them back to their right - // position. When we shift them back the shift operator will automatically sign-extend the - // value. - static const int32_t parseTwosComplement(std::vector::const_iterator& it, const uint8_t size) - { + // Sensor data is stored as two`s complement, we shift the bytes according to + // the datasheet, bu we shift them to fill up the int32_t type then we shift + // them back to their right position. When we shift them back the shift + // operator will automatically sign-extend the value. + static const int32_t + parseTwosComplement(std::vector::const_iterator &it, + const uint8_t size) { assert(size == 3 or size == 2); - if (size == 3) - { + if (size == 3) { return ((*it++ << 24) | (*it++ << 16) | (*it++ << 8)) >> 8; - } - else if (size == 2) - { + } else if (size == 2) { return ((*it++ << 24) | (*it++ << 16)) >> 16; } } }; -} // end namespace stim_300 +} // end namespace stim_300 -#endif // DRIVER_STIM300_DATAGRAM_PARSER_H +#endif // DRIVER_STIM300_DATAGRAM_PARSER_H diff --git a/src/driver_stim300.cpp b/src/driver_stim300.cpp index f2cd490..96dc2d8 100644 --- a/src/driver_stim300.cpp +++ b/src/driver_stim300.cpp @@ -1,186 +1,155 @@ -#include #include "driver_stim300.h" +#include -DriverStim300::DriverStim300(SerialDriver& serial_driver, DatagramIdentifier datagram_id, - GyroOutputUnit gyro_output_unit, AccOutputUnit acc_output_unit, - InclOutputUnit incl_output_unit, AccRange acc_range, SampleFreq freq) - : serial_driver_(serial_driver) - , datagram_parser_(datagram_id, gyro_output_unit, acc_output_unit, incl_output_unit, acc_range) - , datagram_id_(datagramIdentifierToRaw(datagram_id)) - , crc_dummy_bytes_(numberOfPaddingBytes(datagram_id)) - , datagram_size_(calculateDatagramSize(datagram_id)) - , sensor_config_{ '0', 0, freq, datagram_id, false, gyro_output_unit, acc_output_unit, incl_output_unit, acc_range } -{ -} +DriverStim300::DriverStim300(SerialDriver &serial_driver, + DatagramIdentifier datagram_id, + GyroOutputUnit gyro_output_unit, + AccOutputUnit acc_output_unit, + InclOutputUnit incl_output_unit, + AccRange acc_range, SampleFreq freq) + : serial_driver_(serial_driver), + datagram_parser_(datagram_id, gyro_output_unit, acc_output_unit, + incl_output_unit, acc_range), + datagram_id_(datagramIdentifierToRaw(datagram_id)), + crc_dummy_bytes_(numberOfPaddingBytes(datagram_id)), + datagram_size_(calculateDatagramSize(datagram_id)), sensor_config_{ + '0', + 0, + freq, + datagram_id, + false, + gyro_output_unit, + acc_output_unit, + incl_output_unit, + acc_range} {} -DriverStim300::DriverStim300(SerialDriver& serial_driver) - : DriverStim300(serial_driver, DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, GyroOutputUnit::AVERAGE_ANGULAR_RATE, - AccOutputUnit::AVERAGE_ACCELERATION, InclOutputUnit::AVERAGE_ACCELERATION, AccRange::G5, - SampleFreq::S125) -{ -} -double DriverStim300::getAccX() const noexcept -{ - return sensor_data_.acc[0]; -} -double DriverStim300::getAccY() const noexcept -{ - return sensor_data_.acc[1]; -} -double DriverStim300::getAccZ() const noexcept -{ - return sensor_data_.acc[2]; -} -double DriverStim300::getGyroX() const noexcept -{ - return sensor_data_.gyro[0]; -} -double DriverStim300::getGyroY() const noexcept -{ - return sensor_data_.gyro[1]; -} -double DriverStim300::getGyroZ() const noexcept -{ - return sensor_data_.gyro[2]; -} -double DriverStim300::getIncX() const noexcept -{ - return sensor_data_.incl[0]; -} -double DriverStim300::getIncY() const noexcept -{ - return sensor_data_.incl[1]; -} -double DriverStim300::getIncZ() const noexcept -{ - return sensor_data_.incl[2]; -} -uint16_t DriverStim300::getSampleRate() const noexcept -{ +DriverStim300::DriverStim300(SerialDriver &serial_driver) + : DriverStim300(serial_driver, DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, + GyroOutputUnit::AVERAGE_ANGULAR_RATE, + AccOutputUnit::AVERAGE_ACCELERATION, + InclOutputUnit::AVERAGE_ACCELERATION, AccRange::G5, + SampleFreq::S125) {} +double DriverStim300::getAccX() const noexcept { return sensor_data_.acc[0]; } +double DriverStim300::getAccY() const noexcept { return sensor_data_.acc[1]; } +double DriverStim300::getAccZ() const noexcept { return sensor_data_.acc[2]; } +double DriverStim300::getGyroX() const noexcept { return sensor_data_.gyro[0]; } +double DriverStim300::getGyroY() const noexcept { return sensor_data_.gyro[1]; } +double DriverStim300::getGyroZ() const noexcept { return sensor_data_.gyro[2]; } +double DriverStim300::getIncX() const noexcept { return sensor_data_.incl[0]; } +double DriverStim300::getIncY() const noexcept { return sensor_data_.incl[1]; } +double DriverStim300::getIncZ() const noexcept { return sensor_data_.incl[2]; } +uint16_t DriverStim300::getSampleRate() const noexcept { return stim_const::sampleFreq2int(sensor_config_.sample_freq); } -uint16_t DriverStim300::getLatency_us() const noexcept -{ +uint16_t DriverStim300::getLatency_us() const noexcept { return sensor_data_.latency_us; } -bool DriverStim300::isSensorStatusGood() const noexcept -{ +bool DriverStim300::isSensorStatusGood() const noexcept { return sensor_status_ == 0; } -uint8_t DriverStim300::getInternalMeasurementCounter() const noexcept -{ +uint8_t DriverStim300::getInternalMeasurementCounter() const noexcept { return sensor_data_.counter; } -double DriverStim300::getAverageTemp() const noexcept -{ - double sum{ 0 }; - uint8_t count{ 0 }; +double DriverStim300::getAverageTemp() const noexcept { + double sum{0}; + uint8_t count{0}; return count != 0 ? sum / count : std::numeric_limits::quiet_NaN(); } -Stim300Status DriverStim300::readDataStream() -{ +Stim300Status DriverStim300::readDataStream() { // Read stream to identify the start of a datagram. // If no datagram is identified after 100 bytes, ask for a config datagram. // Read number of bytes belonging to the current datagram. // Verify datagram (CRLF and CRC). // Parse Datagram. uint8_t byte; - while (serial_driver_.readByte(byte)) - { - switch (reading_mode_) - { - case ReadingMode::IdentifyingDatagram: - if (byte == datagram_id_) - { - // Use current datagram format - } - else if (byte == datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION)) - { - setDatagramFormat(DatagramIdentifier::CONFIGURATION); + while (serial_driver_.readByte(byte)) { + switch (reading_mode_) { + case ReadingMode::IdentifyingDatagram: + if (byte == datagram_id_) { + // Use current datagram format + } else if (byte == + datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION)) { + setDatagramFormat(DatagramIdentifier::CONFIGURATION); + } else if (byte == datagramIdentifierToRaw( + DatagramIdentifier::CONFIGURATION_CRLF)) { + setDatagramFormat(DatagramIdentifier::CONFIGURATION_CRLF); + } else { + if (++n_checked_bytes > 100) { + // std::cerr << "Not able to recognise datagram" << std::endl; + if (read_config_from_sensor_) + askForConfigDatagram(); + n_checked_bytes = 0; } - else if (byte == datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION_CRLF)) - { - setDatagramFormat(DatagramIdentifier::CONFIGURATION_CRLF); - } - else - { - if (++n_checked_bytes > 100) - { - // std::cerr << "Not able to recognise datagram" << std::endl; - if (read_config_from_sensor_) askForConfigDatagram(); - n_checked_bytes = 0; - } - continue; - } - // if (n_checked_bytes != 0) - // std::cout << "Checked bytes: " << n_checked_bytes << std::endl; - n_checked_bytes = 0; - reading_mode_ = ReadingMode::ReadingDatagram; - buffer_.push_back(byte); - n_new_bytes_ = 1; continue; + } + // if (n_checked_bytes != 0) + // std::cout << "Checked bytes: " << n_checked_bytes << std::endl; + n_checked_bytes = 0; + reading_mode_ = ReadingMode::ReadingDatagram; + buffer_.push_back(byte); + n_new_bytes_ = 1; + continue; - case ReadingMode::ReadingDatagram: + case ReadingMode::ReadingDatagram: - // Circular buffer - buffer_.push_back(byte); - n_new_bytes_++; - while (buffer_.size() > datagram_size_) - buffer_.erase(buffer_.begin()); + // Circular buffer + buffer_.push_back(byte); + n_new_bytes_++; + while (buffer_.size() > datagram_size_) + buffer_.erase(buffer_.begin()); - if (n_new_bytes_ < datagram_size_) - continue; + if (n_new_bytes_ < datagram_size_) + continue; - // else the buffer is filled with a potential new datagram + // else the buffer is filled with a potential new datagram - if (sensor_config_.normal_datagram_CRLF or - datagram_id_ == datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION_CRLF)) - { - reading_mode_ = ReadingMode::VerifyingDatagramCR; - continue; - } - break; + if (sensor_config_.normal_datagram_CRLF or + datagram_id_ == + datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION_CRLF)) { + reading_mode_ = ReadingMode::VerifyingDatagramCR; + continue; + } + break; - case ReadingMode::VerifyingDatagramCR: + case ReadingMode::VerifyingDatagramCR: - if (byte == 0x0D) - { - reading_mode_ = ReadingMode::VerifyingDatagramLF; - continue; - } - reading_mode_ = ReadingMode::IdentifyingDatagram; - return Stim300Status::NORMAL; + if (byte == 0x0D) { + reading_mode_ = ReadingMode::VerifyingDatagramLF; + continue; + } + reading_mode_ = ReadingMode::IdentifyingDatagram; + return Stim300Status::NORMAL; - case ReadingMode::VerifyingDatagramLF: + case ReadingMode::VerifyingDatagramLF: - if (byte == 0x0A) - { - break; - } - reading_mode_ = ReadingMode::IdentifyingDatagram; - return Stim300Status::NORMAL; - } // end reading mode switch + if (byte == 0x0A) { + break; + } + reading_mode_ = ReadingMode::IdentifyingDatagram; + return Stim300Status::NORMAL; + } // end reading mode switch - if (!verifyChecksum(buffer_.cbegin(), buffer_.cend(), crc_dummy_bytes_)) - { + if (!verifyChecksum(buffer_.cbegin(), buffer_.cend(), crc_dummy_bytes_)) { // The "ID" was likely a byte happening to be equal the datagram id, // and not actually the start of a datagram, thus the buffer does // not contain a complete datagram. - //std::cerr << "CRC error" << std::endl; + // std::cerr << "CRC error" << std::endl; reading_mode_ = ReadingMode::IdentifyingDatagram; return Stim300Status::NORMAL; } - if (datagram_id_ == datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION_CRLF) or - datagram_id_ == datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION)) - { - stim_300::SensorConfig sensor_config = datagram_parser_.parseConfig(buffer_.cbegin()); - Stim300Status status{ Stim300Status::NORMAL }; + if (datagram_id_ == + datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION_CRLF) or + datagram_id_ == + datagramIdentifierToRaw(DatagramIdentifier::CONFIGURATION)) { + stim_300::SensorConfig sensor_config = + datagram_parser_.parseConfig(buffer_.cbegin()); + Stim300Status status{Stim300Status::NORMAL}; if (sensor_config != sensor_config_) status = Stim300Status::CONFIG_CHANGED; @@ -188,13 +157,13 @@ Stim300Status DriverStim300::readDataStream() setDatagramFormat(sensor_config_.datagram_id); datagram_parser_.setDataParameters(sensor_config_); read_config_from_sensor_ = false; - //std::cout << "Parsed config datagram" << std::endl; + // std::cout << "Parsed config datagram" << std::endl; reading_mode_ = ReadingMode::IdentifyingDatagram; return status; - } - else // Normal (measurement) datagram + } else // Normal (measurement) datagram { - sensor_status_ = datagram_parser_.parseData(buffer_.cbegin(), sensor_data_); + sensor_status_ = + datagram_parser_.parseData(buffer_.cbegin(), sensor_data_); reading_mode_ = ReadingMode::IdentifyingDatagram; if (sensor_status_ == 0) @@ -216,44 +185,42 @@ Stim300Status DriverStim300::readDataStream() return Stim300Status::NORMAL; } -void DriverStim300::askForConfigDatagram() -{ +void DriverStim300::askForConfigDatagram() { serial_driver_.writeByte('C'); serial_driver_.writeByte('\r'); } -Stim300Status DriverStim300::update() noexcept -{ - switch (mode_) - { - case Mode::Init: - serial_driver_.flush(); - if (read_config_from_sensor_) - askForConfigDatagram(); - mode_ = Mode::Normal; - case Mode::Normal: - return readDataStream(); - case Mode::Service: - // std::string s(buffer_.begin(), buffer_.end()); - // std::cout << s << "\n"; - mode_ = Mode::Normal; - return Stim300Status::NORMAL; +Stim300Status DriverStim300::update() noexcept { + switch (mode_) { + case Mode::Init: + serial_driver_.flush(); + if (read_config_from_sensor_) + askForConfigDatagram(); + mode_ = Mode::Normal; + case Mode::Normal: + return readDataStream(); + case Mode::Service: + // std::string s(buffer_.begin(), buffer_.end()); + // std::cout << s << "\n"; + mode_ = Mode::Normal; + return Stim300Status::NORMAL; } } -bool DriverStim300::setDatagramFormat(DatagramIdentifier id) -{ +bool DriverStim300::setDatagramFormat(DatagramIdentifier id) { datagram_id_ = datagramIdentifierToRaw(id); datagram_size_ = calculateDatagramSize(id); crc_dummy_bytes_ = numberOfPaddingBytes(id); } -bool DriverStim300::verifyChecksum(const std::vector::const_iterator& begin, - const std::vector::const_iterator& end, const uint8_t& crc_dummy_bytes) -{ +bool DriverStim300::verifyChecksum( + const std::vector::const_iterator &begin, + const std::vector::const_iterator &end, + const uint8_t &crc_dummy_bytes) { uint32_t crc = stim_300::DatagramParser::parseCRC(end - sizeof(uint32_t)); - boost::crc_basic<32> crc_32_calculator(0x04C11DB7, 0xFFFFFFFF, 0x00, false, false); + boost::crc_basic<32> crc_32_calculator(0x04C11DB7, 0xFFFFFFFF, 0x00, false, + false); uint8_t buffer_CRC[end - begin - sizeof(uint32_t) + crc_dummy_bytes]; std::copy(begin, end - sizeof(uint32_t) + crc_dummy_bytes, buffer_CRC); @@ -266,31 +233,30 @@ bool DriverStim300::verifyChecksum(const std::vector::const_iterator& b return crc_calck == crc; } -std::string DriverStim300::printSensorConfig() const noexcept -{ +std::string DriverStim300::printSensorConfig() const noexcept { std::stringstream ss; - ss << "\nFirmware: " << sensor_config_.revision << std::to_string(sensor_config_.firmvare_version) << std::endl; + ss << "\nFirmware: " << sensor_config_.revision + << std::to_string(sensor_config_.firmvare_version) << std::endl; ss << "Sample_freq: "; - switch (sensor_config_.sample_freq) - { - case SampleFreq::S125: - ss << "125 Hz"; - break; - case SampleFreq::S250: - ss << "250 Hz"; - break; - case SampleFreq::S500: - ss << "500 Hz"; - break; - case SampleFreq::S1000: - ss << "1000 Hz"; - break; - case SampleFreq::S2000: - ss << "2000 Hz"; - break; - case SampleFreq::TRG: - ss << "External Trigger"; - break; + switch (sensor_config_.sample_freq) { + case SampleFreq::S125: + ss << "125 Hz"; + break; + case SampleFreq::S250: + ss << "250 Hz"; + break; + case SampleFreq::S500: + ss << "500 Hz"; + break; + case SampleFreq::S1000: + ss << "1000 Hz"; + break; + case SampleFreq::S2000: + ss << "2000 Hz"; + break; + case SampleFreq::TRG: + ss << "External Trigger"; + break; } ss << std::endl; auto included_sensors = isIncluded(sensor_config_.datagram_id); @@ -299,76 +265,73 @@ std::string DriverStim300::printSensorConfig() const noexcept ss << "Inlcinometer:\t" << included_sensors[SensorIndx::INCL] << std::endl; ss << "Temprature:\t\t" << included_sensors[SensorIndx::TEMP] << std::endl; ss << "Aux:\t\t\t" << included_sensors[SensorIndx::AUX] << std::endl; - ss << "Normal Datagram termination: " << sensor_config_.normal_datagram_CRLF << std::endl; + ss << "Normal Datagram termination: " << sensor_config_.normal_datagram_CRLF + << std::endl; ss << "Gyro output:\t\t\t"; - switch (sensor_config_.gyro_output_unit) - { - case GyroOutputUnit::ANGULAR_RATE: - ss << "Angular rate"; - break; - case GyroOutputUnit::AVERAGE_ANGULAR_RATE: - ss << "Average angular rate"; - break; - case GyroOutputUnit::INCREMENTAL_ANGLE: - ss << "Incremental angle"; - break; - case GyroOutputUnit::INTEGRATED_ANGLE: - ss << "Integrated angle"; - break; + switch (sensor_config_.gyro_output_unit) { + case GyroOutputUnit::ANGULAR_RATE: + ss << "Angular rate"; + break; + case GyroOutputUnit::AVERAGE_ANGULAR_RATE: + ss << "Average angular rate"; + break; + case GyroOutputUnit::INCREMENTAL_ANGLE: + ss << "Incremental angle"; + break; + case GyroOutputUnit::INTEGRATED_ANGLE: + ss << "Integrated angle"; + break; } ss << std::endl; ss << "Accelerometer output:\t"; - switch (sensor_config_.acc_output_unit) - { - case AccOutputUnit::ACCELERATION: - ss << "Acceleration"; - break; - case AccOutputUnit::AVERAGE_ACCELERATION: - ss << "Average acceleration"; - break; - case AccOutputUnit::INCREMENTAL_VELOCITY: - ss << "Incremental velocity"; - break; - case AccOutputUnit::INTEGRATED_VELOCITY: - ss << "Integrated velocity"; - break; + switch (sensor_config_.acc_output_unit) { + case AccOutputUnit::ACCELERATION: + ss << "Acceleration"; + break; + case AccOutputUnit::AVERAGE_ACCELERATION: + ss << "Average acceleration"; + break; + case AccOutputUnit::INCREMENTAL_VELOCITY: + ss << "Incremental velocity"; + break; + case AccOutputUnit::INTEGRATED_VELOCITY: + ss << "Integrated velocity"; + break; } ss << std::endl; ss << "Inclinometer output:\t"; - switch (sensor_config_.incl_output_unit) - { - case InclOutputUnit::ACCELERATION: - ss << "Acceleration"; - break; - case InclOutputUnit::AVERAGE_ACCELERATION: - ss << "Average acceleration"; - break; - case InclOutputUnit::INCREMENTAL_VELOCITY: - ss << "Incremental velocity"; - break; - case InclOutputUnit::INTEGRATED_VELOCITY: - ss << "Integrated velocity"; - break; + switch (sensor_config_.incl_output_unit) { + case InclOutputUnit::ACCELERATION: + ss << "Acceleration"; + break; + case InclOutputUnit::AVERAGE_ACCELERATION: + ss << "Average acceleration"; + break; + case InclOutputUnit::INCREMENTAL_VELOCITY: + ss << "Incremental velocity"; + break; + case InclOutputUnit::INTEGRATED_VELOCITY: + ss << "Integrated velocity"; + break; } ss << std::endl; ss << "Acceleration range: "; - switch (sensor_config_.acc_range) - { - case AccRange::G2: - ss << "2"; - break; - case AccRange::G5: - ss << "5"; - break; - case AccRange::G10: - ss << "10"; - break; - case AccRange::G30: - ss << "30"; - break; - case AccRange::G80: - ss << "80"; - break; + switch (sensor_config_.acc_range) { + case AccRange::G2: + ss << "2"; + break; + case AccRange::G5: + ss << "5"; + break; + case AccRange::G10: + ss << "10"; + break; + case AccRange::G30: + ss << "30"; + break; + case AccRange::G80: + ss << "80"; + break; } ss << " g." << std::endl; return ss.str(); diff --git a/src/serial_driver.h b/src/serial_driver.h index c08429d..b7c9b0f 100644 --- a/src/serial_driver.h +++ b/src/serial_driver.h @@ -2,13 +2,14 @@ #ifndef DRIVER_STIM300_SERIAL_DRIVER_H #define DRIVER_STIM300_SERIAL_DRIVER_H -class SerialDriver -{ +#include + +class SerialDriver { public: virtual ~SerialDriver() = default; - virtual bool readByte(uint8_t& byte) = 0; + virtual bool readByte(uint8_t &byte) = 0; virtual bool writeByte(uint8_t byte) = 0; virtual bool flush() = 0; }; -#endif // DRIVER_STIM300_SERIAL_DRIVER_H +#endif // DRIVER_STIM300_SERIAL_DRIVER_H diff --git a/src/serial_unix.cpp b/src/serial_unix.cpp index cb55a9d..c450976 100644 --- a/src/serial_unix.cpp +++ b/src/serial_unix.cpp @@ -3,39 +3,35 @@ #include // Everything is learned from // https://en.wikibooks.org/wiki/Serial_Programming/termios -// "termios is the newer (now already a few decades old) Unix API for terminal I/O" +// "termios is the newer (now already a few decades old) Unix API for terminal +// I/O" -SerialUnix::SerialUnix(const std::string& serial_port_name, stim_const::BaudRate baudrate) -{ +SerialUnix::SerialUnix(const std::string &serial_port_name, + stim_const::BaudRate baudrate) { open(serial_port_name, baudrate); } -SerialUnix::~SerialUnix() -{ - close(); -} +SerialUnix::~SerialUnix() { close(); } -void SerialUnix::open(const std::string& serial_port_name, stim_const::BaudRate baudrate) -{ +void SerialUnix::open(const std::string &serial_port_name, + stim_const::BaudRate baudrate) { file_handle_ = ::open(serial_port_name.data(), O_RDWR | O_NOCTTY | O_NDELAY); - if (file_handle_ < 0) - { - throw std::runtime_error{ std::string{ strerror(errno) } + ": " + serial_port_name }; + if (file_handle_ < 0) { + throw std::runtime_error{std::string{strerror(errno)} + ": " + + serial_port_name}; } // // Check if the file descriptor is pointing to a TTY device or not. // - if (!isatty(file_handle_)) - { - throw std::runtime_error{ "Serial port is not TTY device" }; + if (!isatty(file_handle_)) { + throw std::runtime_error{"Serial port is not TTY device"}; } // // Get the current configuration of the serial interface // - if (tcgetattr(file_handle_, &config_) < 0) - { - throw std::runtime_error{ "Could not retrive current serial config" }; + if (tcgetattr(file_handle_, &config_) < 0) { + throw std::runtime_error{"Could not retrive current serial config"}; } // @@ -46,7 +42,8 @@ void SerialUnix::open(const std::string& serial_port_name, stim_const::BaudRate // no input parity check, don't strip high bit off, // no XON/XOFF software flow control // - config_.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + config_.c_iflag &= + ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); // // Output flags - Turn off output processing @@ -89,63 +86,54 @@ void SerialUnix::open(const std::string& serial_port_name, stim_const::BaudRate // constants) // bool failure; - switch (baudrate) - { - case stim_const::BaudRate::BAUD_377400: - failure = (cfsetispeed(&config_, 377400) < 0 || cfsetospeed(&config_, 377400) < 0); - break; - case stim_const::BaudRate::BAUD_460800: - failure = (cfsetispeed(&config_, B460800) < 0 || cfsetospeed(&config_, B460800) < 0); - break; - case stim_const::BaudRate::BAUD_921600: - failure = (cfsetispeed(&config_, B921600) < 0 || cfsetospeed(&config_, B921600) < 0); - break; - case stim_const::BaudRate::BAUD_1843200: // 921600: - failure = (cfsetispeed(&config_, 1843200) < 0 || cfsetospeed(&config_, 1843200) < 0); - break; - default: - failure = true; + switch (baudrate) { + case stim_const::BaudRate::BAUD_377400: + failure = (cfsetispeed(&config_, 377400) < 0 || + cfsetospeed(&config_, 377400) < 0); + break; + case stim_const::BaudRate::BAUD_460800: + failure = (cfsetispeed(&config_, B460800) < 0 || + cfsetospeed(&config_, B460800) < 0); + break; + case stim_const::BaudRate::BAUD_921600: + failure = (cfsetispeed(&config_, B921600) < 0 || + cfsetospeed(&config_, B921600) < 0); + break; + case stim_const::BaudRate::BAUD_1843200: // 921600: + failure = (cfsetispeed(&config_, 1843200) < 0 || + cfsetospeed(&config_, 1843200) < 0); + break; + default: + failure = true; } - if (failure) - { - throw std::runtime_error{ "Could not set baudrate" }; + if (failure) { + throw std::runtime_error{"Could not set baudrate"}; } // // Finally, apply the configuration // - if (tcsetattr(file_handle_, TCSAFLUSH, &config_) != 0) - { - throw std::runtime_error{ strerror(errno) }; + if (tcsetattr(file_handle_, TCSAFLUSH, &config_) != 0) { + throw std::runtime_error{strerror(errno)}; } } -void SerialUnix::close() -{ - ::close(file_handle_); -} +void SerialUnix::close() { ::close(file_handle_); } -bool SerialUnix::writeByte(uint8_t byte) -{ +bool SerialUnix::writeByte(uint8_t byte) { int result = ::write(file_handle_, &byte, 1); - if (result == -1) - { - throw std::runtime_error{ "WriteByte error:" + std::string{ strerror(errno) } }; + if (result == -1) { + throw std::runtime_error{"WriteByte error:" + std::string{strerror(errno)}}; } return result == 1; } -bool SerialUnix::readByte(uint8_t& byte) -{ +bool SerialUnix::readByte(uint8_t &byte) { int result = read(file_handle_, &byte, 1); - if (result == -1) - { - throw std::runtime_error{ "ReadByte error:" + std::string{ strerror(errno) } }; + if (result == -1) { + throw std::runtime_error{"ReadByte error:" + std::string{strerror(errno)}}; } return (result == 1); } -bool SerialUnix::flush() -{ - return tcflush(file_handle_, TCIOFLUSH) == 0; -} \ No newline at end of file +bool SerialUnix::flush() { return tcflush(file_handle_, TCIOFLUSH) == 0; } \ No newline at end of file diff --git a/src/serial_unix.h b/src/serial_unix.h index ada9689..f22d656 100644 --- a/src/serial_unix.h +++ b/src/serial_unix.h @@ -1,38 +1,39 @@ #ifndef DRIVER_STIM300_SERIAL_UBUNTU_H #define DRIVER_STIM300_SERIAL_UBUNTU_H -#include #include "serial_driver.h" -#include #include +#include +#include #include +#include "stim300_constants.h" #include #include -#include "stim300_constants.h" +#include -class SerialUnix : public SerialDriver -{ +class SerialUnix : public SerialDriver { public: - SerialUnix(const std::string& serial_port_name, stim_const::BaudRate baudrate); + SerialUnix(const std::string &serial_port_name, + stim_const::BaudRate baudrate); ~SerialUnix() final; // The class is Non-Copyable - SerialUnix(const SerialUnix& a) = delete; - SerialUnix& operator=(const SerialUnix& a) = delete; + SerialUnix(const SerialUnix &a) = delete; + SerialUnix &operator=(const SerialUnix &a) = delete; // The class is non-movable - SerialUnix(SerialUnix&& a) = delete; - SerialUnix& operator=(SerialUnix&& a) = delete; + SerialUnix(SerialUnix &&a) = delete; + SerialUnix &operator=(SerialUnix &&a) = delete; bool writeByte(uint8_t byte) final; - bool readByte(uint8_t& byte) final; + bool readByte(uint8_t &byte) final; bool flush() final; private: - void open(const std::string& serial_port_name, stim_const::BaudRate baudrate); + void open(const std::string &serial_port_name, stim_const::BaudRate baudrate); void close(); int file_handle_; struct termios config_; }; -#endif // DRIVER_STIM300_SERIAL_UBUNTU_H +#endif // DRIVER_STIM300_SERIAL_UBUNTU_H diff --git a/src/stim300_constants.h b/src/stim300_constants.h index 67bf7b4..78bd4ec 100644 --- a/src/stim300_constants.h +++ b/src/stim300_constants.h @@ -1,12 +1,11 @@ #ifndef DRIVER_STIM300_STIM300_CONSTANTS_H #define DRIVER_STIM300_STIM300_CONSTANTS_H +#include #include #include -#include -namespace stim_const -{ +namespace stim_const { static constexpr uint8_t N_BYTES_DATAGRAM_ID = 1; static constexpr uint8_t N_BYTES_INERTIAL_SENSOR = 3; static constexpr uint8_t N_BYTES_TEMP_SENSOR = 2; @@ -18,8 +17,7 @@ static constexpr uint8_t N_BYTES_STATUS = 1; static constexpr uint8_t N_BYTES_TERMINATION = 2; static constexpr uint8_t MAX_DATAGRAM_SIZE = 65; -enum class DatagramIdentifier -{ +enum class DatagramIdentifier { RATE, RATE_ACC, RATE_INCL, @@ -40,59 +38,38 @@ enum class DatagramIdentifier CONFIGURATION_CRLF }; -enum class AccRange -{ - G2, - G5, - G10, - G30, - G80 -}; +enum class AccRange { G2, G5, G10, G30, G80 }; -enum class BaudRate -{ // defined as bit-rate in datasheet +enum class BaudRate { // defined as bit-rate in datasheet BAUD_377400, BAUD_460800, BAUD_921600, BAUD_1843200, }; -enum class GyroOutputUnit -{ +enum class GyroOutputUnit { ANGULAR_RATE, AVERAGE_ANGULAR_RATE, INCREMENTAL_ANGLE, INTEGRATED_ANGLE }; -enum class AccOutputUnit -{ +enum class AccOutputUnit { ACCELERATION, AVERAGE_ACCELERATION, INCREMENTAL_VELOCITY, INTEGRATED_VELOCITY }; -enum class InclOutputUnit -{ +enum class InclOutputUnit { ACCELERATION, AVERAGE_ACCELERATION, INCREMENTAL_VELOCITY, INTEGRATED_VELOCITY }; -enum class SampleFreq -{ - S125, - S250, - S500, - S1000, - S2000, - TRG -}; +enum class SampleFreq { S125, S250, S500, S1000, S2000, TRG }; -static constexpr uint16_t sampleFreq2int(const SampleFreq& sample_freq) -{ - switch (sample_freq) - { +static constexpr uint16_t sampleFreq2int(const SampleFreq &sample_freq) { + switch (sample_freq) { case SampleFreq::S125: return 125; case SampleFreq::S250: @@ -108,119 +85,152 @@ static constexpr uint16_t sampleFreq2int(const SampleFreq& sample_freq) } } -enum SensorIndx -{ - GYRO = 0, - ACC, - INCL, - TEMP, - AUX -}; +enum SensorIndx { GYRO = 0, ACC, INCL, TEMP, AUX }; -struct DatagramInfo -{ +struct DatagramInfo { DatagramIdentifier id; uint8_t raw_id; std::array included_sensors; uint8_t number_of_padding_bytes; }; -static constexpr std::array datagram_info_map // rate, acc, incl, temp, aux - { { - { DatagramIdentifier::RATE, 0x90, { true, false, false, false, false }, 2 }, - { DatagramIdentifier::RATE_ACC, 0x91, { true, true, false, false, false }, 0 }, - { DatagramIdentifier::RATE_INCL, 0x92, { true, false, true, false, false }, 0 }, - { DatagramIdentifier::RATE_ACC_INCL, 0x93, { true, true, true, false, false }, 2 }, - { DatagramIdentifier::RATE_TEMP, 0x94, { true, false, false, true, false }, 3 }, - { DatagramIdentifier::RATE_ACC_TEMP, 0xA5, { true, true, false, true, false }, 2 }, - { DatagramIdentifier::RATE_INCL_TEMP, 0xA6, { true, false, true, true, false }, 2 }, - { DatagramIdentifier::RATE_ACC_INCL_TEMP, 0xA7, { true, true, true, true, false }, 1 }, - { DatagramIdentifier::RATE_AUX, 0x98, { true, false, false, false, true }, 2 }, - { DatagramIdentifier::RATE_ACC_AUX, 0x99, { true, true, false, false, true }, 0 }, - { DatagramIdentifier::RATE_INCL_AUX, 0x9A, { true, false, true, false, true }, 0 }, - { DatagramIdentifier::RATE_ACC_INCL_AUX, 0x9B, { true, true, true, false, true }, 2 }, - { DatagramIdentifier::RATE_TEMP_AUX, 0x9C, { true, false, false, true, true }, 3 }, - { DatagramIdentifier::RATE_ACC_TEMP_AUX, 0xAD, { true, true, false, true, true }, 2 }, - { DatagramIdentifier::RATE_INCL_TEMP_AUX, 0xAE, { true, false, true, true, true }, 2 }, - { DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, 0xAF, { true, true, true, true, true }, 1 }, - { DatagramIdentifier::CONFIGURATION, 0xBC, { false, false, false, false, false }, 2 }, - { DatagramIdentifier::CONFIGURATION_CRLF, 0xBD, { false, false, false, false, false }, 2 }, - } }; +static constexpr std::array + datagram_info_map // rate, acc, incl, temp, aux + {{ + {DatagramIdentifier::RATE, 0x90, {true, false, false, false, false}, 2}, + {DatagramIdentifier::RATE_ACC, + 0x91, + {true, true, false, false, false}, + 0}, + {DatagramIdentifier::RATE_INCL, + 0x92, + {true, false, true, false, false}, + 0}, + {DatagramIdentifier::RATE_ACC_INCL, + 0x93, + {true, true, true, false, false}, + 2}, + {DatagramIdentifier::RATE_TEMP, + 0x94, + {true, false, false, true, false}, + 3}, + {DatagramIdentifier::RATE_ACC_TEMP, + 0xA5, + {true, true, false, true, false}, + 2}, + {DatagramIdentifier::RATE_INCL_TEMP, + 0xA6, + {true, false, true, true, false}, + 2}, + {DatagramIdentifier::RATE_ACC_INCL_TEMP, + 0xA7, + {true, true, true, true, false}, + 1}, + {DatagramIdentifier::RATE_AUX, + 0x98, + {true, false, false, false, true}, + 2}, + {DatagramIdentifier::RATE_ACC_AUX, + 0x99, + {true, true, false, false, true}, + 0}, + {DatagramIdentifier::RATE_INCL_AUX, + 0x9A, + {true, false, true, false, true}, + 0}, + {DatagramIdentifier::RATE_ACC_INCL_AUX, + 0x9B, + {true, true, true, false, true}, + 2}, + {DatagramIdentifier::RATE_TEMP_AUX, + 0x9C, + {true, false, false, true, true}, + 3}, + {DatagramIdentifier::RATE_ACC_TEMP_AUX, + 0xAD, + {true, true, false, true, true}, + 2}, + {DatagramIdentifier::RATE_INCL_TEMP_AUX, + 0xAE, + {true, false, true, true, true}, + 2}, + {DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, + 0xAF, + {true, true, true, true, true}, + 1}, + {DatagramIdentifier::CONFIGURATION, + 0xBC, + {false, false, false, false, false}, + 2}, + {DatagramIdentifier::CONFIGURATION_CRLF, + 0xBD, + {false, false, false, false, false}, + 2}, + }}; -constexpr uint8_t datagramIdentifierToRaw(DatagramIdentifier d_id) -{ - for (int i = 0; i < datagram_info_map.size(); ++i) - { - if (datagram_info_map[i].id == d_id) - { +constexpr uint8_t datagramIdentifierToRaw(DatagramIdentifier d_id) { + for (int i = 0; i < datagram_info_map.size(); ++i) { + if (datagram_info_map[i].id == d_id) { return datagram_info_map[i].raw_id; } } } -constexpr DatagramIdentifier rawToDatagramIdentifier(uint8_t datagram_id) -{ - for (int i = 0; i < datagram_info_map.size(); ++i) - { - if (datagram_info_map[i].raw_id == datagram_id) - { +constexpr DatagramIdentifier rawToDatagramIdentifier(uint8_t datagram_id) { + for (int i = 0; i < datagram_info_map.size(); ++i) { + if (datagram_info_map[i].raw_id == datagram_id) { return datagram_info_map[i].id; } } - return DatagramIdentifier::CONFIGURATION_CRLF; // Todo: implement error handeling + return DatagramIdentifier::CONFIGURATION_CRLF; // Todo: implement error + // handeling }; -constexpr uint8_t numberOfPaddingBytes(DatagramIdentifier datagram_identifier) -{ - for (int i = 0; i < datagram_info_map.size(); ++i) - { - if (datagram_info_map[i].id == datagram_identifier) - { +constexpr uint8_t numberOfPaddingBytes(DatagramIdentifier datagram_identifier) { + for (int i = 0; i < datagram_info_map.size(); ++i) { + if (datagram_info_map[i].id == datagram_identifier) { return datagram_info_map[i].number_of_padding_bytes; } } } -constexpr std::array isIncluded(DatagramIdentifier datagram_identifier) -{ - for (int i = 0; i < datagram_info_map.size(); ++i) - { - if (datagram_info_map[i].id == datagram_identifier) - { +constexpr std::array +isIncluded(DatagramIdentifier datagram_identifier) { + for (int i = 0; i < datagram_info_map.size(); ++i) { + if (datagram_info_map[i].id == datagram_identifier) { return datagram_info_map[i].included_sensors; } } } -static DatagramIdentifier toDatagramID(std::array isIncluded) -{ - for (int i = 0; i < datagram_info_map.size(); ++i) - { - if (datagram_info_map[i].included_sensors == isIncluded) - { +static DatagramIdentifier toDatagramID(std::array isIncluded) { + for (int i = 0; i < datagram_info_map.size(); ++i) { + if (datagram_info_map[i].included_sensors == isIncluded) { return datagram_info_map[i].id; } } } -static const uint8_t calculateDatagramSize(DatagramIdentifier datagram_identifier) -{ +static const uint8_t +calculateDatagramSize(DatagramIdentifier datagram_identifier) { if (datagram_identifier == DatagramIdentifier::CONFIGURATION or - datagram_identifier == DatagramIdentifier::CONFIGURATION_CRLF) - { - return 26; // CR LF ending not included in datagram. + datagram_identifier == DatagramIdentifier::CONFIGURATION_CRLF) { + return 26; // CR LF ending not included in datagram. } std::array is_included = isIncluded(datagram_identifier); - uint8_t n_inertial_sensors{ 1 }; + uint8_t n_inertial_sensors{1}; n_inertial_sensors += is_included[SensorIndx::ACC] ? 1 : 0; n_inertial_sensors += is_included[SensorIndx::INCL] ? 1 : 0; - uint8_t size{ 0 }; + uint8_t size{0}; size += N_BYTES_DATAGRAM_ID; size += n_inertial_sensors * (3 * N_BYTES_INERTIAL_SENSOR + N_BYTES_STATUS); - size += is_included[SensorIndx::TEMP] ? n_inertial_sensors * (3 * N_BYTES_TEMP_SENSOR + N_BYTES_STATUS) : 0; - size += is_included[SensorIndx::AUX] ? N_BYTES_AUX_SENSOR + N_BYTES_STATUS : 0; + size += is_included[SensorIndx::TEMP] + ? n_inertial_sensors * (3 * N_BYTES_TEMP_SENSOR + N_BYTES_STATUS) + : 0; + size += + is_included[SensorIndx::AUX] ? N_BYTES_AUX_SENSOR + N_BYTES_STATUS : 0; size += N_BYTES_COUNTER; size += N_BYTES_LATENCY; size += N_BYTES_CRC; @@ -228,69 +238,46 @@ static const uint8_t calculateDatagramSize(DatagramIdentifier datagram_identifie return size; } -constexpr uint32_t powerOf2(uint8_t power) -{ - return 1 << power; -} +constexpr uint32_t powerOf2(uint8_t power) { return 1 << power; } -static constexpr double accScale(AccRange acc_range) -{ - switch (acc_range) - { - case AccRange::G2: - return 1.0 / powerOf2(21); - case AccRange::G5: - return 1.0 / powerOf2(20); - case AccRange::G10: - return 1.0 / powerOf2(19); - case AccRange::G30: - return 1.0 / powerOf2(18); - case AccRange::G80: - return 1.0 / powerOf2(16); +static constexpr double accScale(AccRange acc_range) { + switch (acc_range) { + case AccRange::G2: + return 1.0 / powerOf2(21); + case AccRange::G5: + return 1.0 / powerOf2(20); + case AccRange::G10: + return 1.0 / powerOf2(19); + case AccRange::G30: + return 1.0 / powerOf2(18); + case AccRange::G80: + return 1.0 / powerOf2(16); } } -static constexpr double accIncrScale(AccRange acc_range) -{ - switch (acc_range) - { - case AccRange::G2: - return 1.0 / powerOf2(24); - case AccRange::G5: - return 1.0 / powerOf2(23); - case AccRange::G10: - return 1.0 / powerOf2(22); - case AccRange::G30: - return 1.0 / powerOf2(21); - case AccRange::G80: - return 1.0 / powerOf2(19); +static constexpr double accIncrScale(AccRange acc_range) { + switch (acc_range) { + case AccRange::G2: + return 1.0 / powerOf2(24); + case AccRange::G5: + return 1.0 / powerOf2(23); + case AccRange::G10: + return 1.0 / powerOf2(22); + case AccRange::G30: + return 1.0 / powerOf2(21); + case AccRange::G80: + return 1.0 / powerOf2(19); } } -static constexpr double gyroIncrScale() -{ +static constexpr double gyroIncrScale() { return (M_PI / 180.00) / powerOf2(21); } -static constexpr double gyroScale() -{ - return (M_PI / 180.00) / powerOf2(14); -} -static constexpr double inclScale() -{ - return 1.0 / powerOf2(22); -} -static constexpr double inclIncrScale() -{ - return 1.0 / powerOf2(25); -} -static constexpr double tempScale() -{ - return 1.0 / powerOf2(8); -} -static constexpr double auxScale() -{ - return 5.0 / powerOf2(24); -} +static constexpr double gyroScale() { return (M_PI / 180.00) / powerOf2(14); } +static constexpr double inclScale() { return 1.0 / powerOf2(22); } +static constexpr double inclIncrScale() { return 1.0 / powerOf2(25); } +static constexpr double tempScale() { return 1.0 / powerOf2(8); } +static constexpr double auxScale() { return 5.0 / powerOf2(24); } -} // namespace stim_const -#endif // DRIVER_STIM300_STIM300_CONSTANTS_H +} // namespace stim_const +#endif // DRIVER_STIM300_STIM300_CONSTANTS_H diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index d6e50fe..19e472f 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -49,7 +49,6 @@ Quaternion FromRPYToQuaternion(EulerAngles angles) // yaw (Z), pitch (Y), roll ( } - bool responseCalibrateIMU(std_srvs::Trigger::Request &calibration_request, std_srvs::Trigger::Response &calibration_response) { @@ -71,16 +70,18 @@ int main(int argc, char** argv) ros::NodeHandle node; std::string device_name; - double variance_gyro{ 0 }; - double variance_acc{ 0 }; - int sample_rate{ 0 }; - double gravity{ 0 }; + double variance_gyro{0}; + double variance_acc{0}; + int sample_rate{0}; + double gravity{0}; node.param("device_name", device_name, "/dev/ttyUSB0"); + node.param("variance_gyro", variance_gyro,0.0001*2*4.6*pow(10,-4)); node.param("variance_acc", variance_acc, 0.000055); node.param("sample_rate", sample_rate, 125); + node.param("gravity", gravity, 9.80665); // These values have been estimated by having beluga in a pool for a couple of minutes, and then calculate the variance for each values @@ -96,6 +97,7 @@ int main(int argc, char** argv) stim300msg.orientation.z = 0.00000024358; stim300msg.header.frame_id = "imu_0"; + ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000); //ros::Publisher orientationPublisher = node.advertise("imu/orientation", 1000); ros::ServiceServer service = node.advertiseService("IMU_calibration",responseCalibrateIMU); @@ -107,8 +109,7 @@ int main(int argc, char** argv) // should be okey, but to be sure we double it ros::Rate loop_rate(sample_rate * 2); - try - { + try { SerialUnix serial_driver(device_name, stim_const::BaudRate::BAUD_921600); DriverStim300 driver_stim300(serial_driver); @@ -333,16 +334,15 @@ int main(int argc, char** argv) break; case Stim300Status::ERROR: ROS_WARN("Stim 300 IMU: internal error."); + } loop_rate.sleep(); ros::spinOnce(); } return 0; - } - catch (std::runtime_error& error) - { - // TODO: Reset IMU + } catch (std::runtime_error &error) { + // TODO: Reset IMU ROS_ERROR("%s\n", error.what()); return 0; } diff --git a/test/check_driver_stim300.cpp b/test/check_driver_stim300.cpp index 6d26c4d..6f121f1 100644 --- a/test/check_driver_stim300.cpp +++ b/test/check_driver_stim300.cpp @@ -1,7 +1,7 @@ +#include "../include/driver_stim300.h" #include "mock_serial_driver.h" #include "gmock/gmock.h" -#include "../include/driver_stim300.h" using ::testing::_; using ::testing::DoAll; @@ -10,25 +10,26 @@ using ::testing::Return; using ::testing::ReturnRef; using ::testing::SetArgReferee; -TEST(DriverTest, Test1) -{ +TEST(DriverTest, Test1) { MockStim300SerialDriver serial_driver; DatagramBuffer datagram_buffer; - ::testing::InSequence dummy; // Expect_calls must happen in specified sequence + ::testing::InSequence dummy; // Expect_calls must happen in specified sequence EXPECT_CALL(serial_driver, flush()).Times(1); EXPECT_CALL(serial_driver, writeByte('C')).Times(1); EXPECT_CALL(serial_driver, writeByte('\r')).Times(1); EXPECT_CALL(serial_driver, readByte(_)) - .Times(63) - .WillRepeatedly(Invoke(&datagram_buffer, &DatagramBuffer::getNextByte)); + .Times(63) + .WillRepeatedly(Invoke(&datagram_buffer, &DatagramBuffer::getNextByte)); - // Only created a mock datagram for RATE_ACC_INCL_TEMP_AUX with all values equal to zero + // Only created a mock datagram for RATE_ACC_INCL_TEMP_AUX with all values + // equal to zero // TODO: Create more advanced mock datagrams - DriverStim300 driverStim300(serial_driver, stim_const::DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, - GyroOutputUnit::ANGULAR_RATE, AccOutputUnit::ACCELERATION, InclOutputUnit::ACCELERATION, - AccRange::G5, SampleFreq::S125); + DriverStim300 driverStim300( + serial_driver, stim_const::DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX, + GyroOutputUnit::ANGULAR_RATE, AccOutputUnit::ACCELERATION, + InclOutputUnit::ACCELERATION, AccRange::G5, SampleFreq::S125); EXPECT_EQ(Stim300Status::NEW_MEASURMENT, driverStim300.update()); EXPECT_DOUBLE_EQ(0, driverStim300.getGyroX()); @@ -42,8 +43,7 @@ TEST(DriverTest, Test1) EXPECT_EQ(0, driverStim300.getInternalMeasurementCounter()); } -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { // The following line must be executed to initialize Google Mock // (and Google Test) before running the tests. ::testing::InitGoogleMock(&argc, argv); diff --git a/test/check_stim300_constants.cpp b/test/check_stim300_constants.cpp index ae3bb04..01d9737 100644 --- a/test/check_stim300_constants.cpp +++ b/test/check_stim300_constants.cpp @@ -4,27 +4,27 @@ using namespace stim_const; -std::vector DATAGRAM_IDS{ DatagramIdentifier::CONFIGURATION_CRLF, - DatagramIdentifier::CONFIGURATION, - DatagramIdentifier::RATE, - DatagramIdentifier::RATE_ACC, - DatagramIdentifier::RATE_INCL, - DatagramIdentifier::RATE_ACC_INCL, - DatagramIdentifier::RATE_TEMP, - DatagramIdentifier::RATE_ACC_TEMP, - DatagramIdentifier::RATE_INCL_TEMP, - DatagramIdentifier::RATE_ACC_INCL_TEMP, - DatagramIdentifier::RATE_AUX, - DatagramIdentifier::RATE_ACC_AUX, - DatagramIdentifier::RATE_INCL_AUX, - DatagramIdentifier::RATE_ACC_INCL_AUX, - DatagramIdentifier::RATE_TEMP_AUX, - DatagramIdentifier::RATE_ACC_TEMP_AUX, - DatagramIdentifier::RATE_INCL_TEMP_AUX, - DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX }; +std::vector DATAGRAM_IDS{ + DatagramIdentifier::CONFIGURATION_CRLF, + DatagramIdentifier::CONFIGURATION, + DatagramIdentifier::RATE, + DatagramIdentifier::RATE_ACC, + DatagramIdentifier::RATE_INCL, + DatagramIdentifier::RATE_ACC_INCL, + DatagramIdentifier::RATE_TEMP, + DatagramIdentifier::RATE_ACC_TEMP, + DatagramIdentifier::RATE_INCL_TEMP, + DatagramIdentifier::RATE_ACC_INCL_TEMP, + DatagramIdentifier::RATE_AUX, + DatagramIdentifier::RATE_ACC_AUX, + DatagramIdentifier::RATE_INCL_AUX, + DatagramIdentifier::RATE_ACC_INCL_AUX, + DatagramIdentifier::RATE_TEMP_AUX, + DatagramIdentifier::RATE_ACC_TEMP_AUX, + DatagramIdentifier::RATE_INCL_TEMP_AUX, + DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX}; -TEST(Stim300Constants, numberOfPaddingBytes) -{ +TEST(Stim300Constants, numberOfPaddingBytes) { EXPECT_EQ(2, numberOfPaddingBytes(DatagramIdentifier::CONFIGURATION_CRLF)); EXPECT_EQ(2, numberOfPaddingBytes(DatagramIdentifier::CONFIGURATION)); EXPECT_EQ(2, numberOfPaddingBytes(DatagramIdentifier::RATE)); @@ -42,18 +42,18 @@ TEST(Stim300Constants, numberOfPaddingBytes) EXPECT_EQ(3, numberOfPaddingBytes(DatagramIdentifier::RATE_TEMP_AUX)); EXPECT_EQ(2, numberOfPaddingBytes(DatagramIdentifier::RATE_ACC_TEMP_AUX)); EXPECT_EQ(2, numberOfPaddingBytes(DatagramIdentifier::RATE_INCL_TEMP_AUX)); - EXPECT_EQ(1, numberOfPaddingBytes(DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX)); + EXPECT_EQ(1, + numberOfPaddingBytes(DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX)); } -TEST(Stim300Constants, datagramIdentifier) -{ +TEST(Stim300Constants, datagramIdentifier) { for (auto data_id : DATAGRAM_IDS) - EXPECT_EQ(data_id, rawToDatagramIdentifier(datagramIdentifierToRaw(data_id))); - //EXPECT_THROW(rawToDatagramIdentifier(0x00), std::out_of_range); + EXPECT_EQ(data_id, + rawToDatagramIdentifier(datagramIdentifierToRaw(data_id))); + // EXPECT_THROW(rawToDatagramIdentifier(0x00), std::out_of_range); } -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/mock_serial_driver.h b/test/mock_serial_driver.h index ba0e081..320eca3 100644 --- a/test/mock_serial_driver.h +++ b/test/mock_serial_driver.h @@ -2,15 +2,14 @@ #ifndef DRIVER_STIM300_MOCK_SERIAL_DRIVER_H #define DRIVER_STIM300_MOCK_SERIAL_DRIVER_H -#include -#include "gmock/gmock.h" #include "../src/serial_unix.h" #include "../src/stim300_constants.h" +#include "gmock/gmock.h" +#include -class MockStim300SerialDriver : public SerialDriver -{ +class MockStim300SerialDriver : public SerialDriver { public: - MOCK_METHOD1(readByte, bool(uint8_t& byte)); + MOCK_METHOD1(readByte, bool(uint8_t &byte)); MOCK_METHOD1(writeByte, bool(uint8_t byte)); MOCK_METHOD0(flush, bool()); @@ -18,21 +17,20 @@ class MockStim300SerialDriver : public SerialDriver MOCK_METHOD0(close, void()); }; -class DatagramBuffer -{ +class DatagramBuffer { public: - DatagramBuffer() - { + DatagramBuffer() { // Create a simple datagram with crc checksum datagram_.push_back(175); - for (int i = 1; i < 63; ++i) - { + for (int i = 1; i < 63; ++i) { datagram_.push_back(0); } auto end = datagram_.cend(); auto begin = datagram_.cbegin(); - uint8_t crc_dummy_bytes = stim_const::numberOfPaddingBytes(stim_const::DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX); - boost::crc_basic<32> crc_32_calculator(0x04C11DB7, 0xFFFFFFFF, 0x00, false, false); + uint8_t crc_dummy_bytes = stim_const::numberOfPaddingBytes( + stim_const::DatagramIdentifier::RATE_ACC_INCL_TEMP_AUX); + boost::crc_basic<32> crc_32_calculator(0x04C11DB7, 0xFFFFFFFF, 0x00, false, + false); uint8_t buffer_CRC[end - begin - sizeof(uint32_t) + crc_dummy_bytes]; std::copy(begin, end - sizeof(uint32_t) + crc_dummy_bytes, buffer_CRC); @@ -50,8 +48,7 @@ class DatagramBuffer it_ = datagram_.cbegin(); } - bool getNextByte(uint8_t& byte) - { + bool getNextByte(uint8_t &byte) { byte = *it_++; return true; } @@ -61,4 +58,4 @@ class DatagramBuffer std::vector datagram_; }; -#endif // DRIVER_STIM300_MOCK_SERIAL_DRIVER_H +#endif // DRIVER_STIM300_MOCK_SERIAL_DRIVER_H