diff --git a/.github/workflows/ros1-ci.yaml b/.github/workflows/ros2-ci.yaml similarity index 62% rename from .github/workflows/ros1-ci.yaml rename to .github/workflows/ros2-ci.yaml index 056436c..4e27f67 100644 --- a/.github/workflows/ros1-ci.yaml +++ b/.github/workflows/ros2-ci.yaml @@ -1,30 +1,34 @@ -name: ROS1 CI Workflow +name: ROS2 CI Workflow on: pull_request: + branches: + - ros2 push: branches: - - main + - ros2 jobs: - build-ros1: - runs-on: ubuntu-latest + build-ros2: strategy: fail-fast: false + matrix: + config: + - { + ros_distro: "humble" + } + runs-on: ubuntu-latest container: - image: ros:melodic + image: ros:${{ matrix.config.ros_distro }} steps: - name: Checkout Repo uses: actions/checkout@v2 - name: Create Workspace run: | - mkdir -p src_tmp/vesc; \ - mv `find -maxdepth 1 -not -name . -not -name src_tmp` src_tmp/vesc/; \ - mv src_tmp/ src/; \ - cd src; \ - bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_init_workspace' + mkdir -p src_tmp/vesc + mv `find -maxdepth 1 -not -name . -not -name src_tmp` src_tmp/vesc/ + mv src_tmp/ src/ - name: Install Dependencies run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ @@ -33,9 +37,9 @@ jobs: - name: Build Workspace run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_make' + colcon build --event-handlers console_cohesion+' - name: Run Tests run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_make run_tests; \ - catkin_test_results --verbose' + colcon test --event-handlers console_cohesion+; \ + colcon test-result --verbose' diff --git a/README.md b/README.md index 959c0dd..9cd4091 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,34 @@ # Veddar VESC Interface -![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) +Vesc is a new repository that was forked to provide a local instance for developing minor changes required for the C1T system. The only update included in this release is negating the speed value sent to the vesc due to the motors being installed in the opposite direction. Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details + +This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). + +## How to test + +1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. +2. `rosdep update && rosdep install --from-paths src -i -y` +3. Plug in the VESC with a USB cable. +4. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. +5. Build the packages `colcon build` +6. `ros2 launch vesc_driver vesc_driver_node.launch.py` +7. If prompted "permission denied" on the serial port: `sudo chmod 777 /dev/ttyACM0` + +## Contribution +Welcome to the CARMA contributing guide. Please read this guide to learn about our development process, how to propose pull requests and improvements, and how to build and test your changes to this project. [CARMA Contributing Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) + +## Code of Conduct +Please read our [CARMA Code of Conduct](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Code_of_Conduct.md) which outlines our expectations for participants within the CARMA community, as well as steps to reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all and expect our code of conduct to be honored. Anyone who violates this code of conduct may be banned from the community. + +## Attribution +The development team would like to acknowledge the people who have made direct contributions to the design and code in this repository. [CARMA Attribution](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/ATTRIBUTION.txt) + +## License +By contributing to the Federal Highway Administration (FHWA) Connected Automated Research Mobility Applications (CARMA), you agree that your contributions will be licensed under its Apache License 2.0 license. [CARMA License](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/docs/License.md) + +## Contact +Please click on the CARMA logo below to visit the Federal Highway Adminstration(FHWA) CARMA website. For technical support from the CARMA team, please contact the CARMA help desk at CAVSupportServices@dot.gov. + +[![CARMA Image](https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/docs/image/CARMA_icon.png)](https://highways.dot.gov/research/research-programs/operations/CARMA) diff --git a/vesc/CMakeLists.txt b/vesc/CMakeLists.txt index 13b2b81..1caee5d 100644 --- a/vesc/CMakeLists.txt +++ b/vesc/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake_auto REQUIRED) +ament_auto_package() diff --git a/vesc/package.xml b/vesc/package.xml index e1deeb4..e4748c1 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -13,14 +13,15 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley + Haoru Xue - catkin + ament_cmake_auto vesc_driver vesc_msgs vesc_ackermann - + ament_cmake diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 166d201..9a5ba87 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -1,101 +1,48 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_ackermann) -# Setting C++ standard to 11 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) +# Setting C++ standard to 14 +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() -find_package(catkin REQUIRED COMPONENTS - ackermann_msgs - geometry_msgs - nav_msgs - nodelet - pluginlib - roscpp - roslint - std_msgs - tf - vesc_msgs -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - ackermann_msgs - geometry_msgs - nav_msgs - nodelet - pluginlib - std_msgs - tf - vesc_msgs -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} +# nodes library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/ackermann_to_vesc.cpp + src/vesc_to_odom.cpp ) -# node executable -add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp - src/vesc_to_odom.cpp) -add_dependencies(vesc_to_odom_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_to_odom_node - ${catkin_LIBRARIES} +# register nodes as components +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_ackermann::AckermannToVesc + EXECUTABLE ackermann_to_vesc_node ) - -add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp - src/ackermann_to_vesc.cpp) -add_dependencies(ackermann_to_vesc_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ackermann_to_vesc_node - ${catkin_LIBRARIES} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_ackermann::VescToOdom + EXECUTABLE vesc_to_odom_node ) -# nodelet library -add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp - src/ackermann_to_vesc.cpp - src/vesc_to_odom_nodelet.cpp - src/vesc_to_odom.cpp) -add_dependencies(vesc_ackermann_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_ackermann_nodelet - ${catkin_LIBRARIES} -) - -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp() - -############# -## Install ## -############# - -install(TARGETS vesc_to_odom_node ackermann_to_vesc_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(TARGETS vesc_ackermann_nodelet - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES vesc_ackermann_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h deleted file mode 100644 index 33c4d10..0000000 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -// -*- mode:c++; fill-column: 100; -*- - -#ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ -#define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ - -#include -#include - -namespace vesc_ackermann -{ - -class AckermannToVesc -{ -public: - AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); - -private: - // ROS parameters - // conversion gain and offset - double speed_to_erpm_gain_, speed_to_erpm_offset_; - double steering_to_servo_gain_, steering_to_servo_offset_; - - /** @todo consider also providing an interpolated look-up table conversion */ - - // ROS services - ros::Publisher erpm_pub_; - ros::Publisher servo_pub_; - ros::Subscriber ackermann_sub_; - - // ROS callbacks - void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); -}; - -} // namespace vesc_ackermann - -#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp new file mode 100644 index 0000000..b646c36 --- /dev/null +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -0,0 +1,68 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// -*- mode:c++; fill-column: 100; -*- + +#ifndef VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ +#define VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ + +#include +#include +#include + +namespace vesc_ackermann +{ + +using ackermann_msgs::msg::AckermannDriveStamped; +using std_msgs::msg::Float64; + +class AckermannToVesc : public rclcpp::Node +{ +public: + explicit AckermannToVesc(const rclcpp::NodeOptions & options); + +private: + // ROS parameters + // conversion gain and offset + double speed_to_erpm_gain_, speed_to_erpm_offset_; + double steering_to_servo_gain_, steering_to_servo_offset_; + + /** @todo consider also providing an interpolated look-up table conversion */ + + // ROS services + rclcpp::Publisher::SharedPtr erpm_pub_; + rclcpp::Publisher::SharedPtr servo_pub_; + rclcpp::Subscription::SharedPtr ackermann_sub_; + + // ROS callbacks + void ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd); +}; + +} // namespace vesc_ackermann + +#endif // VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h deleted file mode 100644 index f864833..0000000 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h +++ /dev/null @@ -1,77 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -// -*- mode:c++; fill-column: 100; -*- - -#ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ -#define VESC_ACKERMANN_VESC_TO_ODOM_H_ - -#include -#include - -#include -#include -#include -#include - -namespace vesc_ackermann -{ - -class VescToOdom -{ -public: - VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); - -private: - // ROS parameters - std::string odom_frame_; - std::string base_frame_; - /** State message does not report servo position, so use the command instead */ - bool use_servo_cmd_; - // conversion gain and offset - double speed_to_erpm_gain_, speed_to_erpm_offset_; - double steering_to_servo_gain_, steering_to_servo_offset_; - double wheelbase_; - bool publish_tf_; - - // odometry state - double x_, y_, yaw_; - std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value - vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message - - // ROS services - ros::Publisher odom_pub_; - ros::Subscriber vesc_state_sub_; - ros::Subscriber servo_sub_; - std::shared_ptr tf_pub_; - - // ROS callbacks - void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); - void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); -}; - -} // namespace vesc_ackermann - -#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp new file mode 100644 index 0000000..60eb3a7 --- /dev/null +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp @@ -0,0 +1,86 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// -*- mode:c++; fill-column: 100; -*- + +#ifndef VESC_ACKERMANN__VESC_TO_ODOM_HPP_ +#define VESC_ACKERMANN__VESC_TO_ODOM_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include + +namespace vesc_ackermann +{ + +using nav_msgs::msg::Odometry; +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescStateStamped; + +class VescToOdom : public rclcpp::Node +{ +public: + explicit VescToOdom(const rclcpp::NodeOptions & options); + +private: + // ROS parameters + std::string odom_frame_; + std::string base_frame_; + /** State message does not report servo position, so use the command instead */ + bool use_servo_cmd_; + // conversion gain and offset + double speed_to_erpm_gain_, speed_to_erpm_offset_; + double steering_to_servo_gain_, steering_to_servo_offset_; + double wheelbase_; + bool publish_tf_; + + // odometry state + double x_, y_, yaw_; + Float64::SharedPtr last_servo_cmd_; ///< Last servo position commanded value + VescStateStamped::SharedPtr last_state_; ///< Last received state message + + // ROS services + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr vesc_state_sub_; + rclcpp::Subscription::SharedPtr servo_sub_; + std::shared_ptr tf_pub_; + + // ROS callbacks + void vescStateCallback(const VescStateStamped::SharedPtr state); + void servoCmdCallback(const Float64::SharedPtr servo); +}; + +} // namespace vesc_ackermann + +#endif // VESC_ACKERMANN__VESC_TO_ODOM_HPP_ diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/vesc_ackermann/launch/ackermann_to_vesc_node.launch deleted file mode 100644 index d0b2ea9..0000000 --- a/vesc_ackermann/launch/ackermann_to_vesc_node.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml new file mode 100644 index 0000000..4cbf7b7 --- /dev/null +++ b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch deleted file mode 100644 index 5060fe3..0000000 --- a/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/vesc_to_odom_node.launch b/vesc_ackermann/launch/vesc_to_odom_node.launch deleted file mode 100644 index a2cf374..0000000 --- a/vesc_ackermann/launch/vesc_to_odom_node.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/vesc_to_odom_node.launch.xml b/vesc_ackermann/launch/vesc_to_odom_node.launch.xml new file mode 100644 index 0000000..712d443 --- /dev/null +++ b/vesc_ackermann/launch/vesc_to_odom_node.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/vesc_ackermann/launch/vesc_to_odom_nodelet.launch deleted file mode 100644 index 19a3aab..0000000 --- a/vesc_ackermann/launch/vesc_to_odom_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index 34d1301..bd39777 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -14,21 +14,21 @@ Michael T. Boulet Joshua Whitley - catkin - - roslint + ament_cmake_auto ackermann_msgs geometry_msgs nav_msgs - nodelet - pluginlib - roscpp + rclcpp + rclcpp_components std_msgs - tf + tf2_ros vesc_msgs + ament_lint_auto + ament_lint_common + - + ament_cmake diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index da29626..79863f8 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -1,91 +1,88 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_ackermann/ackermann_to_vesc.h" +#include "vesc_ackermann/ackermann_to_vesc.hpp" #include #include #include -#include +#include +#include namespace vesc_ackermann { -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +using ackermann_msgs::msg::AckermannDriveStamped; +using std::placeholders::_1; +using std_msgs::msg::Float64; -AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) +AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) +: Node("ackermann_to_vesc_node", options) { // get conversion parameters - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) - return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) - return; + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain"); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset"); + steering_to_servo_gain_ = + declare_parameter("steering_angle_to_servo_gain"); + steering_to_servo_offset_ = + declare_parameter("steering_angle_to_servo_offset"); // create publishers to vesc electric-RPM (speed) and servo commands - erpm_pub_ = nh.advertise("commands/motor/speed", 10); - servo_pub_ = nh.advertise("commands/servo/position", 10); + erpm_pub_ = create_publisher("commands/motor/speed", 10); + servo_pub_ = create_publisher("commands/servo/position", 10); // subscribe to ackermann topic - ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this); + ackermann_sub_ = create_subscription( + "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); } -typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr; -void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) +void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) { // calc vesc electric RPM (speed) - std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); - erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + Float64 erpm_msg; + erpm_msg.data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; // calc steering angle (servo) - std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); - servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; + Float64 servo_msg; + servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; // publish - if (ros::ok()) - { - erpm_pub_.publish(erpm_msg); - servo_pub_.publish(servo_msg); + if (rclcpp::ok()) { + erpm_pub_->publish(erpm_msg); + servo_pub_->publish(servo_msg); } } -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) -{ - if (nh.getParam(name, *value)) - return true; +} // namespace vesc_ackermann - ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); - return false; -} +#include "rclcpp_components/register_node_macro.hpp" // NOLINT -} // namespace vesc_ackermann +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::AckermannToVesc) diff --git a/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/vesc_ackermann/src/ackermann_to_vesc_node.cpp deleted file mode 100644 index d7fcec0..0000000 --- a/vesc_ackermann/src/ackermann_to_vesc_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_ackermann/ackermann_to_vesc.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "ackermann_to_vesc_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp deleted file mode 100644 index 5b72582..0000000 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_ackermann/ackermann_to_vesc.h" - -namespace vesc_ackermann -{ - -class AckermannToVescNodelet: public nodelet::Nodelet -{ -public: - AckermannToVescNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr ackermann_to_vesc_; -}; // class AckermannToVescNodelet - -void AckermannToVescNodelet::onInit() -{ - NODELET_DEBUG("Initializing ackermann to VESC nodelet"); - ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_ackermann - -PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index cca7d11..c136970 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -1,188 +1,197 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_ackermann/vesc_to_odom.h" +// Changes made from upstream +// - negated current_speed value + +#include "vesc_ackermann/vesc_to_odom.hpp" #include #include -#include -#include +#include +#include namespace vesc_ackermann { -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); - -VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : - odom_frame_("odom"), base_frame_("base_link"), - use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0) +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using std::placeholders::_1; +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescStateStamped; + +VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) +: Node("vesc_to_odom_node", options), + odom_frame_("odom"), + base_frame_("base_link"), + use_servo_cmd_(true), + publish_tf_(false), + x_(0.0), + y_(0.0), + yaw_(0.0) { // get ROS parameters - private_nh.param("odom_frame", odom_frame_, odom_frame_); - private_nh.param("base_frame", base_frame_, base_frame_); - private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_); - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) - return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) - return; - if (use_servo_cmd_) - { - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) - return; - if (!getRequiredParam(nh, "wheelbase", &wheelbase_)) - return; + odom_frame_ = declare_parameter("odom_frame", odom_frame_); + base_frame_ = declare_parameter("base_frame", base_frame_); + use_servo_cmd_ = declare_parameter("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_); + + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain"); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset"); + + if (use_servo_cmd_) { + steering_to_servo_gain_ = + declare_parameter("steering_angle_to_servo_gain"); + steering_to_servo_offset_ = + declare_parameter("steering_angle_to_servo_offset"); + wheelbase_ = declare_parameter("wheelbase"); } - private_nh.param("publish_tf", publish_tf_, publish_tf_); + + publish_tf_ = declare_parameter("publish_tf", publish_tf_); // create odom publisher - odom_pub_ = nh.advertise("odom", 10); + odom_pub_ = create_publisher("odom", 10); // create tf broadcaster - if (publish_tf_) - { - tf_pub_.reset(new tf::TransformBroadcaster); + if (publish_tf_) { + tf_pub_.reset(new tf2_ros::TransformBroadcaster(this)); } // subscribe to vesc state and. optionally, servo command - vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this); - if (use_servo_cmd_) - { - servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, - &VescToOdom::servoCmdCallback, this); + vesc_state_sub_ = create_subscription( + "sensors/core", 10, std::bind(&VescToOdom::vescStateCallback, this, _1)); + + if (use_servo_cmd_) { + servo_sub_ = create_subscription( + "sensors/servo_position_command", 10, std::bind(&VescToOdom::servoCmdCallback, this, _1)); } } -void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state) +void VescToOdom::vescStateCallback(const VescStateStamped::SharedPtr state) { // check that we have a last servo command if we are depending on it for angular velocity - if (use_servo_cmd_ && !last_servo_cmd_) + if (use_servo_cmd_ && !last_servo_cmd_) { return; + } + // Begin change from upstream // convert to engineering units - double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; - if (std::fabs(current_speed) < 0.05) - { + double current_speed = -(-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + // End change from upstream + if (std::fabs(current_speed) < 0.05) { current_speed = 0.0; } double current_steering_angle(0.0), current_angular_velocity(0.0); - if (use_servo_cmd_) - { + if (use_servo_cmd_) { current_steering_angle = (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; } // use current state as last state if this is our first time here - if (!last_state_) + if (!last_state_) { last_state_ = state; + } // calc elapsed time - ros::Duration dt = state->header.stamp - last_state_->header.stamp; + auto dt = rclcpp::Time(state->header.stamp) - rclcpp::Time(last_state_->header.stamp); /** @todo could probably do better propigating odometry, e.g. trapezoidal integration */ // propigate odometry double x_dot = current_speed * cos(yaw_); double y_dot = current_speed * sin(yaw_); - x_ += x_dot * dt.toSec(); - y_ += y_dot * dt.toSec(); - if (use_servo_cmd_) - yaw_ += current_angular_velocity * dt.toSec(); + x_ += x_dot * dt.seconds(); + y_ += y_dot * dt.seconds(); + if (use_servo_cmd_) { + yaw_ += current_angular_velocity * dt.seconds(); + } // save state for next time last_state_ = state; // publish odometry message - nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry); - odom->header.frame_id = odom_frame_; - odom->header.stamp = state->header.stamp; - odom->child_frame_id = base_frame_; + Odometry odom; + odom.header.frame_id = odom_frame_; + odom.header.stamp = state->header.stamp; + odom.child_frame_id = base_frame_; // Position - odom->pose.pose.position.x = x_; - odom->pose.pose.position.y = y_; - odom->pose.pose.orientation.x = 0.0; - odom->pose.pose.orientation.y = 0.0; - odom->pose.pose.orientation.z = sin(yaw_ / 2.0); - odom->pose.pose.orientation.w = cos(yaw_ / 2.0); + odom.pose.pose.position.x = x_; + odom.pose.pose.position.y = y_; + odom.pose.pose.orientation.x = 0.0; + odom.pose.pose.orientation.y = 0.0; + odom.pose.pose.orientation.z = sin(yaw_ / 2.0); + odom.pose.pose.orientation.w = cos(yaw_ / 2.0); // Position uncertainty /** @todo Think about position uncertainty, perhaps get from parameters? */ - odom->pose.covariance[0] = 0.2; ///< x - odom->pose.covariance[7] = 0.2; ///< y - odom->pose.covariance[35] = 0.4; ///< yaw + odom.pose.covariance[0] = 0.2; ///< x + odom.pose.covariance[7] = 0.2; ///< y + odom.pose.covariance[35] = 0.4; ///< yaw // Velocity ("in the coordinate frame given by the child_frame_id") - odom->twist.twist.linear.x = current_speed; - odom->twist.twist.linear.y = 0.0; - odom->twist.twist.angular.z = current_angular_velocity; + odom.twist.twist.linear.x = current_speed; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.angular.z = current_angular_velocity; // Velocity uncertainty /** @todo Think about velocity uncertainty */ - if (publish_tf_) - { - geometry_msgs::TransformStamped tf; + if (publish_tf_) { + TransformStamped tf; tf.header.frame_id = odom_frame_; tf.child_frame_id = base_frame_; - tf.header.stamp = ros::Time::now(); + tf.header.stamp = now(); tf.transform.translation.x = x_; tf.transform.translation.y = y_; tf.transform.translation.z = 0.0; - tf.transform.rotation = odom->pose.pose.orientation; - if (ros::ok()) - { + tf.transform.rotation = odom.pose.pose.orientation; + + if (rclcpp::ok()) { tf_pub_->sendTransform(tf); } } - if (ros::ok()) - { - odom_pub_.publish(odom); + if (rclcpp::ok()) { + odom_pub_->publish(odom); } } -void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) +void VescToOdom::servoCmdCallback(const Float64::SharedPtr servo) { last_servo_cmd_ = servo; } -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) -{ - if (nh.getParam(name, *value)) - return true; +} // namespace vesc_ackermann - ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); - return false; -} +#include "rclcpp_components/register_node_macro.hpp" // NOLINT -} // namespace vesc_ackermann +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::VescToOdom) diff --git a/vesc_ackermann/src/vesc_to_odom_node.cpp b/vesc_ackermann/src/vesc_to_odom_node.cpp deleted file mode 100644 index caea389..0000000 --- a/vesc_ackermann/src/vesc_to_odom_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_ackermann/vesc_to_odom.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "vesc_to_odom_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp deleted file mode 100644 index 5f21c85..0000000 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_ackermann/vesc_to_odom.h" - -namespace vesc_ackermann -{ - -class VescToOdomNodelet: public nodelet::Nodelet -{ -public: - VescToOdomNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr vesc_to_odom_; -}; // class VescToOdomNodelet - -void VescToOdomNodelet::onInit() -{ - NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet"); - vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_ackermann - -PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/vesc_ackermann_nodelet.xml b/vesc_ackermann/vesc_ackermann_nodelet.xml deleted file mode 100644 index b12358a..0000000 --- a/vesc_ackermann/vesc_ackermann_nodelet.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - Nodelet for translating ackermann messages to VESC motor controller commands. - - - Nodelet for translating VESC motor controller state to odometry messages. - - diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 50221d3..5d3eac0 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,100 +1,56 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++11 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) +# Set minimum C++ standard to C++14 +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() -find_package(Boost REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -find_package(catkin REQUIRED COMPONENTS - nodelet - pluginlib - roscpp - roslint - serial - std_msgs - vesc_msgs -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - nodelet - pluginlib - std_msgs - vesc_msgs serial -) +find_package(Threads) ########### ## Build ## ########### -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} +# node library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/vesc_driver.cpp + src/vesc_interface.cpp + src/vesc_packet.cpp + src/vesc_packet_factory.cpp ) - -# node executable -add_executable(vesc_driver_node src/vesc_driver_node.cpp - src/vesc_driver.cpp - src/vesc_interface.cpp - src/vesc_packet.cpp - src/vesc_packet_factory.cpp) -add_dependencies(vesc_driver_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_driver_node - ${catkin_LIBRARIES} +target_link_libraries(${PROJECT_NAME} + ${CMAKE_THREAD_LIBS_INIT} ) - -# nodelet library -add_library(vesc_driver_nodelet src/vesc_driver_nodelet.cpp - src/vesc_driver.cpp - src/vesc_interface.cpp - src/vesc_packet.cpp - src/vesc_packet_factory.cpp) -add_dependencies(vesc_driver_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_driver_nodelet - ${catkin_LIBRARIES} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_driver::VescDriver + EXECUTABLE ${PROJECT_NAME}_node ) -file(GLOB_RECURSE ${PROJECT_NAME}_CPP_FILES - RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} - . *.cpp *.h) -list(REMOVE_ITEM ${PROJECT_NAME}_CPP_FILES - include/${PROJECT_NAME}/crc.h) - -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp(${${PROJECT_NAME}_CPP_FILES}) - -############# -## Install ## -############# - -install(TARGETS vesc_driver_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(TARGETS vesc_driver_nodelet - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +ament_auto_add_executable( + vesc_device_namer + src/vesc_device_namer.cpp + src/vesc_device_uuid_lookup.cpp ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES vesc_driver_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + params +) diff --git a/vesc_driver/include/vesc_driver/crc.h b/vesc_driver/include/vesc_driver/crc.hpp similarity index 57% rename from vesc_driver/include/vesc_driver/crc.h rename to vesc_driver/include/vesc_driver/crc.hpp index 68e322d..1d85830 100644 --- a/vesc_driver/include/vesc_driver/crc.h +++ b/vesc_driver/include/vesc_driver/crc.hpp @@ -1,38 +1,36 @@ -/** - @file CRC.h +// Copyright (c) 2020, Daniel Bahr +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** + @file CRC.hpp @author Daniel Bahr @version 1.0.1.0 @copyright - @parblock - CRC++ - Copyright (c) 2020, Daniel Bahr - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of CRC++ nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - @endparblock */ /* @@ -52,78 +50,78 @@ multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation may be faster on processor architectures which support single-instruction integer multiplication. #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). - #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. + #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. */ -#ifndef VESC_DRIVER_CRC_H_ -#define VESC_DRIVER_CRC_H_ +#ifndef VESC_DRIVER__CRC_HPP_ +#define VESC_DRIVER__CRC_HPP_ -#include // Includes CHAR_BIT +#include // Includes CHAR_BIT #ifdef CRCPP_USE_CPP11 -#include // Includes ::std::size_t -#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t +#include // Includes ::std::size_t +#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t #else -#include // Includes size_t -#include // Includes uint8_t, uint16_t, uint32_t, uint64_t +#include // Includes size_t +#include // Includes uint8_t, uint16_t, uint32_t, uint64_t #endif -#include // Includes ::std::numeric_limits -#include // Includes ::std::move +#include // Includes ::std::numeric_limits +#include // Includes ::std::move #ifndef crcpp_uint8 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. # define crcpp_uint8 ::std::uint8_t # else - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. # define crcpp_uint8 uint8_t # endif #endif #ifndef crcpp_uint16 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. # define crcpp_uint16 ::std::uint16_t # else - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. # define crcpp_uint16 uint16_t # endif #endif #ifndef crcpp_uint32 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. # define crcpp_uint32 ::std::uint32_t # else - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. # define crcpp_uint32 uint32_t # endif #endif #ifndef crcpp_uint64 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. # define crcpp_uint64 ::std::uint64_t # else - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. # define crcpp_uint64 uint64_t # endif #endif #ifndef crcpp_size # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned size definition, used for specifying data sizes. +/// @brief Unsigned size definition, used for specifying data sizes. # define crcpp_size ::std::size_t # else - /// @brief Unsigned size definition, used for specifying data sizes. +/// @brief Unsigned size definition, used for specifying data sizes. # define crcpp_size size_t # endif #endif #ifdef CRCPP_USE_CPP11 - /// @brief Compile-time expression definition. +/// @brief Compile-time expression definition. # define crcpp_constexpr constexpr #else - /// @brief Compile-time expression definition. +/// @brief Compile-time expression definition. # define crcpp_constexpr const #endif @@ -142,163 +140,176 @@ namespace CRCPP class CRC { public: - // Forward declaration - template - struct Table; - - /** - @brief CRC parameters. - */ - template - struct Parameters - { - CRCType polynomial; ///< CRC polynomial - CRCType initialValue; ///< Initial CRC value - CRCType finalXOR; ///< Value to XOR with the final CRC - bool reflectInput; ///< true to reflect all input bytes - bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) - - Table MakeTable() const; - }; - - /** - @brief CRC lookup table. After construction, the CRC parameters are fixed. - @note A CRC table can be used for multiple CRC calculations. - */ - template - struct Table - { - // Constructors are intentionally NOT marked explicit. - Table(const Parameters & parameters); + // Forward declaration + template + struct Table; + + /** + @brief CRC parameters. + */ + template + struct Parameters + { + CRCType polynomial; ///< CRC polynomial + CRCType initialValue; ///< Initial CRC value + CRCType finalXOR; ///< Value to XOR with the final CRC + bool reflectInput; ///< true to reflect all input bytes + bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) // NOLINT + + Table MakeTable() const; + }; + + /** + @brief CRC lookup table. After construction, the CRC parameters are fixed. + @note A CRC table can be used for multiple CRC calculations. + */ + template + struct Table + { + // Constructors are intentionally NOT marked explicit. + explicit Table(const Parameters & parameters); #ifdef CRCPP_USE_CPP11 - Table(Parameters && parameters); + explicit Table(Parameters && parameters); #endif - const Parameters & GetParameters() const; - - const CRCType * GetTable() const; - - CRCType operator[](unsigned char index) const; - - private: - void InitTable(); - - Parameters parameters; ///< CRC parameters used to construct the table - CRCType table[1 << CHAR_BIT]; ///< CRC lookup table - }; - - // The number of bits in CRCType must be at least as large as CRCWidth. - // CRCType must be an unsigned integer type or a custom type with operator overloads. - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters); - - template - static CRCType Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + const Parameters & GetParameters() const; - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable); + const CRCType * GetTable() const; - template - static CRCType Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + CRCType operator[](unsigned char index) const; - // Common CRCs up to 64 bits. - // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) +private: + void InitTable(); + + Parameters parameters; ///< CRC parameters used to construct the table + CRCType table[1 << CHAR_BIT]; ///< CRC lookup table + }; + + // The number of bits in CRCType must be at least as large as CRCWidth. + // CRCType must be an unsigned integer type or a custom type with operator overloads. + template + static CRCType Calculate( + const void * data, crcpp_size size, const Parameters & parameters); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Table & lookupTable); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + + // Common CRCs up to 64 bits. + // Note: Check values are the computed CRCs when given an ASCII input of "123456789" + // (without null terminator) #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); - static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); - static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); - static const Parameters< crcpp_uint8, 7> & CRC_7(); + static const Parameters & CRC_4_ITU(); + static const Parameters & CRC_5_EPC(); + static const Parameters & CRC_5_ITU(); + static const Parameters & CRC_5_USB(); + static const Parameters & CRC_6_CDMA2000A(); + static const Parameters & CRC_6_CDMA2000B(); + static const Parameters & CRC_6_ITU(); + static const Parameters & CRC_7(); #endif - static const Parameters< crcpp_uint8, 8> & CRC_8(); + static const Parameters & CRC_8(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); - static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); - static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); - static const Parameters & CRC_10(); - static const Parameters & CRC_10_CDMA2000(); - static const Parameters & CRC_11(); - static const Parameters & CRC_12_CDMA2000(); - static const Parameters & CRC_12_DECT(); - static const Parameters & CRC_12_UMTS(); - static const Parameters & CRC_13_BBC(); - static const Parameters & CRC_15(); - static const Parameters & CRC_15_MPT1327(); + static const Parameters & CRC_8_EBU(); + static const Parameters & CRC_8_MAXIM(); + static const Parameters & CRC_8_WCDMA(); + static const Parameters & CRC_10(); + static const Parameters & CRC_10_CDMA2000(); + static const Parameters & CRC_11(); + static const Parameters & CRC_12_CDMA2000(); + static const Parameters & CRC_12_DECT(); + static const Parameters & CRC_12_UMTS(); + static const Parameters & CRC_13_BBC(); + static const Parameters & CRC_15(); + static const Parameters & CRC_15_MPT1327(); #endif - static const Parameters & CRC_16_ARC(); - static const Parameters & CRC_16_BUYPASS(); - static const Parameters & CRC_16_CCITTFALSE(); + static const Parameters & CRC_16_ARC(); + static const Parameters & CRC_16_BUYPASS(); + static const Parameters & CRC_16_CCITTFALSE(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_CDMA2000(); - static const Parameters & CRC_16_CMS(); - static const Parameters & CRC_16_DECTR(); - static const Parameters & CRC_16_DECTX(); - static const Parameters & CRC_16_DNP(); + static const Parameters & CRC_16_CDMA2000(); + static const Parameters & CRC_16_CMS(); + static const Parameters & CRC_16_DECTR(); + static const Parameters & CRC_16_DECTX(); + static const Parameters & CRC_16_DNP(); #endif - static const Parameters & CRC_16_GENIBUS(); - static const Parameters & CRC_16_KERMIT(); + static const Parameters & CRC_16_GENIBUS(); + static const Parameters & CRC_16_KERMIT(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_MAXIM(); - static const Parameters & CRC_16_MODBUS(); - static const Parameters & CRC_16_T10DIF(); - static const Parameters & CRC_16_USB(); + static const Parameters & CRC_16_MAXIM(); + static const Parameters & CRC_16_MODBUS(); + static const Parameters & CRC_16_T10DIF(); + static const Parameters & CRC_16_USB(); #endif - static const Parameters & CRC_16_X25(); - static const Parameters & CRC_16_XMODEM(); + static const Parameters & CRC_16_X25(); + static const Parameters & CRC_16_XMODEM(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_17_CAN(); - static const Parameters & CRC_21_CAN(); - static const Parameters & CRC_24(); - static const Parameters & CRC_24_FLEXRAYA(); - static const Parameters & CRC_24_FLEXRAYB(); - static const Parameters & CRC_30(); + static const Parameters & CRC_17_CAN(); + static const Parameters & CRC_21_CAN(); + static const Parameters & CRC_24(); + static const Parameters & CRC_24_FLEXRAYA(); + static const Parameters & CRC_24_FLEXRAYB(); + static const Parameters & CRC_30(); #endif - static const Parameters & CRC_32(); - static const Parameters & CRC_32_BZIP2(); + static const Parameters & CRC_32(); + static const Parameters & CRC_32_BZIP2(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_C(); + static const Parameters & CRC_32_C(); #endif - static const Parameters & CRC_32_MPEG2(); - static const Parameters & CRC_32_POSIX(); + static const Parameters & CRC_32_MPEG2(); + static const Parameters & CRC_32_POSIX(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_Q(); - static const Parameters & CRC_40_GSM(); - static const Parameters & CRC_64(); + static const Parameters & CRC_32_Q(); + static const Parameters & CRC_40_GSM(); + static const Parameters & CRC_64(); #endif #ifdef CRCPP_USE_CPP11 - CRC() = delete; - CRC(const CRC & other) = delete; - CRC & operator=(const CRC & other) = delete; - CRC(CRC && other) = delete; - CRC & operator=(CRC && other) = delete; + CRC() = delete; + CRC(const CRC & other) = delete; + CRC & operator=(const CRC & other) = delete; + CRC(CRC && other) = delete; + CRC & operator=(CRC && other) = delete; #endif private: #ifndef CRCPP_USE_CPP11 - CRC(); - CRC(const CRC & other); - CRC & operator=(const CRC & other); + CRC(); + CRC(const CRC & other); + CRC & operator=(const CRC & other); #endif - template - static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); + template + static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); - template - static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + template + static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - template - static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + template + static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); + template + static CRCType CalculateRemainder( + const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); + template + static CRCType CalculateRemainder( + const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); }; /** @@ -309,11 +320,11 @@ class CRC @tparam CRCWidth Number of bits in the CRC @return CRC lookup table */ -template +template inline CRC::Table CRC::Parameters::MakeTable() const { - // This should take advantage of RVO and optimize out the copy. - return CRC::Table(*this); + // This should take advantage of RVO and optimize out the copy. + return CRC::Table(*this); } /** @@ -322,11 +333,11 @@ inline CRC::Table CRC::Parameters::MakeTab @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template -inline CRC::Table::Table(const Parameters & params) : - parameters(params) +template +inline CRC::Table::Table(const Parameters & params) +: parameters(params) { - InitTable(); + InitTable(); } #ifdef CRCPP_USE_CPP11 @@ -336,11 +347,11 @@ inline CRC::Table::Table(const Parameters @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template -inline CRC::Table::Table(Parameters && params) : - parameters(::std::move(params)) +template +inline CRC::Table::Table(Parameters && params) +: parameters(::std::move(params)) { - InitTable(); + InitTable(); } #endif @@ -350,10 +361,11 @@ inline CRC::Table::Table(Parameters && par @tparam CRCWidth Number of bits in the CRC @return CRC parameters */ -template -inline const CRC::Parameters & CRC::Table::GetParameters() const +template +inline const CRC::Parameters & CRC::Table::GetParameters() const { - return parameters; + return parameters; } /** @@ -362,10 +374,10 @@ inline const CRC::Parameters & CRC::Table: @tparam CRCWidth Number of bits in the CRC @return CRC table */ -template +template inline const CRCType * CRC::Table::GetTable() const { - return table; + return table; } /** @@ -375,10 +387,10 @@ inline const CRCType * CRC::Table::GetTable() const @tparam CRCWidth Number of bits in the CRC @return CRC table entry */ -template +template inline CRCType CRC::Table::operator[](unsigned char index) const { - return table[index]; + return table[index]; } /** @@ -386,39 +398,39 @@ inline CRCType CRC::Table::operator[](unsigned char index) co @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template +template inline void CRC::Table::InitTable() { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + // For masking off the bits for the CRC + // (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - CRCType crc; - unsigned char byte = 0; + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - // Loop over each dividend (each possible number storable in an unsigned char) - do - { - crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); + CRCType crc; + unsigned char byte = 0; - // This mask might not be necessary; all unit tests pass with this line commented out, - // but that might just be a coincidence based on the CRC parameters used for testing. - // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. - crc &= BIT_MASK; + // Loop over each dividend (each possible number storable in an unsigned char) + do { + crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); - if (!parameters.reflectInput && CRCWidth < CHAR_BIT) - { - // Undo the special operation at the end of the CalculateRemainder() - // function for non-reflected CRCs < CHAR_BIT. - crc = static_cast(crc << SHIFT); - } + // This mask might not be necessary; all unit tests pass with this line commented out, + // but that might just be a coincidence based on the CRC parameters used for testing. + // In any case, this is harmless to leave in and only adds a single machine instruction + // per loop iteration. + crc &= BIT_MASK; - table[byte] = crc; + if (!parameters.reflectInput && CRCWidth < CHAR_BIT) { + // Undo the special operation at the end of the CalculateRemainder() + // function for non-reflected CRCs < CHAR_BIT. + crc = static_cast(crc << SHIFT); } - while (++byte); + + table[byte] = crc; + } while (++byte); } /** @@ -430,14 +442,18 @@ inline void CRC::Table::InitTable() @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Parameters & parameters) { - CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); + CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @brief Appends additional data to a previous CRC calculation. @@ -450,16 +466,23 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Paramete @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) { - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + CRCType remainder = UndoFinalize( + crc, parameters.finalXOR, + parameters.reflectInput != + parameters.reflectOutput); - remainder = CalculateRemainder(data, size, parameters, remainder); + remainder = CalculateRemainder(data, size, parameters, remainder); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -471,16 +494,20 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Paramete @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Table & lookupTable) { - const Parameters & parameters = lookupTable.GetParameters(); + const Parameters & parameters = lookupTable.GetParameters(); - CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); + CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -494,18 +521,25 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) { - const Parameters & parameters = lookupTable.GetParameters(); + const Parameters & parameters = lookupTable.GetParameters(); - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + CRCType remainder = UndoFinalize( + crc, parameters.finalXOR, + parameters.reflectInput != + parameters.reflectOutput); - remainder = CalculateRemainder(data, size, lookupTable, remainder); + remainder = CalculateRemainder(data, size, lookupTable, remainder); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -515,18 +549,17 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table +template inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) { - IntegerType reversedValue(0); + IntegerType reversedValue(0); - for (crcpp_uint16 i = 0; i < numBits; ++i) - { - reversedValue = static_cast((reversedValue << 1) | (value & 1)); - value = static_cast(value >> 1); - } + for (crcpp_uint16 i = 0; i < numBits; ++i) { + reversedValue = static_cast((reversedValue << 1) | (value & 1)); + value = static_cast(value >> 1); + } - return reversedValue; + return reversedValue; } /** @@ -538,25 +571,26 @@ inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) @tparam CRCWidth Number of bits in the CRC @return Final CRC */ -template +template inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + // For masking off the bits for the CRC + // (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - if (reflectOutput) - { - remainder = Reflect(remainder, CRCWidth); - } + if (reflectOutput) { + remainder = Reflect(remainder, CRCWidth); + } - return (remainder ^ finalXOR) & BIT_MASK; + return (remainder ^ finalXOR) & BIT_MASK; } /** @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. @note This function allows for computation of multi-part CRCs - @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: + @note Calling UndoFinalize() followed by Finalize() (or vice versa) + will always return the original remainder value: CRCType x = ...; CRCType y = Finalize(x, finalXOR, reflectOutput); @@ -570,21 +604,21 @@ inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOu @tparam CRCWidth Number of bits in the CRC @return Un-finalized CRC remainder */ -template +template inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + // For masking off the bits for the CRC + // (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - crc = (crc & BIT_MASK) ^ finalXOR; + crc = (crc & BIT_MASK) ^ finalXOR; - if (reflectOutput) - { - crc = Reflect(crc, CRCWidth); - } + if (reflectOutput) { + crc = Reflect(crc, CRCWidth); + } - return crc; + return crc; } /** @@ -597,111 +631,122 @@ inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutp @tparam CRCWidth Number of bits in the CRC @return CRC remainder */ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) +template +inline CRCType CRC::CalculateRemainder( + const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) { #ifdef CRCPP_USE_CPP11 - // This static_assert is put here because this function will always be compiled in no matter what - // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. - static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); + // This static_assert is put here because this function will always be compiled in no matter what + // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. + static_assert( + ::std::numeric_limits::digits >= CRCWidth, + "CRCType is too small to contain a CRC of width CRCWidth."); #else - // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's - // better than nothing. - enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; + // Catching this compile-time error is very important. Sadly, + // the compiler error will be very cryptic, but it's better than nothing. + // cppcheck-suppress zerodiv + enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / + (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; #endif - const unsigned char * current = reinterpret_cast(data); + const unsigned char * current = reinterpret_cast(data); - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much - // computation from the inner loop (looping over each bit) as possible. - if (parameters.reflectInput) - { - CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); - while (size--) - { - remainder = static_cast(remainder ^ *current++); + // Slightly different implementations based on the parameters. + // The current implementations try to eliminate as much + // computation from the inner loop (looping over each bit) as possible. + if (parameters.reflectInput) { + CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); + while (size--) { + remainder = static_cast(remainder ^ *current++); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & 1) - // remainder = (remainder >> 1) ^ polynomial; - // else - // remainder >>= 1; - remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. + // This code is equivalent to the following: + // if (remainder & 1) + // remainder = (remainder >> 1) ^ polynomial; + // else + // remainder >>= 1; + remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); #else - remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); + remainder = + static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> + 1)); #endif - } - } + } } - else if (CRCWidth >= CHAR_BIT) - { - static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); + } else if (CRCWidth >= CHAR_BIT) { + static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); #ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); + static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); #endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - while (size--) - { - remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); + while (size--) { + remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CRC_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ parameters.polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. + // This code is equivalent to the following: + // if (remainder & CRC_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ parameters.polynomial; + // else + // remainder <<= 1; + remainder = + static_cast((remainder << + 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); #else - remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); + remainder = + static_cast((remainder & + CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); #endif - } - } + } } - else - { - static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); + } else { + static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); #ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); + static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); #endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast(remainder << SHIFT); + CRCType polynomial = static_cast(parameters.polynomial << SHIFT); + remainder = static_cast(remainder << SHIFT); - while (size--) - { - remainder = static_cast(remainder ^ *current++); + while (size--) { + remainder = static_cast(remainder ^ *current++); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. + // This code is equivalent to the following: + // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ polynomial; + // else + // remainder <<= 1; + remainder = + static_cast((remainder << + 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); #else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); + remainder = + static_cast((remainder & + CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); #endif - } - } - - remainder = static_cast(remainder >> SHIFT); + } } - return remainder; + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; } /** @@ -709,60 +754,61 @@ inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const @param[in] data Data over which the remainder will be computed @param[in] size Size of the data @param[in] lookupTable CRC lookup table - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @param[in] remainder Running CRC remainder. + Can be an initial value or the result of a previous CRC remainder calculation. @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC @return CRC remainder */ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) +template +inline CRCType CRC::CalculateRemainder( + const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) { - const unsigned char * current = reinterpret_cast(data); + const unsigned char * current = reinterpret_cast(data); - if (lookupTable.GetParameters().reflectInput) - { - while (size--) - { + if (lookupTable.GetParameters().reflectInput) { + while (size--) { #if defined(WIN32) || defined(_WIN32) || defined(WINCE) - // Disable warning about data loss when doing (remainder >> CHAR_BIT) when - // remainder is one byte long. The algorithm is still correct in this case, - // though it's possible that one additional machine instruction will be executed. + // Disable warning about data loss when doing (remainder >> CHAR_BIT) when + // remainder is one byte long. The algorithm is still correct in this case, + // though it's possible that one additional machine instruction will be executed. # pragma warning (push) # pragma warning (disable : 4333) #endif - remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); + remainder = + static_cast((remainder >> + CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); #if defined(WIN32) || defined(_WIN32) || defined(WINCE) # pragma warning (pop) #endif - } } - else if (CRCWidth >= CHAR_BIT) - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); - } - } - else - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + } else if (CRCWidth >= CHAR_BIT) { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - remainder = static_cast(remainder << SHIFT); + while (size--) { + remainder = + static_cast((remainder << + CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); + } + } else { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - while (size--) - { - // Note: no need to mask here since remainder is guaranteed to fit in a single byte. - remainder = lookupTable[static_cast(remainder ^ *current++)]; - } + remainder = static_cast(remainder << SHIFT); - remainder = static_cast(remainder >> SHIFT); + while (size--) { + // Note: no need to mask here since remainder is guaranteed to fit in a single byte. + remainder = lookupTable[static_cast(remainder ^ *current++)]; } - return remainder; + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -780,8 +826,8 @@ inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const */ inline const CRC::Parameters & CRC::CRC_4_ITU() { - static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; - return parameters; + static const Parameters parameters = {0x3, 0x0, 0x0, true, true}; + return parameters; } /** @@ -798,8 +844,8 @@ inline const CRC::Parameters & CRC::CRC_4_ITU() */ inline const CRC::Parameters & CRC::CRC_5_EPC() { - static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x09, 0x09, 0x00, false, false}; + return parameters; } /** @@ -816,8 +862,8 @@ inline const CRC::Parameters & CRC::CRC_5_EPC() */ inline const CRC::Parameters & CRC::CRC_5_ITU() { - static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x15, 0x00, 0x00, true, true}; + return parameters; } /** @@ -834,8 +880,8 @@ inline const CRC::Parameters & CRC::CRC_5_ITU() */ inline const CRC::Parameters & CRC::CRC_5_USB() { - static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; - return parameters; + static const Parameters parameters = {0x05, 0x1F, 0x1F, true, true}; + return parameters; } /** @@ -852,8 +898,8 @@ inline const CRC::Parameters & CRC::CRC_5_USB() */ inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() { - static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x27, 0x3F, 0x00, false, false}; + return parameters; } /** @@ -870,8 +916,8 @@ inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() */ inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() { - static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x07, 0x3F, 0x00, false, false}; + return parameters; } /** @@ -888,8 +934,8 @@ inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() */ inline const CRC::Parameters & CRC::CRC_6_ITU() { - static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x03, 0x00, 0x00, true, true}; + return parameters; } /** @@ -906,10 +952,10 @@ inline const CRC::Parameters & CRC::CRC_6_ITU() */ inline const CRC::Parameters & CRC::CRC_7() { - static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x09, 0x00, 0x00, false, false}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-8 SMBus. @@ -925,8 +971,8 @@ inline const CRC::Parameters & CRC::CRC_7() */ inline const CRC::Parameters & CRC::CRC_8() { - static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x07, 0x00, 0x00, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -944,8 +990,8 @@ inline const CRC::Parameters & CRC::CRC_8() */ inline const CRC::Parameters & CRC::CRC_8_EBU() { - static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x1D, 0xFF, 0x00, true, true}; + return parameters; } /** @@ -962,8 +1008,8 @@ inline const CRC::Parameters & CRC::CRC_8_EBU() */ inline const CRC::Parameters & CRC::CRC_8_MAXIM() { - static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x31, 0x00, 0x00, true, true}; + return parameters; } /** @@ -980,8 +1026,8 @@ inline const CRC::Parameters & CRC::CRC_8_MAXIM() */ inline const CRC::Parameters & CRC::CRC_8_WCDMA() { - static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x9B, 0x00, 0x00, true, true}; + return parameters; } /** @@ -998,8 +1044,8 @@ inline const CRC::Parameters & CRC::CRC_8_WCDMA() */ inline const CRC::Parameters & CRC::CRC_10() { - static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x233, 0x000, 0x000, false, false}; + return parameters; } /** @@ -1016,8 +1062,8 @@ inline const CRC::Parameters & CRC::CRC_10() */ inline const CRC::Parameters & CRC::CRC_10_CDMA2000() { - static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x3D9, 0x3FF, 0x000, false, false}; + return parameters; } /** @@ -1034,8 +1080,8 @@ inline const CRC::Parameters & CRC::CRC_10_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_11() { - static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x385, 0x01A, 0x000, false, false}; + return parameters; } /** @@ -1052,8 +1098,8 @@ inline const CRC::Parameters & CRC::CRC_11() */ inline const CRC::Parameters & CRC::CRC_12_CDMA2000() { - static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0xF13, 0xFFF, 0x000, false, false}; + return parameters; } /** @@ -1070,8 +1116,8 @@ inline const CRC::Parameters & CRC::CRC_12_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_12_DECT() { - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x80F, 0x000, 0x000, false, false}; + return parameters; } /** @@ -1088,8 +1134,8 @@ inline const CRC::Parameters & CRC::CRC_12_DECT() */ inline const CRC::Parameters & CRC::CRC_12_UMTS() { - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; - return parameters; + static const Parameters parameters = {0x80F, 0x000, 0x000, false, true}; + return parameters; } /** @@ -1106,8 +1152,8 @@ inline const CRC::Parameters & CRC::CRC_12_UMTS() */ inline const CRC::Parameters & CRC::CRC_13_BBC() { - static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1CF5, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1124,8 +1170,8 @@ inline const CRC::Parameters & CRC::CRC_13_BBC() */ inline const CRC::Parameters & CRC::CRC_15() { - static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x4599, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1142,10 +1188,10 @@ inline const CRC::Parameters & CRC::CRC_15() */ inline const CRC::Parameters & CRC::CRC_15_MPT1327() { - static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; - return parameters; + static const Parameters parameters = {0x6815, 0x0000, 0x0001, false, false}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). @@ -1161,8 +1207,8 @@ inline const CRC::Parameters & CRC::CRC_15_MPT1327() */ inline const CRC::Parameters & CRC::CRC_16_ARC() { - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0x0000, true, true}; + return parameters; } /** @@ -1179,8 +1225,8 @@ inline const CRC::Parameters & CRC::CRC_16_ARC() */ inline const CRC::Parameters & CRC::CRC_16_BUYPASS() { - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1197,8 +1243,8 @@ inline const CRC::Parameters & CRC::CRC_16_BUYPASS() */ inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0x0000, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1216,8 +1262,8 @@ inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() */ inline const CRC::Parameters & CRC::CRC_16_CDMA2000() { - static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0xC867, 0xFFFF, 0x0000, false, false}; + return parameters; } /** @@ -1234,8 +1280,8 @@ inline const CRC::Parameters & CRC::CRC_16_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_16_CMS() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0x0000, false, false}; + return parameters; } /** @@ -1252,8 +1298,8 @@ inline const CRC::Parameters & CRC::CRC_16_CMS() */ inline const CRC::Parameters & CRC::CRC_16_DECTR() { - static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; - return parameters; + static const Parameters parameters = {0x0589, 0x0000, 0x0001, false, false}; + return parameters; } /** @@ -1270,8 +1316,8 @@ inline const CRC::Parameters & CRC::CRC_16_DECTR() */ inline const CRC::Parameters & CRC::CRC_16_DECTX() { - static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x0589, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1288,10 +1334,10 @@ inline const CRC::Parameters & CRC::CRC_16_DECTX() */ inline const CRC::Parameters & CRC::CRC_16_DNP() { - static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x3D65, 0x0000, 0xFFFF, true, true}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-16 GENIBUS (aka CRC-16 EPC, CRC-16 I-CODE, CRC-16 DARC). @@ -1307,8 +1353,8 @@ inline const CRC::Parameters & CRC::CRC_16_DNP() */ inline const CRC::Parameters & CRC::CRC_16_GENIBUS() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0xFFFF, false, false}; + return parameters; } /** @@ -1325,8 +1371,8 @@ inline const CRC::Parameters & CRC::CRC_16_GENIBUS() */ inline const CRC::Parameters & CRC::CRC_16_KERMIT() { - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x1021, 0x0000, 0x0000, true, true}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1344,8 +1390,8 @@ inline const CRC::Parameters & CRC::CRC_16_KERMIT() */ inline const CRC::Parameters & CRC::CRC_16_MAXIM() { - static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0xFFFF, true, true}; + return parameters; } /** @@ -1362,8 +1408,8 @@ inline const CRC::Parameters & CRC::CRC_16_MAXIM() */ inline const CRC::Parameters & CRC::CRC_16_MODBUS() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0x0000, true, true}; + return parameters; } /** @@ -1380,8 +1426,8 @@ inline const CRC::Parameters & CRC::CRC_16_MODBUS() */ inline const CRC::Parameters & CRC::CRC_16_T10DIF() { - static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8BB7, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1398,11 +1444,11 @@ inline const CRC::Parameters & CRC::CRC_16_T10DIF() */ inline const CRC::Parameters & CRC::CRC_16_USB() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0xFFFF, true, true}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). @@ -1418,8 +1464,8 @@ inline const CRC::Parameters & CRC::CRC_16_USB() */ inline const CRC::Parameters & CRC::CRC_16_X25() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0xFFFF, true, true}; + return parameters; } /** @@ -1436,8 +1482,8 @@ inline const CRC::Parameters & CRC::CRC_16_X25() */ inline const CRC::Parameters & CRC::CRC_16_XMODEM() { - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0x0000, 0x0000, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1455,8 +1501,8 @@ inline const CRC::Parameters & CRC::CRC_16_XMODEM() */ inline const CRC::Parameters & CRC::CRC_17_CAN() { - static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; - return parameters; + static const Parameters parameters = {0x1685B, 0x00000, 0x00000, false, false}; + return parameters; } /** @@ -1473,8 +1519,9 @@ inline const CRC::Parameters & CRC::CRC_17_CAN() */ inline const CRC::Parameters & CRC::CRC_21_CAN() { - static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x102899, 0x000000, 0x000000, false, false}; + return parameters; } /** @@ -1491,8 +1538,9 @@ inline const CRC::Parameters & CRC::CRC_21_CAN() */ inline const CRC::Parameters & CRC::CRC_24() { - static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x864CFB, 0xB704CE, 0x000000, false, false}; + return parameters; } /** @@ -1509,8 +1557,9 @@ inline const CRC::Parameters & CRC::CRC_24() */ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() { - static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x5D6DCB, 0xFEDCBA, 0x000000, false, false}; + return parameters; } /** @@ -1527,8 +1576,9 @@ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() */ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() { - static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x5D6DCB, 0xABCDEF, 0x000000, false, false}; + return parameters; } /** @@ -1545,10 +1595,11 @@ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() */ inline const CRC::Parameters & CRC::CRC_30() { - static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). @@ -1564,8 +1615,9 @@ inline const CRC::Parameters & CRC::CRC_30() */ inline const CRC::Parameters & CRC::CRC_32() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true}; + return parameters; } /** @@ -1582,8 +1634,9 @@ inline const CRC::Parameters & CRC::CRC_32() */ inline const CRC::Parameters & CRC::CRC_32_BZIP2() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1601,8 +1654,9 @@ inline const CRC::Parameters & CRC::CRC_32_BZIP2() */ inline const CRC::Parameters & CRC::CRC_32_C() { - static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true}; + return parameters; } #endif @@ -1620,8 +1674,9 @@ inline const CRC::Parameters & CRC::CRC_32_C() */ inline const CRC::Parameters & CRC::CRC_32_MPEG2() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false}; + return parameters; } /** @@ -1638,8 +1693,9 @@ inline const CRC::Parameters & CRC::CRC_32_MPEG2() */ inline const CRC::Parameters & CRC::CRC_32_POSIX() { - static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1657,8 +1713,9 @@ inline const CRC::Parameters & CRC::CRC_32_POSIX() */ inline const CRC::Parameters & CRC::CRC_32_Q() { - static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x814141AB, 0x00000000, 0x00000000, false, false}; + return parameters; } /** @@ -1675,8 +1732,9 @@ inline const CRC::Parameters & CRC::CRC_32_Q() */ inline const CRC::Parameters & CRC::CRC_40_GSM() { - static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false}; + return parameters; } /** @@ -1693,13 +1751,14 @@ inline const CRC::Parameters & CRC::CRC_40_GSM() */ inline const CRC::Parameters & CRC::CRC_64() { - static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; - return parameters; + static const Parameters parameters = {0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false}; + return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS #ifdef CRCPP_USE_NAMESPACE -} +} // namespace CRCPP #endif -#endif // CRCPP_CRC_H_ +#endif // VESC_DRIVER__CRC_HPP_ diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h deleted file mode 100644 index 1497a20..0000000 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ /dev/null @@ -1,212 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -/* - Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se - - This file is part of VESC Tool. - - VESC Tool is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - VESC Tool is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#ifndef VESC_DRIVER_DATATYPES_H -#define VESC_DRIVER_DATATYPES_H - -#include - -typedef struct -{ - bool isVesc; -} -VSerialInfo_t; - -typedef enum -{ - CFG_T_UNDEFINED = 0, - CFG_T_DOUBLE, - CFG_T_INT, - CFG_T_QSTRING, - CFG_T_ENUM, - CFG_T_BOOL -} -CFG_T; - -typedef enum -{ - VESC_TX_UNDEFINED = 0, - VESC_TX_UINT8, - VESC_TX_INT8, - VESC_TX_UINT16, - VESC_TX_INT16, - VESC_TX_UINT32, - VESC_TX_INT32, - VESC_TX_DOUBLE16, - VESC_TX_DOUBLE32, - VESC_TX_DOUBLE32_AUTO -} -VESC_TX_T; - -typedef enum -{ - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR -} -mc_fault_code; - -typedef enum -{ - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR -} -disp_pos_mode; - -struct MC_VALUES -{ -public: - double v_in; - double temp_mos; - double temp_motor; - double current_motor; - double current_in; - double id; - double iq; - double rpm; - double duty_now; - double amp_hours; - double amp_hours_charged; - double watt_hours; - double watt_hours_charged; - int tachometer; - int tachometer_abs; - double position; - mc_fault_code fault_code; -}; - -typedef enum -{ - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES -} -debug_sampling_mode; - -typedef enum -{ - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_HANDBRAKE, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_GET_MCCONF_DEFAULT, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_GET_APPCONF_DEFAULT, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_DETECT_MOTOR_R_L, - COMM_DETECT_MOTOR_FLUX_LINKAGE, - COMM_DETECT_ENCODER, - COMM_DETECT_HALL_FOC, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN, - COMM_SET_CHUCK_DATA, - COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING -} -COMM_PACKET_ID; - -typedef struct -{ - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; -} -chuck_data; - -struct bldc_detect -{ -public: - double cycle_int_limit; - double bemf_coupling_k; - int hall_res; -}; - -typedef enum -{ - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL -} -NRF_PAIR_RES; - -#endif // VESC_DRIVER_DATATYPES_H diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp new file mode 100644 index 0000000..8548f76 --- /dev/null +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -0,0 +1,1106 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +/* + Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se + + This file is part of the VESC firmware. + + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef VESC_DRIVER__DATATYPES_HPP_ +#define VESC_DRIVER__DATATYPES_HPP_ + +#include +#include + +#include + +namespace vesc_driver +{ + +typedef struct +{ + bool isVesc; +} +VSerialInfo_t; + +typedef enum +{ + CFG_T_UNDEFINED = 0, + CFG_T_DOUBLE, + CFG_T_INT, + CFG_T_QSTRING, + CFG_T_ENUM, + CFG_T_BOOL +} +CFG_T; + +typedef enum +{ + VESC_TX_UNDEFINED = 0, + VESC_TX_UINT8, + VESC_TX_INT8, + VESC_TX_UINT16, + VESC_TX_INT16, + VESC_TX_UINT32, + VESC_TX_INT32, + VESC_TX_DOUBLE16, + VESC_TX_DOUBLE32, + VESC_TX_DOUBLE32_AUTO +} VESC_TX_T; + +// #include "ch.h" +typedef uint32_t systime_t; // defined in ch.h + +// Data types +typedef enum +{ + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, +} mc_state; + +typedef enum +{ + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +} mc_pwm_mode; + +typedef enum +{ + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY +} mc_comm_mode; + +typedef enum +{ + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID +} mc_sensor_mode; + +typedef enum +{ + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL, + FOC_SENSOR_MODE_HFI +} mc_foc_sensor_mode; + +// Auxiliary output mode +typedef enum +{ + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S, + OUT_AUX_MODE_UNUSED +} out_aux_mode; + +// Temperature sensor type +typedef enum +{ + TEMP_SENSOR_NTC_10K_25C = 0, + TEMP_SENSOR_PTC_1K_100C, + TEMP_SENSOR_KTY83_122, +} temp_sensor_type; + +// General purpose drive output mode +typedef enum +{ + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT +} gpd_output_mode; + +typedef enum +{ + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD +} mc_motor_type; + +// FOC current controller decoupling mode. +typedef enum +{ + FOC_CC_DECOUPLING_DISABLED = 0, + FOC_CC_DECOUPLING_CROSS, + FOC_CC_DECOUPLING_BEMF, + FOC_CC_DECOUPLING_CROSS_BEMF +} mc_foc_cc_decoupling_mode; + +typedef enum +{ + FOC_OBSERVER_ORTEGA_ORIGINAL = 0, + FOC_OBSERVER_ORTEGA_ITERATIVE +} mc_foc_observer_type; + +typedef enum +{ + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS +} mc_fault_code; + +typedef enum +{ + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_OPENLOOP_DUTY, + CONTROL_MODE_OPENLOOP_DUTY_PHASE, + CONTROL_MODE_NONE +} mc_control_mode; + +typedef enum +{ + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR +} disp_pos_mode; + +typedef enum +{ + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205, + SENSOR_PORT_MODE_SINCOS, + SENSOR_PORT_MODE_TS5700N8501, + SENSOR_PORT_MODE_TS5700N8501_MULTITURN +} sensor_port_mode; + +typedef struct +{ + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + float time_at_comm; +} mc_rpm_dep_struct; + +typedef enum +{ + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED +} drv8301_oc_mode; + +typedef enum +{ + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES +} debug_sampling_mode; + +typedef enum +{ + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M, + CAN_BAUD_10K, + CAN_BAUD_20K, + CAN_BAUD_50K, + CAN_BAUD_75K +} CAN_BAUD; + +typedef enum +{ + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID +} BATTERY_TYPE; + +typedef enum +{ + HFI_SAMPLES_8 = 0, + HFI_SAMPLES_16, + HFI_SAMPLES_32 +} FOC_HFI_SAMPLES; + +typedef struct +{ + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + float l_duty_start; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_encoder_sin_offset; + float foc_encoder_sin_gain; + float foc_encoder_cos_offset; + float foc_encoder_cos_gain; + float foc_encoder_sincos_filter_constant; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + mc_foc_cc_decoupling_mode foc_cc_decoupling; + mc_foc_observer_type foc_observer_type; + float foc_hfi_voltage_start; + float foc_hfi_voltage_run; + float foc_hfi_voltage_max; + float foc_sl_erpm_hfi; + uint16_t foc_hfi_start_samples; + float foc_hfi_obs_ovr_sec; + FOC_HFI_SAMPLES foc_hfi_samples; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + temp_sensor_type m_motor_temp_sens_type; + float m_ptc_motor_coeff; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; +} mc_configuration; + +// Applications to use +typedef enum +{ + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM, + APP_BALANCE +} app_use; + +// Throttle curve mode +typedef enum +{ + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY +} thr_exp_mode; + +// PPM control types +typedef enum +{ + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV, + PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, + PPM_CTRL_TYPE_CURRENT_SMART_REV +} ppm_control_type; + +typedef struct +{ + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + float max_erpm_for_dir; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} ppm_config; + +// ADC control types +typedef enum +{ + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON +} adc_control_type; + +typedef struct +{ + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; +} adc_config; + +// Nunchuk control types +typedef enum +{ + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV +} chuk_control_type; + +typedef struct +{ + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; + bool use_smart_rev; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} chuk_config; + +// NRF Datatypes +typedef enum +{ + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M +} NRF_SPEED; + +typedef enum +{ + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, + NRF_POWER_OFF +} NRF_POWER; + +typedef enum +{ + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 +} NRF_AW; + +typedef enum +{ + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B +} NRF_CRC; + +typedef enum +{ + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US +} NRF_RETR_DELAY; + +typedef struct +{ + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; +} nrf_config; + +typedef struct +{ + float kp; + float ki; + float kd; + uint16_t hertz; + float pitch_fault; + float roll_fault; + float adc1; + float adc2; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float tiltback_high_voltage; + float tiltback_low_voltage; + float startup_pitch_tolerance; + float startup_roll_tolerance; + float startup_speed; + float deadzone; + float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float tiltback_constant; + float roll_steer_erpm_kp; + float yaw_current_clamp; + uint16_t adc_half_fault_erpm; + float setpoint_pitch_filter; + float setpoint_target_filter; + float setpoint_clamp; +} balance_config; + +// CAN status modes +typedef enum +{ + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4, + CAN_STATUS_1_2_3_4_5 +} CAN_STATUS_MODE; + +typedef enum +{ + SHUTDOWN_MODE_ALWAYS_OFF = 0, + SHUTDOWN_MODE_ALWAYS_ON, + SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, + SHUTDOWN_MODE_OFF_AFTER_10S, + SHUTDOWN_MODE_OFF_AFTER_1M, + SHUTDOWN_MODE_OFF_AFTER_5M, + SHUTDOWN_MODE_OFF_AFTER_10M, + SHUTDOWN_MODE_OFF_AFTER_30M, + SHUTDOWN_MODE_OFF_AFTER_1H, + SHUTDOWN_MODE_OFF_AFTER_5H, +} SHUTDOWN_MODE; + +typedef enum +{ + IMU_TYPE_OFF = 0, + IMU_TYPE_INTERNAL, + IMU_TYPE_EXTERNAL_MPU9X50, + IMU_TYPE_EXTERNAL_ICM20948, + IMU_TYPE_EXTERNAL_BMI160 +} IMU_TYPE; + +typedef enum +{ + AHRS_MODE_MADGWICK = 0, + AHRS_MODE_MAHONY +} AHRS_MODE; + +typedef struct +{ + IMU_TYPE type; + AHRS_MODE mode; + int sample_rate_hz; + float accel_confidence_decay; + float mahony_kp; + float mahony_ki; + float madgwick_beta; + float rot_roll; + float rot_pitch; + float rot_yaw; + float accel_offsets[3]; + float gyro_offsets[3]; + float gyro_offset_comp_fact[3]; + float gyro_offset_comp_clamp; +} imu_config; + +typedef enum +{ + CAN_MODE_VESC = 0, + CAN_MODE_UAVCAN, + CAN_MODE_COMM_BRIDGE +} CAN_MODE; + +typedef struct +{ + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + bool permanent_uart_enabled; + SHUTDOWN_MODE shutdown_mode; + + // CAN modes + CAN_MODE can_mode; + uint8_t uavcan_esc_index; + + // Application to use + app_use app_to_use; + + // PPM application settings + ppm_config app_ppm_conf; + + // ADC application settings + adc_config app_adc_conf; + + // UART application settings + uint32_t app_uart_baudrate; + + // Nunchuk application settings + chuk_config app_chuk_conf; + + // NRF application settings + nrf_config app_nrf_conf; + + // Balance application settings + balance_config app_balance_conf; + + // IMU Settings + imu_config imu_conf; +} app_configuration; + +// Communication commands +typedef enum +{ + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION +} COMM_PACKET_ID; + +// CAN commands +typedef enum +{ + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN +} CAN_PACKET_ID; + +// Logged fault data +typedef struct +{ + uint8_t motor; + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; +} fault_data; + +// External LED state +typedef enum +{ + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT +} LED_EXT_STATE; + +typedef struct +{ + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; +} chuck_data; + +typedef struct +{ + int id; + systime_t rx_time; + float rpm; + float current; + float duty; +} can_status_msg; + +typedef struct +{ + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; +} can_status_msg_2; + +typedef struct +{ + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; +} can_status_msg_3; + +typedef struct +{ + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; +} can_status_msg_4; + +typedef struct +{ + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; +} can_status_msg_5; + +typedef struct +{ + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; +} mote_state; + +typedef enum +{ + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO +} MOTE_PACKET; + +typedef struct +{ + float v_in; + float temp_mos; + float temp_mos_1; + float temp_mos_2; + float temp_mos_3; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + float position; + mc_fault_code fault_code; + int vesc_id; + float vd; + float vq; +} mc_values; + +typedef enum +{ + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL +} NRF_PAIR_RES; + +// Orientation data +typedef struct +{ + float q0; + float q1; + float q2; + float q3; + float integralFBx; + float integralFBy; + float integralFBz; + float accMagP; + int initialUpdateDone; +} ATTITUDE_INFO; + +// Custom EEPROM variables +typedef union { + uint32_t as_u32; + int32_t as_i32; + float as_float; +} eeprom_var; + +#define EEPROM_VARS_HW 64 +#define EEPROM_VARS_CUSTOM 64 + +typedef struct +{ + float ah_tot; + float ah_charge_tot; + float wh_tot; + float wh_charge_tot; + float current_tot; + float current_in_tot; + uint8_t num_vescs; +} setup_values; +} // namespace vesc_driver +#endif // VESC_DRIVER__DATATYPES_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp new file mode 100644 index 0000000..673a69f --- /dev/null +++ b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp @@ -0,0 +1,73 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ + +#ifndef VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ +#define VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ + + +#include +#include + +#include "vesc_driver/datatypes.hpp" +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet.hpp" + +namespace vesc_driver +{ + +class VescDeviceLookup +{ +public: + explicit VescDeviceLookup(std::string device); + + const char * deviceUUID() const; + const char * version() const; + const char * hwname() const; + void close(); + bool isReady(); + +private: + std::string device_; + std::string uuid_; + std::string version_; + std::string hwname_; + std::string error_; + bool ready_; + +private: + // interface to the VESC + VescInterface vesc_; + void vescPacketCallback(const std::shared_ptr & packet); + void vescErrorCallback(const std::string & error); + + static std::string decode_uuid(const uint8_t * uuid) + { + char uuid_data[100] = ""; + std::string uuid_founded; + snprintf( + uuid_data, sizeof(uuid_data), + "%02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", + uuid[0], uuid[1], uuid[2], + uuid[3], uuid[4], uuid[5], + uuid[6], uuid[7], uuid[8], + uuid[9], uuid[10], uuid[11] + ); + return uuid_data; + } +}; +} // namespace vesc_driver + +#endif // VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h deleted file mode 100644 index bdc5ffd..0000000 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -// -*- mode:c++; fill-column: 100; -*- - -#ifndef VESC_DRIVER_VESC_DRIVER_H_ -#define VESC_DRIVER_VESC_DRIVER_H_ - -#include -#include - -#include -#include -#include - -#include "vesc_driver/vesc_interface.h" -#include "vesc_driver/vesc_packet.h" - -namespace vesc_driver -{ - -class VescDriver -{ -public: - VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh); - -private: - // interface to the VESC - VescInterface vesc_; - void vescPacketCallback(const std::shared_ptr& packet); - void vescErrorCallback(const std::string& error); - - // limits on VESC commands - struct CommandLimit - { - CommandLimit(const ros::NodeHandle& nh, const std::string& str, - const boost::optional& min_lower = boost::optional(), - const boost::optional& max_upper = boost::optional()); - double clip(double value); - std::string name; - boost::optional lower; - boost::optional upper; - }; - CommandLimit duty_cycle_limit_; - CommandLimit current_limit_; - CommandLimit brake_limit_; - CommandLimit speed_limit_; - CommandLimit position_limit_; - CommandLimit servo_limit_; - - // ROS services - ros::Publisher state_pub_; - ros::Publisher servo_sensor_pub_; - ros::Subscriber duty_cycle_sub_; - ros::Subscriber current_sub_; - ros::Subscriber brake_sub_; - ros::Subscriber speed_sub_; - ros::Subscriber position_sub_; - ros::Subscriber servo_sub_; - ros::Timer timer_; - - // driver modes (possible states) - typedef enum - { - MODE_INITIALIZING, - MODE_OPERATING - } - driver_mode_t; - - // other variables - driver_mode_t driver_mode_; ///< driver state machine mode (state) - int fw_version_major_; ///< firmware major version reported by vesc - int fw_version_minor_; ///< firmware minor version reported by vesc - - // ROS callbacks - void timerCallback(const ros::TimerEvent& event); - void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle); - void currentCallback(const std_msgs::Float64::ConstPtr& current); - void brakeCallback(const std_msgs::Float64::ConstPtr& brake); - void speedCallback(const std_msgs::Float64::ConstPtr& speed); - void positionCallback(const std_msgs::Float64::ConstPtr& position); - void servoCallback(const std_msgs::Float64::ConstPtr& servo); -}; - -} // namespace vesc_driver - -#endif // VESC_DRIVER_VESC_DRIVER_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp new file mode 100644 index 0000000..eddf44d --- /dev/null +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -0,0 +1,133 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// -*- mode:c++; fill-column: 100; -*- + +#ifndef VESC_DRIVER__VESC_DRIVER_HPP_ +#define VESC_DRIVER__VESC_DRIVER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet.hpp" + +namespace vesc_driver +{ + +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescState; +using vesc_msgs::msg::VescStateStamped; +using vesc_msgs::msg::VescImuStamped; +using sensor_msgs::msg::Imu; + +class VescDriver + : public rclcpp::Node +{ +public: + explicit VescDriver(const rclcpp::NodeOptions & options); + +private: + // interface to the VESC + VescInterface vesc_; + void vescPacketCallback(const std::shared_ptr & packet); + void vescErrorCallback(const std::string & error); + + // limits on VESC commands + struct CommandLimit + { + CommandLimit( + rclcpp::Node * node_ptr, + const std::string & str, + const std::experimental::optional & min_lower = std::experimental::optional(), + const std::experimental::optional & max_upper = + std::experimental::optional()); + double clip(double value); + rclcpp::Node * node_ptr; + rclcpp::Logger logger; + std::string name; + std::experimental::optional lower; + std::experimental::optional upper; + }; + + CommandLimit duty_cycle_limit_; + CommandLimit current_limit_; + CommandLimit brake_limit_; + CommandLimit speed_limit_; + CommandLimit position_limit_; + CommandLimit servo_limit_; + + // ROS services + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr imu_std_pub_; + + rclcpp::Publisher::SharedPtr servo_sensor_pub_; + rclcpp::SubscriptionBase::SharedPtr duty_cycle_sub_; + rclcpp::SubscriptionBase::SharedPtr current_sub_; + rclcpp::SubscriptionBase::SharedPtr brake_sub_; + rclcpp::SubscriptionBase::SharedPtr speed_sub_; + rclcpp::SubscriptionBase::SharedPtr position_sub_; + rclcpp::SubscriptionBase::SharedPtr servo_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // driver modes (possible states) + typedef enum + { + MODE_INITIALIZING, + MODE_OPERATING + } + driver_mode_t; + + // other variables + driver_mode_t driver_mode_; ///< driver state machine mode (state) + int fw_version_major_; ///< firmware major version reported by vesc + int fw_version_minor_; ///< firmware minor version reported by vesc + + // ROS callbacks + void brakeCallback(const Float64::SharedPtr brake); + void currentCallback(const Float64::SharedPtr current); + void dutyCycleCallback(const Float64::SharedPtr duty_cycle); + void positionCallback(const Float64::SharedPtr position); + void servoCallback(const Float64::SharedPtr servo); + void speedCallback(const Float64::SharedPtr speed); + void timerCallback(); +}; + +} // namespace vesc_driver + +#endif // VESC_DRIVER__VESC_DRIVER_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.hpp similarity index 52% rename from vesc_driver/include/vesc_driver/vesc_interface.h rename to vesc_driver/include/vesc_driver/vesc_interface.hpp index 558ec2b..dcca50f 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.hpp @@ -1,32 +1,37 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_INTERFACE_H_ -#define VESC_DRIVER_VESC_INTERFACE_H_ +#ifndef VESC_DRIVER__VESC_INTERFACE_HPP_ +#define VESC_DRIVER__VESC_INTERFACE_HPP_ + +#include "vesc_driver/vesc_packet.hpp" #include #include @@ -35,8 +40,6 @@ #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { @@ -46,8 +49,8 @@ namespace vesc_driver class VescInterface { public: - typedef std::function PacketHandlerFunction; - typedef std::function ErrorHandlerFunction; + typedef std::function PacketHandlerFunction; + typedef std::function ErrorHandlerFunction; /** * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not @@ -60,9 +63,10 @@ class VescInterface * * @throw SerialException */ - VescInterface(const std::string& port = std::string(), - const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), - const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); + VescInterface( + const std::string & port = std::string(), + const PacketHandlerFunction & packet_handler = PacketHandlerFunction(), + const ErrorHandlerFunction & error_handler = ErrorHandlerFunction()); /** * Delete copy constructor and equals operator. @@ -78,20 +82,20 @@ class VescInterface /** * Sets / updates the function that this class calls when a VESC packet is received. */ - void setPacketHandler(const PacketHandlerFunction& handler); + void setPacketHandler(const PacketHandlerFunction & handler); /** * Sets / updates the function that this class calls when an error is detected, such as a bad * checksum. */ - void setErrorHandler(const ErrorHandlerFunction& handler); + void setErrorHandler(const ErrorHandlerFunction & handler); /** * Opens the serial port interface to the VESC. * * @throw SerialException */ - void connect(const std::string& port); + void connect(const std::string & port); /** * Closes the serial port interface to the VESC. @@ -108,10 +112,12 @@ class VescInterface /** * Send a VESC packet. */ - void send(const VescPacket& packet); + void send(const VescPacket & packet); void requestFWVersion(); void requestState(); + void requestImuData(); + void setDutyCycle(double duty_cycle); void setCurrent(double current); void setBrake(double brake); @@ -129,18 +135,20 @@ class VescInterface class SerialException : public std::exception { // Disable copy constructors - SerialException& operator=(const SerialException&); + SerialException & operator=(const SerialException &); std::string e_what_; + public: - explicit SerialException(const char *description) + explicit SerialException(const char * description) { std::stringstream ss; ss << "SerialException " << description << " failed."; e_what_ = ss.str(); } - SerialException(const SerialException& other) : e_what_(other.e_what_) {} + SerialException(const SerialException & other) + : e_what_(other.e_what_) {} virtual ~SerialException() throw() {} - virtual const char* what() const throw() + virtual const char * what() const throw() { return e_what_.c_str(); } @@ -148,4 +156,4 @@ class SerialException : public std::exception } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_INTERFACE_H_ +#endif // VESC_DRIVER__VESC_INTERFACE_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.hpp similarity index 51% rename from vesc_driver/include/vesc_driver/vesc_packet.h rename to vesc_driver/include/vesc_driver/vesc_packet.hpp index 307e2f5..950ad91 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -1,32 +1,35 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_PACKET_H_ -#define VESC_DRIVER_VESC_PACKET_H_ +#ifndef VESC_DRIVER__VESC_PACKET_HPP_ +#define VESC_DRIVER__VESC_PACKET_HPP_ #include #include @@ -35,7 +38,7 @@ #include #define CRCPP_USE_CPP11 -#include "vesc_driver/crc.h" +#include "vesc_driver/crc.hpp" namespace vesc_driver { @@ -51,7 +54,7 @@ class VescFrame virtual ~VescFrame() {} // getters - virtual const Buffer& frame() const + virtual const Buffer & frame() const { return *frame_; } @@ -59,13 +62,15 @@ class VescFrame // VESC packet properties static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes - static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes + // v Largest VESC frame size, in bytes + static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value /** CRC parameters for the VESC */ - static constexpr CRC::Parameters CRC_TYPE = { 0x1021, 0x0000, 0x0000, false, false }; + static constexpr CRC::Parameters CRC_TYPE = {0x1021, 0x0000, 0x0000, false, false}; protected: /** Construct frame with specified payload size. */ @@ -76,7 +81,7 @@ class VescFrame private: /** Construct from buffer. Used by VescPacketFactory factory. */ - VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload); + VescFrame(const BufferRangeConst & frame, const BufferRangeConst & payload); /** Give VescPacketFactory access to private constructor. */ friend class VescPacketFactory; @@ -90,14 +95,14 @@ class VescPacket : public VescFrame public: virtual ~VescPacket() {} - virtual const std::string& name() const + virtual const std::string & name() const { return name_; } protected: - VescPacket(const std::string& name, int payload_size, int payload_id); - VescPacket(const std::string& name, std::shared_ptr raw); + VescPacket(const std::string & name, int payload_size, int payload_id); + VescPacket(const std::string & name, std::shared_ptr raw); private: std::string name_; @@ -115,6 +120,19 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; + + std::string hwname() const; + const uint8_t * uuid() const; + bool paired() const; + uint8_t devVersion() const; + +private: + int minor_; + int major_; + std::string hwname_; + bool paired_; + uint8_t uuid_[12]; + uint8_t devVersion_; }; class VescPacketRequestFWVersion : public VescPacket @@ -130,25 +148,31 @@ class VescPacketValues : public VescPacket public: explicit VescPacketValues(std::shared_ptr raw); - double v_in() const; - double temp_mos1() const; - double temp_mos2() const; - double temp_mos3() const; - double temp_mos4() const; - double temp_mos5() const; - double temp_mos6() const; - double temp_pcb() const; - double current_motor() const; - double current_in() const; - double rpm() const; - double duty_now() const; - double amp_hours() const; - double amp_hours_charged() const; - double watt_hours() const; - double watt_hours_charged() const; - double tachometer() const; - double tachometer_abs() const; - int fault_code() const; + double temp_fet() const; + double temp_motor() const; + double avg_motor_current() const; + double avg_input_current() const; + double avg_id() const; + double avg_iq() const; + double duty_cycle_now() const; + double rpm() const; + double duty_now() const; + double v_in() const; + double amp_hours() const; + double amp_hours_charged() const; + double watt_hours() const; + double watt_hours_charged() const; + int32_t tachometer() const; + int32_t tachometer_abs() const; + int fault_code() const; + double pid_pos_now() const; + int32_t controller_id() const; + + double temp_mos1() const; + double temp_mos2() const; + double temp_mos3() const; + double avg_vd() const; + double avg_vq() const; }; class VescPacketRequestValues : public VescPacket @@ -156,7 +180,6 @@ class VescPacketRequestValues : public VescPacket public: VescPacketRequestValues(); }; - /*------------------------------------------------------------------------------------------------*/ class VescPacketSetDuty : public VescPacket @@ -217,6 +240,68 @@ class VescPacketSetServoPos : public VescPacket // double servo_pos() const; }; +/*------------------------------------------------------------------------------------------------*/ +class VescPacketRequestImu : public VescPacket +{ +public: + VescPacketRequestImu(); +}; + +class VescPacketImu : public VescPacket +{ +public: + explicit VescPacketImu(std::shared_ptr raw); + + int mask() const; + + double yaw() const; + double pitch() const; + double roll() const; + + double acc_x() const; + double acc_y() const; + double acc_z() const; + + double gyr_x() const; + double gyr_y() const; + double gyr_z() const; + + double mag_x() const; + double mag_y() const; + double mag_z() const; + + double q_w() const; + double q_x() const; + double q_y() const; + double q_z() const; + +private: + double getFloat32Auto(uint32_t * pos) const; + + uint32_t mask_; + double roll_; + double pitch_; + double yaw_; + + double acc_x_; + double acc_y_; + double acc_z_; + + double gyr_x_; + double gyr_y_; + double gyr_z_; + + double mag_x_; + double mag_y_; + double mag_z_; + + double q0_; + double q1_; + double q2_; + double q3_; +}; + +/*------------------------------------------------------------------------------------------------*/ } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_H_ +#endif // VESC_DRIVER__VESC_PACKET_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp similarity index 58% rename from vesc_driver/include/vesc_driver/vesc_packet_factory.h rename to vesc_driver/include/vesc_driver/vesc_packet_factory.hpp index 643d6f9..0322d81 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.h +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp @@ -1,32 +1,37 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ -#define VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#ifndef VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ +#define VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ + +#include "vesc_driver/vesc_packet.hpp" #include #include @@ -35,8 +40,6 @@ #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { @@ -47,7 +50,7 @@ class VescPacketFactory { public: /** Return the global factory object */ - static VescPacketFactory* getFactory(); + static VescPacketFactory * getFactory(); /** * Create a VescPacket from a buffer (factory function). Packet must start (start of frame @@ -69,9 +72,10 @@ class VescPacketFactory * * @return Pointer to a valid VescPacket if successful. Otherwise, an empty pointer. */ - static VescPacketPtr createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, - int* num_bytes_needed, std::string* what); + static VescPacketPtr createPacket( + const Buffer::const_iterator & begin, + const Buffer::const_iterator & end, + int * num_bytes_needed, std::string * what); typedef std::function)> CreateFn; @@ -86,8 +90,8 @@ class VescPacketFactory private: VescPacketFactory(); - typedef std::map FactoryMap; - static FactoryMap* getMap(); + typedef std::map FactoryMap; + static FactoryMap * getMap(); }; template @@ -107,8 +111,8 @@ class PacketFactoryTemplate /** Use this macro to register packets */ #define REGISTER_PACKET_TYPE(id, klass) \ -static PacketFactoryTemplate global_##klass##Factory((id)); + static PacketFactoryTemplate global_ ## klass ## Factory((id)); } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#endif // VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch deleted file mode 100644 index f7d4765..0000000 --- a/vesc_driver/launch/vesc_driver_node.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py new file mode 100644 index 0000000..b9777e7 --- /dev/null +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -0,0 +1,58 @@ +# Copyright 2020 F1TENTH Foundation +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + vesc_config = os.path.join( + get_package_share_directory('vesc_driver'), + 'params', + 'vesc_config.yaml' + ) + return LaunchDescription([ + DeclareLaunchArgument( + name="config", + default_value=vesc_config, + description="VESC yaml configuration file.", + ), + Node( + package='vesc_driver', + executable='vesc_driver_node', + name='vesc_driver_node', + parameters=[LaunchConfiguration("config")] + ), + + ]) diff --git a/vesc_driver/launch/vesc_driver_nodelet.launch b/vesc_driver/launch/vesc_driver_nodelet.launch deleted file mode 100644 index c9f4508..0000000 --- a/vesc_driver/launch/vesc_driver_nodelet.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 8b58cc4..c40e286 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -13,18 +13,23 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley + Haoru Xue - catkin - roslint + ament_cmake_auto - nodelet - pluginlib - roscpp - serial + rclcpp + rclcpp_components std_msgs + geometry_msgs vesc_msgs + serial_driver + sensor_msgs + + + ament_lint_auto + ament_lint_common - + ament_cmake diff --git a/vesc_driver/params/vesc_config.yaml b/vesc_driver/params/vesc_config.yaml new file mode 100644 index 0000000..98d2a32 --- /dev/null +++ b/vesc_driver/params/vesc_config.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + port: "/dev/ttyACM0" + brake_max: 200000.0 + brake_min: -20000.0 + current_max: 100.0 + current_min: 0.0 + duty_cycle_max: 0.0 + duty_cycle_min: 0.0 + position_max: 0.0 + position_min: 0.0 + servo_max: 0.85 + servo_min: 0.15 + speed_max: 23250.0 + speed_min: -23250.0 diff --git a/vesc_driver/scripts/99-vesc6.rules b/vesc_driver/scripts/99-vesc6.rules new file mode 100644 index 0000000..839c7d2 --- /dev/null +++ b/vesc_driver/scripts/99-vesc6.rules @@ -0,0 +1,9 @@ +# Device vesc6 udev definition +# idVendor 0x0403 +# idProduct 0x5740 + + +# Alias the Sparkfun 9DoF as imu, and the electronic speed controller as VESC +#ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="1b4f", ATTRS{idProduct}=="9d0f", SYMLINK+="imu" , GROUP="dialout" +#ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", SYMLINK+="vesc%n", GROUP="dialout" +ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",PROGRAM="/bin/vesc_device_lookup %k", SYMLINK+="vesc/%c", GROUP="dialout" \ No newline at end of file diff --git a/vesc_driver/scripts/vesc_device_lookup b/vesc_driver/scripts/vesc_device_lookup new file mode 100755 index 0000000..59da038 --- /dev/null +++ b/vesc_driver/scripts/vesc_device_lookup @@ -0,0 +1,5 @@ +dme#!/bin/bash + +export LD_LIBRARY_PATH=/opt/ros/foxy/lib + +vesc_device_namer $1 1000 diff --git a/vesc_driver/src/vesc_device_namer.cpp b/vesc_driver/src/vesc_device_namer.cpp new file mode 100644 index 0000000..b50c846 --- /dev/null +++ b/vesc_driver/src/vesc_device_namer.cpp @@ -0,0 +1,47 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ + +#include +#include + +#include +#include + +#include + +int main(int argc, char ** argv) +{ + std::string devicePort = (argc > 1 ? argv[1] : "ttyACM0"); + std::string maxRetry_ = (argc > 2 ? argv[2] : "50"); + std::string VESC_UUID_ENV = "VESC_UUID_ENV="; + + vesc_driver::VescDeviceLookup lookup("/dev/" + devicePort); + int maxRetry = stoi(maxRetry_); + for (int i = 0; i < maxRetry; i++) { + usleep(20); + if (lookup.isReady()) {break;} + } + + if (lookup.isReady()) { + VESC_UUID_ENV += lookup.deviceUUID(); + + std::cout << lookup.deviceUUID() << std::endl; + setenv("VESC_UUID_ENV", lookup.deviceUUID(), true); + return 0; + } else { + return -1; + } +} diff --git a/vesc_driver/src/vesc_device_uuid_lookup.cpp b/vesc_driver/src/vesc_device_uuid_lookup.cpp new file mode 100644 index 0000000..11d3e45 --- /dev/null +++ b/vesc_driver/src/vesc_device_uuid_lookup.cpp @@ -0,0 +1,96 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ +#include +#include +#include +#include +#include +#include +#include + +#include "vesc_driver/vesc_device_uuid_lookup.hpp" + + +namespace vesc_driver +{ +using std::placeholders::_1; + +VescDeviceLookup::VescDeviceLookup(std::string name) +: vesc_( + std::string(), + std::bind(&VescDeviceLookup::vescPacketCallback, this, _1), + std::bind(&VescDeviceLookup::vescErrorCallback, this, _1) +), + ready_(false), + device_(name) +{ + try { + vesc_.connect(device_); + vesc_.requestFWVersion(); + } catch (SerialException e) { + std::cerr << "VESC error on port " << device_ << std::endl << e.what() << std::endl; + return; + } +} + +void VescDeviceLookup::close() +{ + vesc_.disconnect(); +} + +void VescDeviceLookup::vescPacketCallback(const std::shared_ptr & packet) +{ + if (packet->name() == "FWVersion") { + std::shared_ptr fw_version = + std::dynamic_pointer_cast(packet); + + const uint8_t * uuid = fw_version->uuid(); + + hwname_ = fw_version->hwname(); + version_ = fw_version->fwMajor() + "." + fw_version->fwMinor(); + uuid_ += decode_uuid(uuid); + ready_ = true; + } +} + +void VescDeviceLookup::vescErrorCallback(const std::string & error) +{ + error_ = error; + ready_ = false; +} + +bool VescDeviceLookup::isReady() +{ + return ready_; +} + +const char * VescDeviceLookup::deviceUUID() const +{ + return uuid_.c_str(); +} + + +const char * VescDeviceLookup::version() const +{ + return version_.c_str(); +} + +const char * VescDeviceLookup::hwname() const +{ + return hwname_.c_str(); +} + +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 95371eb..5c8ef62 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -1,94 +1,110 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_driver.h" +#include "vesc_driver/vesc_driver.hpp" + +#include +#include #include +#include #include #include #include #include -#include - namespace vesc_driver { +using namespace std::chrono_literals; using std::placeholders::_1; - -VescDriver::VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh) : - vesc_(std::string(), - std::bind(&VescDriver::vescPacketCallback, this, _1), - std::bind(&VescDriver::vescErrorCallback, this, _1)), - duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0), current_limit_(private_nh, "current"), - brake_limit_(private_nh, "brake"), speed_limit_(private_nh, "speed"), - position_limit_(private_nh, "position"), servo_limit_(private_nh, "servo", 0.0, 1.0), - driver_mode_(MODE_INITIALIZING), fw_version_major_(-1), fw_version_minor_(-1) +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescStateStamped; +using sensor_msgs::msg::Imu; + +VescDriver::VescDriver(const rclcpp::NodeOptions & options) +: rclcpp::Node("vesc_driver", options), + vesc_( + std::string(), + std::bind(&VescDriver::vescPacketCallback, this, _1), + std::bind(&VescDriver::vescErrorCallback, this, _1)), + duty_cycle_limit_(this, "duty_cycle", -1.0, 1.0), + current_limit_(this, "current"), + brake_limit_(this, "brake"), + speed_limit_(this, "speed"), + position_limit_(this, "position"), + servo_limit_(this, "servo", 0.0, 1.0), + driver_mode_(MODE_INITIALIZING), + fw_version_major_(-1), + fw_version_minor_(-1) { // get vesc serial port address - std::string port; - if (!private_nh.getParam("port", port)) - { - ROS_FATAL("VESC communication port parameter required."); - ros::shutdown(); - return; - } + std::string port = declare_parameter("port", ""); // attempt to connect to the serial port - try - { + try { vesc_.connect(port); - } - catch (SerialException e) - { - ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); - ros::shutdown(); + } catch (SerialException e) { + RCLCPP_FATAL(get_logger(), "Failed to connect to the VESC, %s.", e.what()); + rclcpp::shutdown(); return; } // create vesc state (telemetry) publisher - state_pub_ = nh.advertise("sensors/core", 10); + state_pub_ = create_publisher("sensors/core", rclcpp::QoS{10}); + imu_pub_ = create_publisher("sensors/imu", rclcpp::QoS{10}); + imu_std_pub_ = create_publisher("sensors/imu/raw", rclcpp::QoS{10}); // since vesc state does not include the servo position, publish the commanded // servo position as a "sensor" - servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); + servo_sensor_pub_ = create_publisher( + "sensors/servo_position_command", rclcpp::QoS{10}); // subscribe to motor and servo command topics - duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, - &VescDriver::dutyCycleCallback, this); - current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); - brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); - speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); - position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); - servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); + duty_cycle_sub_ = create_subscription( + "commands/motor/duty_cycle", rclcpp::QoS{10}, std::bind( + &VescDriver::dutyCycleCallback, this, + _1)); + current_sub_ = create_subscription( + "commands/motor/current", rclcpp::QoS{10}, std::bind(&VescDriver::currentCallback, this, _1)); + brake_sub_ = create_subscription( + "commands/motor/brake", rclcpp::QoS{10}, std::bind(&VescDriver::brakeCallback, this, _1)); + speed_sub_ = create_subscription( + "commands/motor/speed", rclcpp::QoS{10}, std::bind(&VescDriver::speedCallback, this, _1)); + position_sub_ = create_subscription( + "commands/motor/position", rclcpp::QoS{10}, std::bind(&VescDriver::positionCallback, this, _1)); + servo_sub_ = create_subscription( + "commands/servo/position", rclcpp::QoS{10}, std::bind(&VescDriver::servoCallback, this, _1)); // create a 50Hz timer, used for state machine & polling VESC telemetry - timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); + timer_ = create_wall_timer(20ms, std::bind(&VescDriver::timerCallback, this)); } /* TODO or TO-THINKABOUT LIST @@ -104,14 +120,12 @@ VescDriver::VescDriver(ros::NodeHandle nh, - try to predict vesc bounds (from vesc config) and command detect bounds errors */ -void VescDriver::timerCallback(const ros::TimerEvent& event) +void VescDriver::timerCallback() { // VESC interface should not unexpectedly disconnect, but test for it anyway - if (!vesc_.isConnected()) - { - ROS_FATAL("Unexpectedly disconnected from serial port."); - timer_.stop(); - ros::shutdown(); + if (!vesc_.isConnected()) { + RCLCPP_FATAL(get_logger(), "Unexpectedly disconnected from serial port."); + rclcpp::shutdown(); return; } @@ -120,67 +134,133 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) * INITIALIZING - request and wait for vesc version * OPERATING - receiving commands from subscriber topics */ - if (driver_mode_ == MODE_INITIALIZING) - { + if (driver_mode_ == MODE_INITIALIZING) { // request version number, return packet will update the internal version numbers vesc_.requestFWVersion(); - if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) - { - ROS_INFO("Connected to VESC with firmware version %d.%d", - fw_version_major_, fw_version_minor_); + if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) { + RCLCPP_INFO( + get_logger(), "Connected to VESC with firmware version %d.%d", + fw_version_major_, fw_version_minor_); driver_mode_ = MODE_OPERATING; } - } - else if (driver_mode_ == MODE_OPERATING) - { + } else if (driver_mode_ == MODE_OPERATING) { // poll for vesc state (telemetry) vesc_.requestState(); - } - else - { + // poll for vesc imu + vesc_.requestImuData(); + } else { // unknown mode, how did that happen? assert(false && "unknown driver mode"); } } -void VescDriver::vescPacketCallback(const std::shared_ptr& packet) +void VescDriver::vescPacketCallback(const std::shared_ptr & packet) { - if (packet->name() == "Values") - { + if (packet->name() == "Values") { std::shared_ptr values = std::dynamic_pointer_cast(packet); - vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); - state_msg->header.stamp = ros::Time::now(); - state_msg->state.voltage_input = values->v_in(); - state_msg->state.temperature_pcb = values->temp_pcb(); - state_msg->state.current_motor = values->current_motor(); - state_msg->state.current_input = values->current_in(); - state_msg->state.speed = values->rpm(); - state_msg->state.duty_cycle = values->duty_now(); - state_msg->state.charge_drawn = values->amp_hours(); - state_msg->state.charge_regen = values->amp_hours_charged(); - state_msg->state.energy_drawn = values->watt_hours(); - state_msg->state.energy_regen = values->watt_hours_charged(); - state_msg->state.displacement = values->tachometer(); - state_msg->state.distance_traveled = values->tachometer_abs(); - state_msg->state.fault_code = values->fault_code(); - - state_pub_.publish(state_msg); - } - else if (packet->name() == "FWVersion") - { + auto state_msg = VescStateStamped(); + state_msg.header.stamp = now(); + + state_msg.state.voltage_input = values->v_in(); + state_msg.state.current_motor = values->avg_motor_current(); + state_msg.state.current_input = values->avg_input_current(); + state_msg.state.avg_id = values->avg_id(); + state_msg.state.avg_iq = values->avg_iq(); + state_msg.state.duty_cycle = values->duty_cycle_now(); + state_msg.state.speed = values->rpm(); + + state_msg.state.charge_drawn = values->amp_hours(); + state_msg.state.charge_regen = values->amp_hours_charged(); + state_msg.state.energy_drawn = values->watt_hours(); + state_msg.state.energy_regen = values->watt_hours_charged(); + state_msg.state.displacement = values->tachometer(); + state_msg.state.distance_traveled = values->tachometer_abs(); + state_msg.state.fault_code = values->fault_code(); + + state_msg.state.pid_pos_now = values->pid_pos_now(); + state_msg.state.controller_id = values->controller_id(); + + state_msg.state.ntc_temp_mos1 = values->temp_mos1(); + state_msg.state.ntc_temp_mos2 = values->temp_mos2(); + state_msg.state.ntc_temp_mos3 = values->temp_mos3(); + state_msg.state.avg_vd = values->avg_vd(); + state_msg.state.avg_vq = values->avg_vq(); + + state_pub_->publish(state_msg); + } else if (packet->name() == "FWVersion") { std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); + RCLCPP_INFO( + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); + } else if (packet->name() == "ImuData") { + std::shared_ptr imuData = + std::dynamic_pointer_cast(packet); + + auto imu_msg = VescImuStamped(); + auto std_imu_msg = Imu(); + imu_msg.header.stamp = now(); + std_imu_msg.header.stamp = now(); + + imu_msg.imu.ypr.x = imuData->roll(); + imu_msg.imu.ypr.y = imuData->pitch(); + imu_msg.imu.ypr.z = imuData->yaw(); + + imu_msg.imu.linear_acceleration.x = imuData->acc_x(); + imu_msg.imu.linear_acceleration.y = imuData->acc_y(); + imu_msg.imu.linear_acceleration.z = imuData->acc_z(); + + imu_msg.imu.angular_velocity.x = imuData->gyr_x(); + imu_msg.imu.angular_velocity.y = imuData->gyr_y(); + imu_msg.imu.angular_velocity.z = imuData->gyr_z(); + + imu_msg.imu.compass.x = imuData->mag_x(); + imu_msg.imu.compass.y = imuData->mag_y(); + imu_msg.imu.compass.z = imuData->mag_z(); + + imu_msg.imu.orientation.w = imuData->q_w(); + imu_msg.imu.orientation.x = imuData->q_x(); + imu_msg.imu.orientation.y = imuData->q_y(); + imu_msg.imu.orientation.z = imuData->q_z(); + + std_imu_msg.linear_acceleration.x = imuData->acc_x(); + std_imu_msg.linear_acceleration.y = imuData->acc_y(); + std_imu_msg.linear_acceleration.z = imuData->acc_z(); + + std_imu_msg.angular_velocity.x = imuData->gyr_x(); + std_imu_msg.angular_velocity.y = imuData->gyr_y(); + std_imu_msg.angular_velocity.z = imuData->gyr_z(); + + std_imu_msg.orientation.w = imuData->q_w(); + std_imu_msg.orientation.x = imuData->q_x(); + std_imu_msg.orientation.y = imuData->q_y(); + std_imu_msg.orientation.z = imuData->q_z(); + + + imu_pub_->publish(imu_msg); + imu_std_pub_->publish(std_imu_msg); } + auto & clk = *this->get_clock(); + RCLCPP_DEBUG_THROTTLE( + get_logger(), + clk, + 5000, + "%s packet received", + packet->name().c_str() + ); } -void VescDriver::vescErrorCallback(const std::string& error) +void VescDriver::vescErrorCallback(const std::string & error) { - ROS_ERROR("%s", error.c_str()); + RCLCPP_ERROR(get_logger(), "%s", error.c_str()); } /** @@ -188,10 +268,9 @@ void VescDriver::vescErrorCallback(const std::string& error) * note that the VESC may impose a more restrictive bounds on the range depending * on its configuration, e.g. absolute value is between 0.05 and 0.95. */ -void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) +void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } } @@ -201,10 +280,9 @@ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle * note that the VESC may impose a more restrictive bounds on the range depending on * its configuration. */ -void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) +void VescDriver::currentCallback(const Float64::SharedPtr current) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { vesc_.setCurrent(current_limit_.clip(current->data)); } } @@ -214,10 +292,9 @@ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) * However, note that the VESC may impose a more restrictive bounds on the range * depending on its configuration. */ -void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) +void VescDriver::brakeCallback(const Float64::SharedPtr brake) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { vesc_.setBrake(brake_limit_.clip(brake->data)); } } @@ -228,10 +305,9 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) * driver. However, note that the VESC may impose a more restrictive bounds on the * range depending on its configuration. */ -void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) +void VescDriver::speedCallback(const Float64::SharedPtr speed) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { vesc_.setSpeed(speed_limit_.clip(speed->data)); } } @@ -240,10 +316,9 @@ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. * Note that the VESC must be in encoder mode for this command to have an effect. */ -void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) +void VescDriver::positionCallback(const Float64::SharedPtr position) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { // ROS uses radians but VESC seems to use degrees. Convert to degrees. double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; vesc_.setPosition(position_deg); @@ -253,81 +328,76 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) /** * @param servo Commanded VESC servo output position. Valid range is 0 to 1. */ -void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) +void VescDriver::servoCallback(const Float64::SharedPtr servo) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ == MODE_OPERATING) { double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); // publish clipped servo value as a "sensor" - std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); - servo_sensor_msg->data = servo_clipped; - servo_sensor_pub_.publish(servo_sensor_msg); + auto servo_sensor_msg = Float64(); + servo_sensor_msg.data = servo_clipped; + servo_sensor_pub_->publish(servo_sensor_msg); } } -VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, - const boost::optional& min_lower, - const boost::optional& max_upper) : +VescDriver::CommandLimit::CommandLimit( + rclcpp::Node * node_ptr, + const std::string & str, + const std::experimental::optional & min_lower, + const std::experimental::optional & max_upper) +: node_ptr(node_ptr), + logger(node_ptr->get_logger()), name(str) { // check if user's minimum value is outside of the range min_lower to max_upper - double param_min; - if (nh.getParam(name + "_min", param_min)) - { - if (min_lower && param_min < *min_lower) - { + auto param_min = + node_ptr->declare_parameter(name + "_min", rclcpp::ParameterValue(0.0)); + + if (param_min.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + if (min_lower && param_min.get() < *min_lower) { lower = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_min > *max_upper) - { + RCLCPP_WARN_STREAM( + logger, "Parameter " << name << "_min (" << param_min.get() << + ") is less than the feasible minimum (" << *min_lower << ")."); + } else if (max_upper && param_min.get() > *max_upper) { lower = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is greater than the feasible maximum (" << *max_upper << ")."); - } - else - { - lower = param_min; + RCLCPP_WARN_STREAM( + logger, "Parameter " << name << "_min (" << param_min.get() << + ") is greater than the feasible maximum (" << *max_upper << ")."); + } else { + lower = param_min.get(); } - } - else if (min_lower) - { + } else if (min_lower) { lower = *min_lower; } // check if the uers' maximum value is outside of the range min_lower to max_upper - double param_max; - if (nh.getParam(name + "_max", param_max)) - { - if (min_lower && param_max < *min_lower) - { + auto param_max = + node_ptr->declare_parameter(name + "_max", rclcpp::ParameterValue(0.0)); + + if (param_max.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + if (min_lower && param_max.get() < *min_lower) { upper = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_max > *max_upper) - { + RCLCPP_WARN_STREAM( + logger, "Parameter " << name << "_max (" << param_max.get() << + ") is less than the feasible minimum (" << *min_lower << ")."); + } else if (max_upper && param_max.get() > *max_upper) { upper = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is greater than the feasible maximum (" << *max_upper << ")."); + RCLCPP_WARN_STREAM( + logger, "Parameter " << name << "_max (" << param_max.get() << + ") is greater than the feasible maximum (" << *max_upper << ")."); + } else { + upper = param_max.get(); } - else - { - upper = param_max; - } - } - else if (max_upper) - { + } else if (max_upper) { upper = *max_upper; } // check for min > max - if (upper && lower && *lower > *upper) - { - ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper - << ") is less than parameter " << name << "_min (" << *lower << ")."); + if (upper && lower && *lower > *upper) { + RCLCPP_WARN_STREAM( + logger, "Parameter " << name << "_max (" << *upper << + ") is less than parameter " << name << "_min (" << *lower << ")."); double temp(*lower); lower = *upper; upper = temp; @@ -335,29 +405,43 @@ VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::str std::ostringstream oss; oss << " " << name << " limit: "; - if (lower) oss << *lower << " "; - else oss << "(none) "; - if (upper) oss << *upper; - else oss << "(none)"; - ROS_DEBUG_STREAM(oss.str()); + + if (lower) { + oss << *lower << " "; + } else { + oss << "(none) "; + } + + if (upper) { + oss << *upper; + } else { + oss << "(none)"; + } + + RCLCPP_DEBUG_STREAM(logger, oss.str()); } double VescDriver::CommandLimit::clip(double value) { - if (lower && value < lower) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", - name.c_str(), value, *lower); + auto clock = rclcpp::Clock(RCL_ROS_TIME); + + if (lower && value < lower) { + RCLCPP_INFO_THROTTLE( + logger, clock, 10, "%s command value (%f) below minimum limit (%f), clipping.", + name.c_str(), value, *lower); return *lower; } - if (upper && value > upper) - { - ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", - name.c_str(), value, *upper); + if (upper && value > upper) { + RCLCPP_INFO_THROTTLE( + logger, clock, 10, "%s command value (%f) above maximum limit (%f), clipping.", + name.c_str(), value, *upper); return *upper; } return value; } - } // namespace vesc_driver + +#include "rclcpp_components/register_node_macro.hpp" // NOLINT + +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_driver::VescDriver) diff --git a/vesc_driver/src/vesc_driver_node.cpp b/vesc_driver/src/vesc_driver_node.cpp deleted file mode 100644 index 2c61547..0000000 --- a/vesc_driver/src/vesc_driver_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_driver/vesc_driver.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "vesc_driver_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_driver::VescDriver vesc_driver(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_driver/src/vesc_driver_nodelet.cpp b/vesc_driver/src/vesc_driver_nodelet.cpp deleted file mode 100644 index 34134af..0000000 --- a/vesc_driver/src/vesc_driver_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_driver/vesc_driver.h" - -namespace vesc_driver -{ - -class VescDriverNodelet: public nodelet::Nodelet -{ -public: - VescDriverNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr vesc_driver_; -}; // class VescDriverNodelet - -void VescDriverNodelet::onInit() -{ - NODELET_DEBUG("Initializing VESC driver nodelet"); - vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_driver - -PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 3febe1a..9557431 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -1,44 +1,48 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_interface.h" - -#include +#include "vesc_driver/vesc_interface.hpp" #include #include #include #include +#include +#include #include #include +#include +#include -#include - -#include "vesc_driver/vesc_packet_factory.h" +#include "vesc_driver/vesc_packet_factory.hpp" +#include "serial_driver/serial_driver.hpp" namespace vesc_driver { @@ -46,56 +50,61 @@ namespace vesc_driver class VescInterface::Impl { public: - Impl() : - serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100), - serial::eightbits, serial::parity_none, serial::stopbits_one, serial::flowcontrol_none) + Impl() + : owned_ctx{new IoContext(2)}, + serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} {} + void packet_creation_thread(); + void on_configure(); + void connect(const std::string & port); - void* rxThread(void); + bool packet_thread_run_; + std::unique_ptr packet_thread_; + PacketHandlerFunction packet_handler_; + ErrorHandlerFunction error_handler_; + std::unique_ptr device_config_; + std::string device_name_; + std::unique_ptr owned_ctx{}; + std::unique_ptr serial_driver_; - static void* rxThreadHelper(void *context) + ~Impl() { - return ((VescInterface::Impl*)context)->rxThread(); + if (owned_ctx) { + owned_ctx->waitForExit(); + } } - pthread_t rx_thread_; - bool rx_thread_run_; - PacketHandlerFunction packet_handler_; - ErrorHandlerFunction error_handler_; - serial::Serial serial_; +private: + std::vector buffer_; }; -void* VescInterface::Impl::rxThread(void) +void VescInterface::Impl::packet_creation_thread() { - Buffer buffer; - buffer.reserve(4096); - - while (rx_thread_run_) - { + static auto temp_buffer = Buffer(2048, 0); + while (packet_thread_run_) { + const auto bytes_read = serial_driver_->port()->receive(temp_buffer); + buffer_.reserve(buffer_.size() + temp_buffer.size()); + buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read); int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) - { + if (!buffer_.empty()) { // search buffer for valid packet(s) - Buffer::iterator iter(buffer.begin()); - Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) - { + auto iter = buffer_.begin(); + auto iter_begin = buffer_.begin(); + while (iter != buffer_.end()) { // check if valid start-of-frame character if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) { // good start, now attempt to create packet std::string error; VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) - { + VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); + if (packet) { // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " - << std::distance(iter_begin, iter) << " bytes."; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); } // call packet handler @@ -105,14 +114,10 @@ void* VescInterface::Impl::rxThread(void) iter_begin = iter; // continue to look for another frame in buffer continue; - } - else if (bytes_needed > 0) - { + } else if (bytes_needed > 0) { // need more data, break out of while loop break; // for (iter_sof... - } - else - { + } else { // else, this was not a packet, move on to next byte error_handler_(error); } @@ -122,41 +127,49 @@ void* VescInterface::Impl::rxThread(void) } // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer.end()) + if (iter == buffer_.end()) { bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + } // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); } - buffer.erase(buffer.begin(), iter); - } - - // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = - std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); - int bytes_read = serial_.read(buffer, bytes_to_read); - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) - { - error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); + buffer_.erase(buffer_.begin(), iter); } + // Only attempt to read every 5 ms + std::this_thread::sleep_for(std::chrono::milliseconds(5)); } } +void VescInterface::Impl::connect(const std::string & port) +{ + uint32_t baud_rate = 115200; + auto fc = drivers::serial_driver::FlowControl::HARDWARE; + auto pt = drivers::serial_driver::Parity::NONE; + auto sb = drivers::serial_driver::StopBits::ONE; + device_config_ = + std::make_unique(baud_rate, fc, pt, sb); + serial_driver_->init_port(port, *device_config_); + if (!serial_driver_->port()->is_open()) { + serial_driver_->port()->open(); + } +} -VescInterface::VescInterface(const std::string& port, - const PacketHandlerFunction& packet_handler, - const ErrorHandlerFunction& error_handler) : - impl_(new Impl()) +VescInterface::VescInterface( + const std::string & port, + const PacketHandlerFunction & packet_handler, + const ErrorHandlerFunction & error_handler) +: impl_(new Impl()) { setPacketHandler(packet_handler); setErrorHandler(error_handler); // attempt to conect if the port is specified - if (!port.empty()) + if (!port.empty()) { connect(port); + } } VescInterface::~VescInterface() @@ -164,76 +177,68 @@ VescInterface::~VescInterface() disconnect(); } -void VescInterface::setPacketHandler(const PacketHandlerFunction& handler) +void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) { // todo - definately need mutex impl_->packet_handler_ = handler; } -void VescInterface::setErrorHandler(const ErrorHandlerFunction& handler) +void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) { // todo - definately need mutex impl_->error_handler_ = handler; } -void VescInterface::connect(const std::string& port) +void VescInterface::connect(const std::string & port) { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { throw SerialException("Already connected to serial port."); } // connect to serial port - try - { - impl_->serial_.setPort(port); - impl_->serial_.open(); - } - catch (const std::exception& e) - { + try { + impl_->connect(port); + } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); + ss << "Failed to open the serial port " << port << " to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } // start up a monitoring thread - impl_->rx_thread_run_ = true; - int result = - pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); - assert(0 == result); + impl_->packet_thread_run_ = true; + impl_->packet_thread_ = std::unique_ptr( + new std::thread( + &VescInterface::Impl::packet_creation_thread, impl_.get())); } void VescInterface::disconnect() { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { // bring down read thread - impl_->rx_thread_run_ = false; - int result = pthread_join(impl_->rx_thread_, NULL); - assert(0 == result); - - impl_->serial_.close(); + impl_->packet_thread_run_ = false; + requestFWVersion(); + impl_->packet_thread_->join(); + impl_->serial_driver_->port()->close(); } } bool VescInterface::isConnected() const { - return impl_->serial_.isOpen(); + auto port = impl_->serial_driver_->port(); + if (port) { + return port->is_open(); + } else { + return false; + } } -void VescInterface::send(const VescPacket& packet) +void VescInterface::send(const VescPacket & packet) { - size_t written = impl_->serial_.write(packet.frame()); - if (written != packet.frame().size()) - { - std::stringstream ss; - ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; - throw SerialException(ss.str().c_str()); - } + impl_->serial_driver_->port()->async_send(packet.frame()); } void VescInterface::requestFWVersion() @@ -276,4 +281,9 @@ void VescInterface::setServo(double servo) send(VescPacketSetServoPos(servo)); } +void VescInterface::requestImuData() +{ + send(VescPacketRequestImu()); +} + } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 8e96365..b9ab26f 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -1,39 +1,44 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_packet.h" +#include "vesc_driver/vesc_packet.hpp" #include #include #include #include +#include + +#include "vesc_driver/datatypes.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" -#include "vesc_driver/vesc_packet_factory.h" -#include "vesc_driver/datatypes.h" namespace vesc_driver { @@ -44,16 +49,13 @@ VescFrame::VescFrame(int payload_size) { assert(payload_size >= 0 && payload_size <= 1024); - if (payload_size < 256) - { + if (payload_size < 256) { // single byte payload size frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); *frame_->begin() = 2; *(frame_->begin() + 1) = payload_size; payload_.first = frame_->begin() + 2; - } - else - { + } else { // two byte payload size frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); *frame_->begin() = 3; @@ -66,55 +68,89 @@ VescFrame::VescFrame(int payload_size) *(frame_->end() - 1) = 3; } -VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload) +VescFrame::VescFrame(const BufferRangeConst & frame, const BufferRangeConst & payload) { /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap checks anyway */ assert(std::distance(frame.first, frame.second) >= VESC_MIN_FRAME_SIZE); assert(std::distance(frame.first, frame.second) <= VESC_MAX_FRAME_SIZE); assert(std::distance(payload.first, payload.second) <= VESC_MAX_PAYLOAD_SIZE); - assert(std::distance(frame.first, payload.first) > 0 && - std::distance(payload.second, frame.second) > 0); + assert( + std::distance(frame.first, payload.first) > 0 && + std::distance(payload.second, frame.second) > 0); frame_.reset(new Buffer(frame.first, frame.second)); payload_.first = frame_->begin() + std::distance(frame.first, payload.first); payload_.second = frame_->begin() + std::distance(frame.first, payload.second); } -VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) : - VescFrame(payload_size), name_(name) +VescPacket::VescPacket(const std::string & name, int payload_size, int payload_id) +: VescFrame(payload_size), name_(name) { assert(payload_id >= 0 && payload_id < 256); assert(std::distance(payload_.first, payload_.second) > 0); *payload_.first = payload_id; } -VescPacket::VescPacket(const std::string& name, std::shared_ptr raw) : - VescFrame(*raw), name_(name) +VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) +: VescFrame(*raw), name_(name) { } /*------------------------------------------------------------------------------------------------*/ -VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : - VescPacket("FWVersion", raw) +VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) +: VescPacket("FWVersion", raw) { + major_ = *(payload_.first + 1); + minor_ = *(payload_.first + 2); + int j = 0; + + for (int i = 0; *(payload_.first + 3 + i) != 0x00; i++) { + j = i; + hwname_ += *(payload_.first + 3 + i); + } + j++; + + for (int u = 0; u < 12; u++) { + uuid_[u] = *(payload_.first + 3 + j + 1 + u); + } + + paired_ = *(payload_.first + 3 + j + 1 + 12); + devVersion_ = *(payload_.first + 3 + j + 2 + 12 + 1); } int VescPacketFWVersion::fwMajor() const { - return *(payload_.first + 1); + return major_; } int VescPacketFWVersion::fwMinor() const { - return *(payload_.first + 2); + return minor_; +} + +std::string VescPacketFWVersion::hwname() const +{ + return hwname_.c_str(); +} +const uint8_t * VescPacketFWVersion::uuid() const +{ + return uuid_; +} +bool VescPacketFWVersion::paired() const +{ + return paired_; +} +uint8_t VescPacketFWVersion::devVersion() const +{ + return devVersion_; } REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) -VescPacketRequestFWVersion::VescPacketRequestFWVersion() : - VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) +VescPacketRequestFWVersion::VescPacketRequestFWVersion() +: VescPacket("RequestFWVersion", 1, COMM_FW_VERSION) { uint16_t crc = CRC::Calculate( &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); @@ -124,129 +160,250 @@ VescPacketRequestFWVersion::VescPacketRequestFWVersion() : /*------------------------------------------------------------------------------------------------*/ -VescPacketValues::VescPacketValues(std::shared_ptr raw) : - VescPacket("Values", raw) +VescPacketValues::VescPacketValues(std::shared_ptr raw) +: VescPacket("Values", raw) { } - -double VescPacketValues::temp_mos1() const +double VescPacketValues::temp_fet() const { - int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 1)) << 8) + + (static_cast(*(payload_.first + 2)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::temp_mos2() const +double VescPacketValues::temp_motor() const { - int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 3)) << 8) + + (static_cast(*(payload_.first + 4)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::current_motor() const + +double VescPacketValues::avg_motor_current() const { - int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + - (static_cast(*(payload_.first + 6)) << 16) + - (static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + (static_cast(*(payload_.first + 8)) ) + ); return static_cast(v) / 100.0; } -double VescPacketValues::current_in() const + +double VescPacketValues::avg_input_current() const { - int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + - (static_cast(*(payload_.first + 10)) << 16) + - (static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + (static_cast(*(payload_.first + 12)) ) + ); return static_cast(v) / 100.0; } -double VescPacketValues::duty_now() const +double VescPacketValues::avg_id() const { - int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + (static_cast(*(payload_.first + 16)) ) + ); + return static_cast(v) / 100.0; +} + +double VescPacketValues::avg_iq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + (static_cast(*(payload_.first + 20)) ) + ); + return static_cast(v) / 100.0; +} + + +double VescPacketValues::duty_cycle_now() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 21)) << 8) + + (static_cast(*(payload_.first + 22)) ) + ); return static_cast(v) / 1000.0; } double VescPacketValues::rpm() const { - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); - return static_cast(-1 * v); + int32_t v = static_cast( + (static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + (static_cast(*(payload_.first + 26)) ) + ); + return static_cast(v) / 1.0; +} + +double VescPacketValues::v_in() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 27)) << 8) + + (static_cast(*(payload_.first + 28)) ) + ); + return static_cast(v) / 10.0; +} + +double VescPacketValues::amp_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + (static_cast(*(payload_.first + 32)) ) + ); + return static_cast(v) / 1e4; } -double VescPacketValues::amp_hours() const +double VescPacketValues::amp_hours_charged() const { - int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + - (static_cast(*(payload_.first + 28)) << 16) + - (static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v); + int32_t v = static_cast( + (static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + (static_cast(*(payload_.first + 36)) ) + ); + return static_cast(v) / 1e4; } -double VescPacketValues::amp_hours_charged() const +double VescPacketValues::watt_hours() const { - int32_t v = static_cast((static_cast(*(payload_.first + 31)) << 24) + - (static_cast(*(payload_.first + 32)) << 16) + - (static_cast(*(payload_.first + 33)) << 8) + - static_cast(*(payload_.first + 34))); - return static_cast(v); + int32_t v = static_cast( + (static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + (static_cast(*(payload_.first + 40)) ) + ); + return static_cast(v) / 1e4; } -double VescPacketValues::tachometer() const +double VescPacketValues::watt_hours_charged() const { - int32_t v = static_cast((static_cast(*(payload_.first + 35)) << 24) + - (static_cast(*(payload_.first + 36)) << 16) + - (static_cast(*(payload_.first + 37)) << 8) + - static_cast(*(payload_.first + 38))); - return static_cast(v); + int32_t v = static_cast( + (static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + (static_cast(*(payload_.first + 44)) ) + ); + return static_cast(v) / 1e4; } -double VescPacketValues::tachometer_abs() const +int32_t VescPacketValues::tachometer() const { - int32_t v = static_cast((static_cast(*(payload_.first + 39)) << 24) + - (static_cast(*(payload_.first + 40)) << 16) + - (static_cast(*(payload_.first + 41)) << 8) + - static_cast(*(payload_.first + 42))); - return static_cast(v); + int32_t v = static_cast( + (static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + (static_cast(*(payload_.first + 48)) ) + ); + return static_cast(v); +} + +int32_t VescPacketValues::tachometer_abs() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + (static_cast(*(payload_.first + 52)) ) + ); + return static_cast(v); } int VescPacketValues::fault_code() const { - return static_cast(*(payload_.first + 56)); + return static_cast(*(payload_.first + 53)); } -double VescPacketValues::v_in() const + +double VescPacketValues::pid_pos_now() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + (static_cast(*(payload_.first + 57)) ) + ); + return static_cast(v) / 1e6; +} + +int32_t VescPacketValues::controller_id() const { - int32_t v = 0; - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); + + return static_cast(v); } -double VescPacketValues::temp_pcb() const +double VescPacketValues::temp_mos1() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast( + (static_cast(*(payload_.first + 59)) << 8) + + (static_cast(*(payload_.first + 60)) ) + ); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours() const +double VescPacketValues::temp_mos2() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast( + (static_cast(*(payload_.first + 61)) << 8) + + (static_cast(*(payload_.first + 62)) ) + ); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours_charged() const +double VescPacketValues::temp_mos3() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast( + (static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)) ) + ); + return static_cast(v) / 10.0; } +double VescPacketValues::avg_vd() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 65)) << 24) + + (static_cast(*(payload_.first + 66)) << 16) + + (static_cast(*(payload_.first + 67)) << 8) + + (static_cast(*(payload_.first + 68)) ) + ); + return static_cast(v) / 1e3; +} + + +double VescPacketValues::avg_vq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 69)) << 24) + + (static_cast(*(payload_.first + 70)) << 16) + + (static_cast(*(payload_.first + 71)) << 8) + + (static_cast(*(payload_.first + 72)) ) + ); + return static_cast(v) / 1e3; +} + + REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) -VescPacketRequestValues::VescPacketRequestValues() : - VescPacket("RequestValues", 1, COMM_GET_VALUES) +VescPacketRequestValues::VescPacketRequestValues() +: VescPacket("RequestValues", 1, COMM_GET_VALUES) { uint16_t crc = CRC::Calculate( &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); @@ -257,8 +414,8 @@ VescPacketRequestValues::VescPacketRequestValues() : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetDuty::VescPacketSetDuty(double duty) : - VescPacket("SetDuty", 5, COMM_SET_DUTY) +VescPacketSetDuty::VescPacketSetDuty(double duty) +: VescPacket("SetDuty", 5, COMM_SET_DUTY) { /** @todo range check duty */ @@ -277,8 +434,8 @@ VescPacketSetDuty::VescPacketSetDuty(double duty) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetCurrent::VescPacketSetCurrent(double current) : - VescPacket("SetCurrent", 5, COMM_SET_CURRENT) +VescPacketSetCurrent::VescPacketSetCurrent(double current) +: VescPacket("SetCurrent", 5, COMM_SET_CURRENT) { int32_t v = static_cast(current * 1000.0); @@ -295,8 +452,8 @@ VescPacketSetCurrent::VescPacketSetCurrent(double current) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : - VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) +VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) +: VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) { int32_t v = static_cast(current_brake * 1000.0); @@ -313,8 +470,8 @@ VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetRPM::VescPacketSetRPM(double rpm) : - VescPacket("SetRPM", 5, COMM_SET_RPM) +VescPacketSetRPM::VescPacketSetRPM(double rpm) +: VescPacket("SetRPM", 5, COMM_SET_RPM) { int32_t v = static_cast(rpm); @@ -331,8 +488,8 @@ VescPacketSetRPM::VescPacketSetRPM(double rpm) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetPos::VescPacketSetPos(double pos) : - VescPacket("SetPos", 5, COMM_SET_POS) +VescPacketSetPos::VescPacketSetPos(double pos) +: VescPacket("SetPos", 5, COMM_SET_POS) { /** @todo range check pos */ @@ -351,8 +508,8 @@ VescPacketSetPos::VescPacketSetPos(double pos) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : - VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) +VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) +: VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) { /** @todo range check pos */ @@ -367,4 +524,166 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : *(frame_->end() - 2) = static_cast(crc & 0xFF); } + +VescPacketImu::VescPacketImu(std::shared_ptr raw) +: VescPacket("ImuData", raw) +{ + uint32_t ind = 1; + mask_ = static_cast( + (static_cast(*(payload_.first + ind )) << 8) + + static_cast(*(payload_.first + ind + 1 )) + ); + ind += 2; + + if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} +} + +int VescPacketImu::mask() const +{ + return mask_; +} + +double VescPacketImu::getFloat32Auto(uint32_t * idx) const +{ + int pos = *idx; + uint32_t res = static_cast( + (static_cast(*(payload_.first + (pos ) )) << 24) + + (static_cast(*(payload_.first + (pos + 1) )) << 16) + + (static_cast(*(payload_.first + (pos + 2) )) << 8) + + static_cast(*(payload_.first + (pos + 3) )) + ); + + *idx += 4; + + int e = (res >> 23) & 0xFF; + int fr = res & 0x7FFFFF; + bool negative = res & (1u << 31); + + float f = 0.0; + if (e != 0 || fr != 0) { + f = static_cast(fr) / (8388608.0 * 2.0) + 0.5; + e -= 126; + } + + if (negative) { + f = -f; + } + return ldexpf(f, e); +} + +double VescPacketImu::roll() const +{ + return roll_ * 180 / M_PI; // da rad a gradi per debug deg +} + +double VescPacketImu::pitch() const +{ + return pitch_ * 180 / M_PI; +} + + +double VescPacketImu::yaw() const +{ + return yaw_ * 180 / M_PI; +} + + +double VescPacketImu::acc_x()const +{ + return acc_x_; // g/s +} + +double VescPacketImu::acc_y() const +{ + return acc_y_; +} + +double VescPacketImu::acc_z() const +{ + return acc_z_; +} + +double VescPacketImu::gyr_x() const +{ + return gyr_x_; // deg/s +} + +double VescPacketImu::gyr_y() const +{ + return gyr_y_; +} + +double VescPacketImu::gyr_z() const +{ + return gyr_z_; +} + +double VescPacketImu::mag_x() const +{ + return mag_x_; +} + +double VescPacketImu::mag_y() const +{ + return mag_y_; +} + +double VescPacketImu::mag_z() const +{ + return mag_z_; +} + +double VescPacketImu::q_w() const +{ + return q0_; +} + +double VescPacketImu::q_x() const +{ + return q1_; +} + +double VescPacketImu::q_y() const +{ + return q2_; +} + +double VescPacketImu::q_z() const +{ + return q3_; +} + +REGISTER_PACKET_TYPE(COMM_GET_IMU_DATA, VescPacketImu) + +VescPacketRequestImu::VescPacketRequestImu() +: VescPacket("RequestImuData", 3, COMM_GET_IMU_DATA) +{ + *(payload_.first + 1) = static_cast(0xFF); + *(payload_.first + 2) = static_cast(0xFF); + + uint16_t crc = CRC::Calculate( + &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} +/*------------------------------------------------------------------------------------------------*/ } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 328401e..de53bb6 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -1,44 +1,46 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_packet_factory.h" +#include "vesc_driver/vesc_packet.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" #include #include #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { /** Construct map on first use */ -VescPacketFactory::FactoryMap* VescPacketFactory::getMap() +VescPacketFactory::FactoryMap * VescPacketFactory::getMap() { static FactoryMap m; return &m; @@ -46,49 +48,52 @@ VescPacketFactory::FactoryMap* VescPacketFactory::getMap() void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn) { - FactoryMap* p_map(getMap()); + FactoryMap * p_map(getMap()); assert(0 == p_map->count(payload_id)); (*p_map)[payload_id] = fn; } /** Helper function for when createPacket can not create a packet */ -VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, - const std::string& what, int num_bytes_needed = 0) +VescPacketPtr createFailed( + int * p_num_bytes_needed, std::string * p_what, + const std::string & what, int num_bytes_needed = 0) { - if (p_num_bytes_needed != NULL) *p_num_bytes_needed = num_bytes_needed; - if (p_what != NULL) *p_what = what; + if (p_num_bytes_needed != NULL) {*p_num_bytes_needed = num_bytes_needed;} + if (p_what != NULL) {*p_what = what;} return VescPacketPtr(); } -VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, - int* num_bytes_needed, std::string* what) +VescPacketPtr VescPacketFactory::createPacket( + const Buffer::const_iterator & begin, + const Buffer::const_iterator & end, + int * num_bytes_needed, std::string * what) { // initialize output variables - if (num_bytes_needed != NULL) *num_bytes_needed = 0; - if (what != NULL) what->clear(); + if (num_bytes_needed != NULL) {*num_bytes_needed = 0;} + if (what != NULL) {what->clear();} // need at least VESC_MIN_FRAME_SIZE bytes in buffer int buffer_size(std::distance(begin, end)); - if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); + if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) { + return createFailed( + num_bytes_needed, what, "Buffer does not contain a complete frame", + VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); + } // buffer must begin with a start-of-frame if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && - VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) + VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) + { return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character"); + } // get a view of the payload BufferRangeConst view_payload; - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) - { + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) { // payload size field is one byte view_payload.first = begin + 2; view_payload.second = view_payload.first + *(begin + 1); - } - else - { + } else { assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); // payload size field is two bytes view_payload.first = begin + 3; @@ -96,8 +101,9 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi } // check length - if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) + if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) { return createFailed(num_bytes_needed, what, "Invalid payload length"); + } // get iterators to crc field, end-of-frame field, and a view of the whole frame Buffer::const_iterator iter_crc(view_payload.second); @@ -106,41 +112,41 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi // do we have enough data in the buffer to complete the frame? int frame_size = std::distance(view_frame.first, view_frame.second); - if (buffer_size < frame_size) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - frame_size - buffer_size); + if (buffer_size < frame_size) { + return createFailed( + num_bytes_needed, what, "Buffer does not contain a complete frame", + frame_size - buffer_size); + } // is the end-of-frame character valid? - if (VescFrame::VESC_EOF_VAL != *iter_eof) + if (VescFrame::VESC_EOF_VAL != *iter_eof) { return createFailed(num_bytes_needed, what, "Invalid end-of-frame character"); + } // is the crc valid? uint16_t crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); if (crc != CRC::Calculate( - &(*view_payload.first), std::distance(view_payload.first, view_payload.second), VescFrame::CRC_TYPE)) + &(*view_payload.first), std::distance(view_payload.first, view_payload.second), + VescFrame::CRC_TYPE)) + { return createFailed(num_bytes_needed, what, "Invalid checksum"); + } // frame looks good, construct the raw frame std::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); // if the packet has a payload, construct the corresponding subclass - if (std::distance(view_payload.first, view_payload.second) > 0) - { + if (std::distance(view_payload.first, view_payload.second) > 0) { // get constructor function from payload id - FactoryMap* p_map(getMap()); + FactoryMap * p_map(getMap()); FactoryMap::const_iterator search(p_map->find(*view_payload.first)); - if (search != p_map->end()) - { + if (search != p_map->end()) { return search->second(raw_frame); - } - else - { + } else { // no subclass constructor for this packet return createFailed(num_bytes_needed, what, "Unkown payload type."); } - } - else - { + } else { // no payload return createFailed(num_bytes_needed, what, "Frame does not have a payload"); } diff --git a/vesc_driver/vesc_driver_nodelet.xml b/vesc_driver/vesc_driver_nodelet.xml deleted file mode 100644 index 5660e73..0000000 --- a/vesc_driver/vesc_driver_nodelet.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Vedder VESC motor controller interface nodelet. - - diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index d1365bd..14ee8a2 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -1,22 +1,23 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_msgs) -find_package(catkin REQUIRED COMPONENTS - std_msgs - message_generation -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -add_message_files( - DIRECTORY msg - FILES - VescState.msg - VescStateStamped.msg -) -generate_messages( +rosidl_generate_interfaces(vesc_msgs + "msg/VescState.msg" + "msg/VescStateStamped.msg" + "msg/VescImu.msg" + "msg/VescImuStamped.msg" DEPENDENCIES - std_msgs + builtin_interfaces + std_msgs + geometry_msgs ) -catkin_package( - CATKIN_DEPENDS std_msgs message_runtime -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/vesc_msgs/msg/VescImu.msg b/vesc_msgs/msg/VescImu.msg new file mode 100644 index 0000000..2765024 --- /dev/null +++ b/vesc_msgs/msg/VescImu.msg @@ -0,0 +1,8 @@ + + +geometry_msgs/Vector3 ypr +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 angular_velocity + +geometry_msgs/Vector3 compass +geometry_msgs/Quaternion orientation diff --git a/vesc_msgs/msg/VescImuStamped.msg b/vesc_msgs/msg/VescImuStamped.msg new file mode 100644 index 0000000..c8915b1 --- /dev/null +++ b/vesc_msgs/msg/VescImuStamped.msg @@ -0,0 +1,4 @@ + + +std_msgs/Header header +VescImu imu \ No newline at end of file diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index 276d62a..2edfd30 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -9,16 +9,33 @@ int32 FAULT_CODE_ABS_OVER_CURRENT=4 int32 FAULT_CODE_OVER_TEMP_FET=5 int32 FAULT_CODE_OVER_TEMP_MOTOR=6 +# follow the bledc firwmare: commands.c +float64 temp_fet # fet temperature +float64 temp_motor # motor temperature +float64 current_motor # motor current (ampere) avg_motor_current +float64 current_input # input current (ampere) avg_input_current +float64 avg_id +float64 avg_iq +float64 duty_cycle # duty cycle (0 to 1) duty_cycle_now +float64 speed # motor electrical speed (revolutions per minute) rpm + float64 voltage_input # input voltage (volt) -float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) -float64 current_motor # motor current (ampere) -float64 current_input # input current (ampere) -float64 speed # motor electrical speed (revolutions per minute) -float64 duty_cycle # duty cycle (0 to 1) -float64 charge_drawn # electric charge drawn from input (ampere-hour) -float64 charge_regen # electric charge regenerated to input (ampere-hour) +float64 charge_drawn # electric charge drawn from input (ampere-hours) +float64 charge_regen # electric charge regenerated to input (ampere-hour) amp_hours_charged float64 energy_drawn # energy drawn from input (watt-hour) -float64 energy_regen # energy regenerated to input (watt-hour) -float64 displacement # net tachometer (counts) -float64 distance_traveled # total tachnometer (counts) +float64 energy_regen # energy regenerated to input (watt_hours_charged) +int32 displacement # net tachometer (counts) tachometer +int32 distance_traveled # total tachnometer (counts) tachometer_abs int32 fault_code + +float64 pid_pos_now +int32 controller_id + + +float64 ntc_temp_mos1 +float64 ntc_temp_mos2 +float64 ntc_temp_mos3 +float64 avg_vd +float64 avg_vq + +#float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) diff --git a/vesc_msgs/msg/VescStateStamped.msg b/vesc_msgs/msg/VescStateStamped.msg index 086d5b8..bc1aec9 100644 --- a/vesc_msgs/msg/VescStateStamped.msg +++ b/vesc_msgs/msg/VescStateStamped.msg @@ -1,4 +1,4 @@ # Timestamped VESC open source motor controller state (telemetry) -Header header -VescState state \ No newline at end of file +std_msgs/Header header +VescState state diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index dc14307..60cc2ba 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -14,10 +14,20 @@ Michael T. Boulet Joshua Whitley - catkin - message_generation + ament_cmake_auto + rosidl_default_generators + builtin_interfaces std_msgs + geometry_msgs + rosidl_default_runtime - message_runtime + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake +