diff --git a/.gitignore b/.gitignore index 1377554..3a25c0a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ *.swp +.vscode/ diff --git a/README.md b/README.md index 959c0dd..10390ed 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ # Veddar VESC Interface +# ROS1 VESC Package ![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details + +The ```noetic``` branch contains the ROS2 branch back ported to ROS1 such that it is compatible with VESC (tested on the VESC EDU) diff --git a/vesc.yaml b/vesc.yaml new file mode 100644 index 0000000..4b8588c --- /dev/null +++ b/vesc.yaml @@ -0,0 +1,42 @@ + +# erpm (electrical rpm) = speed_to_erpm_gain * speed (meters / second) + speed_to_erpm_offset +speed_to_erpm_gain: 5420 #4614 +speed_to_erpm_offset: 0.0 + +# Set gains for converting acceleration to current and brake control values +accel_to_current_gain: 100 +accel_to_brake_gain: -80 + +tachometer_ticks_to_meters_gain: 0.00225 +# servo smoother - limits rotation speed and smooths anything above limit +max_servo_speed: 3.2 # radians/second +servo_smoother_rate: 75.0 # messages/sec + +# servo smoother - limits acceleration and smooths anything above limit +max_acceleration: 2.5 # meters/second^2 +throttle_smoother_rate: 75.0 # messages/sec + +# servo value (0 to 1) = steering_angle_to_servo_gain * steering angle (radians) + steering_angle_to_servo_offset +steering_angle_to_servo_gain: -1.2135 +steering_angle_to_servo_offset: 0.55 #0.5304 + +# publish odom to base link tf +vesc_to_odom/publish_tf: false + +# car wheelbase is about 25cm +wheelbase: .256 + +vesc_driver: + port: /dev/sensors/vesc + duty_cycle_min: 0.0 + duty_cycle_max: 0.0 + current_min: 0.0 + current_max: 100.0 + brake_min: -20000.0 + brake_max: 200000.0 + speed_min: -23250 + speed_max: 23250 + position_min: 0.0 + position_max: 0.0 + servo_min: 0.15 + servo_max: 0.85 diff --git a/vesc/package.xml b/vesc/package.xml index e1deeb4..e961311 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -14,13 +14,18 @@ Michael T. Boulet Joshua Whitley + http://www.ros.org/wiki/vesc + https://github.mit.edu/racecar/vesc + https://github.mit.edu/racecar/vesc/issues + catkin - vesc_driver - vesc_msgs - vesc_ackermann + vesc_driver + vesc_msgs + vesc_ackermann + diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h index f864833..7a8b21e 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h @@ -34,6 +34,7 @@ #include #include #include +#include #include namespace vesc_ackermann @@ -58,14 +59,14 @@ class VescToOdom // odometry state double x_, y_, yaw_; - std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value + 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_; + boost::shared_ptr tf_pub_; // ROS callbacks void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index 34d1301..f2ddcf6 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -17,18 +17,28 @@ catkin roslint + nodelet + pluginlib + roscpp + nav_msgs + std_msgs + geometry_msgs + tf + ackermann_msgs + vesc_msgs - ackermann_msgs - geometry_msgs - nav_msgs nodelet pluginlib roscpp + nav_msgs std_msgs + geometry_msgs tf + ackermann_msgs vesc_msgs + diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index c8c057f..01f1c0b 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -37,28 +37,22 @@ namespace vesc_ackermann { template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value); AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) { // get conversion parameters - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) + if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_)) return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) + if (!getRequiredParam(nh, "speed_to_erpm_offset", speed_to_erpm_offset_)) return; - if (!getRequiredParam(nh, "accel_to_current_gain", &accel_to_current_gain_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_gain", steering_to_servo_gain_)) return; - if (!getRequiredParam(nh, "accel_to_brake_gain", &accel_to_brake_gain_)) - 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_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) return; // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = nh.advertise("commands/motor/speed", 10); - current_pub_ = nh.advertise("commands/motor/current", 10); - brake_pub_ = nh.advertise("commands/motor/brake", 10); servo_pub_ = nh.advertise("commands/servo/position", 10); // subscribe to ackermann topic @@ -72,22 +66,6 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; - // calc vesc current/brake (acceleration) - bool is_positive_accel = true; - std_msgs::Float64::Ptr current_msg(new std_msgs::Float64); - std_msgs::Float64::Ptr brake_msg(new std_msgs::Float64); - current_msg->data = 0; - brake_msg->data = 0; - if (cmd->drive.acceleration < 0) - { - brake_msg->data = accel_to_brake_gain_ * cmd->drive.acceleration; - is_positive_accel = false; - } - else - { - current_msg->data = accel_to_current_gain_ * cmd->drive.acceleration; - } - // 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_; @@ -95,40 +73,15 @@ void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) // publish if (ros::ok()) { - // The below code attempts to stick to the previous mode until a new message forces a mode switch. - if (erpm_msg->data != 0 || previous_mode_speed_) - { - erpm_pub_.publish(erpm_msg); - } - else - { - if (is_positive_accel) - { - current_pub_.publish(current_msg); - } - else - { - brake_pub_.publish(brake_msg); - } - } + erpm_pub_.publish(erpm_msg); servo_pub_.publish(servo_msg); } - - // The lines below are to determine which mode we are in so we can hold until new messages force a switch. - if (erpm_msg->data != 0) - { - previous_mode_speed_ = true; - } - else if (current_msg->data != 0 || brake_msg->data != 0) - { - previous_mode_speed_ = false; - } } template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) { - if (nh.getParam(name, *value)) + if (nh.getParam(name, value)) return true; ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp index 5b72582..2900faa 100644 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp @@ -23,26 +23,28 @@ // 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 - #include "vesc_ackermann/ackermann_to_vesc.h" namespace vesc_ackermann { -class AckermannToVescNodelet: public nodelet::Nodelet +class AckermannToVescNodelet : public nodelet::Nodelet { public: - AckermannToVescNodelet() {} + AckermannToVescNodelet() + { + } private: virtual void onInit(void); - std::shared_ptr ackermann_to_vesc_; + boost::shared_ptr ackermann_to_vesc_; + }; // class AckermannToVescNodelet void AckermannToVescNodelet::onInit() diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index cca7d11..ad5c63e 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -28,7 +28,6 @@ #include "vesc_ackermann/vesc_to_odom.h" #include -#include #include #include @@ -37,27 +36,26 @@ namespace vesc_ackermann { template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +inline bool getRequiredParam(const ros::NodeHandle& nh, 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) +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) { // 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_)) + if (!getRequiredParam(nh, "speed_to_erpm_gain", speed_to_erpm_gain_)) return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) + 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_)) + 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_)) + if (!getRequiredParam(nh, "steering_angle_to_servo_offset", steering_to_servo_offset_)) return; - if (!getRequiredParam(nh, "wheelbase", &wheelbase_)) + if (!getRequiredParam(nh, "wheelbase", wheelbase_)) return; } private_nh.param("publish_tf", publish_tf_, publish_tf_); @@ -75,8 +73,7 @@ VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : 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); + servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, &VescToOdom::servoCmdCallback, this); } } @@ -95,8 +92,7 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& double current_steering_angle(0.0), current_angular_velocity(0.0); if (use_servo_cmd_) { - current_steering_angle = - (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; + 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_; } @@ -136,8 +132,8 @@ void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& // 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[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") @@ -176,9 +172,9 @@ void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) } template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) +inline bool getRequiredParam(const ros::NodeHandle& nh, std::string name, T& value) { - if (nh.getParam(name, *value)) + if (nh.getParam(name, value)) return true; ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp index 5f21c85..9d0be7c 100644 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp @@ -23,26 +23,28 @@ // 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 - #include "vesc_ackermann/vesc_to_odom.h" namespace vesc_ackermann { -class VescToOdomNodelet: public nodelet::Nodelet +class VescToOdomNodelet : public nodelet::Nodelet { public: - VescToOdomNodelet() {} + VescToOdomNodelet() + { + } private: virtual void onInit(void); - std::shared_ptr vesc_to_odom_; + boost::shared_ptr vesc_to_odom_; + }; // class VescToOdomNodelet void VescToOdomNodelet::onInit() diff --git a/vesc_driver/CHANGELOG.rst b/vesc_driver/CHANGELOG.rst index 1b48d3e..c505185 100644 --- a/vesc_driver/CHANGELOG.rst +++ b/vesc_driver/CHANGELOG.rst @@ -9,7 +9,7 @@ Changelog for package vesc_driver * Exclude crc.h from roslint. * Replacing boost::crc with CRCPP. * Replacing boost::begin, boost::end, and boost::distance with Standard Library equivalents. -* Replacing boost::bind with Standard Library equivalent. +* Replacing boost::bind with Standards Library equivalent. * Replaing boost::noncopyable with C++ equivalent. * Replacing boost::function with Standard Library version. * Replacing Boost smart pointers with Standard Library equivalents. diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 50221d3..2a8e97d 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -20,15 +20,17 @@ find_package(catkin REQUIRED COMPONENTS serial std_msgs vesc_msgs + serial ) catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - nodelet - pluginlib - std_msgs - vesc_msgs serial + nodelet + pluginlib + std_msgs + vesc_msgs serial + CATKIN_DEPENDS nodelet pluginlib roscpp std_msgs vesc_msgs serial ) ########### @@ -97,4 +99,4 @@ install(DIRECTORY launch/ if(CATKIN_ENABLE_TESTING) roslint_add_test() -endif() +endif() \ No newline at end of file diff --git a/vesc_driver/include/vesc_driver/crc.h b/vesc_driver/include/vesc_driver/crc.h deleted file mode 100644 index 68e322d..0000000 --- a/vesc_driver/include/vesc_driver/crc.h +++ /dev/null @@ -1,1705 +0,0 @@ -/** - @file CRC.h - @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 -*/ - -/* - CRC++ can be configured by setting various #defines before #including this header file: - - #define crcpp_uint8 - Specifies the type used to store CRCs that have a width of 8 bits or less. - This type is not used in CRC calculations. Defaults to ::std::uint8_t. - #define crcpp_uint16 - Specifies the type used to store CRCs that have a width between 9 and 16 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint16_t. - #define crcpp_uint32 - Specifies the type used to store CRCs that have a width between 17 and 32 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint32_t. - #define crcpp_uint64 - Specifies the type used to store CRCs that have a width between 33 and 64 bits (inclusive). - This type is not used in CRC calculations. Defaults to ::std::uint64_t. - #define crcpp_size - This type is used for loop iteration and function signatures only. Defaults to ::std::size_t. - #define CRCPP_USE_NAMESPACE - Define to place all CRC++ code within the ::CRCPP namespace. - #define CRCPP_BRANCHLESS - Define to enable a branchless CRC implementation. The branchless implementation uses a single integer - 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. -*/ - -#ifndef VESC_DRIVER_CRC_H_ -#define VESC_DRIVER_CRC_H_ - -#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 -#else -#include // Includes size_t -#include // Includes uint8_t, uint16_t, uint32_t, uint64_t -#endif -#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. -# define crcpp_uint8 ::std::uint8_t -# else - /// @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. -# define crcpp_uint16 ::std::uint16_t -# else - /// @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. -# define crcpp_uint32 ::std::uint32_t -# else - /// @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. -# define crcpp_uint64 ::std::uint64_t -# else - /// @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. -# define crcpp_size ::std::size_t -# else - /// @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. -# define crcpp_constexpr constexpr -#else - /// @brief Compile-time expression definition. -# define crcpp_constexpr const -#endif - -#ifdef CRCPP_USE_NAMESPACE -namespace CRCPP -{ -#endif - -/** - @brief Static class for computing CRCs. - @note This class supports computation of full and multi-part CRCs, using a bit-by-bit algorithm or a - byte-by-byte lookup table. The CRCs are calculated using as many optimizations as is reasonable. - If compiling with C++11, the constexpr keyword is used liberally so that many calculations are - performed at compile-time instead of at runtime. -*/ -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); - -#ifdef CRCPP_USE_CPP11 - 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); - - 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(); -#endif - static const Parameters< crcpp_uint8, 8> & 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(); -#endif - 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(); -#endif - 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(); -#endif - 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(); -#endif - static const Parameters & CRC_32(); - static const Parameters & CRC_32_BZIP2(); -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_C(); -#endif - 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(); -#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; -#endif - -private: -#ifndef CRCPP_USE_CPP11 - CRC(); - CRC(const CRC & other); - CRC & operator=(const CRC & other); -#endif - - template - static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); - - template - static CRCType Finalize(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 Table & lookupTable, CRCType remainder); -}; - -/** - @brief Returns a CRC lookup table construct using these CRC parameters. - @note This function primarily exists to allow use of the auto keyword instead of instantiating - a table directly, since template parameters are not inferred in constructors. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC lookup table -*/ -template -inline CRC::Table CRC::Parameters::MakeTable() const -{ - // This should take advantage of RVO and optimize out the copy. - return CRC::Table(*this); -} - -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC parameters - @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) -{ - InitTable(); -} - -#ifdef CRCPP_USE_CPP11 -/** - @brief Constructs a CRC table from a set of CRC parameters - @param[in] params CRC 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)) -{ - InitTable(); -} -#endif - -/** - @brief Gets the CRC parameters used to construct the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC parameters -*/ -template -inline const CRC::Parameters & CRC::Table::GetParameters() const -{ - return parameters; -} - -/** - @brief Gets the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table -*/ -template -inline const CRCType * CRC::Table::GetTable() const -{ - return table; -} - -/** - @brief Gets an entry in the CRC table - @param[in] index Index into the CRC table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC table entry -*/ -template -inline CRCType CRC::Table::operator[](unsigned char index) const -{ - return table[index]; -} - -/** - @brief Initializes a CRC table. - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC -*/ -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); - - CRCType crc; - unsigned char byte = 0; - - // Loop over each dividend (each possible number storable in an unsigned char) - do - { - crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(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; - - 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); - } - - table[byte] = crc; - } - while (++byte); -} - -/** - @brief Computes a CRC. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @tparam CRCType Integer type for storing the CRC result - @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 remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); - - // 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); -} -/** - @brief Appends additional data to a previous CRC calculation. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @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) -{ - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, parameters, remainder); - - // 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); -} - -/** - @brief Computes a CRC via a lookup table. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] lookupTable CRC lookup table - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); - - // 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); -} - -/** - @brief Appends additional data to a previous CRC calculation using a lookup table. - @note This function can be used to compute multi-part CRCs. - @param[in] data Data over which CRC will be computed - @param[in] size Size of the data - @param[in] lookupTable CRC lookup table - @param[in] crc CRC from a previous calculation - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return CRC -*/ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) -{ - const Parameters & parameters = lookupTable.GetParameters(); - - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); - - remainder = CalculateRemainder(data, size, lookupTable, remainder); - - // 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); -} - -/** - @brief Reflects (i.e. reverses the bits within) an integer value. - @param[in] value Value to reflect - @param[in] numBits Number of bits in the integer which will be reflected - @tparam IntegerType Integer type of the value being reflected - @return Reflected value -*/ -template -inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) -{ - IntegerType reversedValue(0); - - for (crcpp_uint16 i = 0; i < numBits; ++i) - { - reversedValue = static_cast((reversedValue << 1) | (value & 1)); - value = static_cast(value >> 1); - } - - return reversedValue; -} - -/** - @brief Computes the final reflection and XOR of a CRC remainder. - @param[in] remainder CRC remainder to reflect and XOR - @param[in] finalXOR Final value to XOR with the remainder - @param[in] reflectOutput true to reflect each byte of the remainder before the XOR - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Final CRC -*/ -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)); - - if (reflectOutput) - { - remainder = Reflect(remainder, CRCWidth); - } - - 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: - - CRCType x = ...; - CRCType y = Finalize(x, finalXOR, reflectOutput); - CRCType z = UndoFinalize(y, finalXOR, reflectOutput); - assert(x == z); - - @param[in] crc Reflected and XORed CRC - @param[in] finalXOR Final value XORed with the remainder - @param[in] reflectOutput true if the remainder is to be reflected - @tparam CRCType Integer type for storing the CRC result - @tparam CRCWidth Number of bits in the CRC - @return Un-finalized CRC remainder -*/ -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)); - - crc = (crc & BIT_MASK) ^ finalXOR; - - if (reflectOutput) - { - crc = Reflect(crc, CRCWidth); - } - - return crc; -} - -/** - @brief Computes a CRC remainder. - @param[in] data Data over which the remainder will be computed - @param[in] size Size of the data - @param[in] parameters CRC parameters - @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 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."); -#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) }; -#endif - - 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++); - - // 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)); -#else - 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)); -#ifndef CRCPP_BRANCHLESS - 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); - - 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) - { -#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)); -#else - 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); -#ifndef CRCPP_BRANCHLESS - 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); - - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast(remainder << SHIFT); - - 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) - { -#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)); -#else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); -#endif - } - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -/** - @brief Computes a CRC remainder using lookup table. - @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. - @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) -{ - const unsigned char * current = reinterpret_cast(data); - - 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. -# pragma warning (push) -# pragma warning (disable : 4333) -#endif - 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); - - 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++)]; - } - - remainder = static_cast(remainder >> SHIFT); - } - - return remainder; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-4 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-4 ITU has the following parameters and check value: - - polynomial = 0x3 - - initial value = 0x0 - - final XOR = 0x0 - - reflect input = true - - reflect output = true - - check value = 0x7 - @return CRC-4 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_4_ITU() -{ - static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 EPC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 EPC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x09 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x00 - @return CRC-5 EPC parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_EPC() -{ - static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 ITU has the following parameters and check value: - - polynomial = 0x15 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x07 - @return CRC-5 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_ITU() -{ - static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-5 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-5 USB has the following parameters and check value: - - polynomial = 0x05 - - initial value = 0x1F - - final XOR = 0x1F - - reflect input = true - - reflect output = true - - check value = 0x19 - @return CRC-5 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_5_USB() -{ - static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x27 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x0D - @return CRC-6 CDMA2000-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() -{ - static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 CDMA2000-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 CDMA2000-A has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x3F - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x3B - @return CRC-6 CDMA2000-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() -{ - static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-6 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-6 ITU has the following parameters and check value: - - polynomial = 0x03 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x06 - @return CRC-6 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_6_ITU() -{ - static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-7 JEDEC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-7 JEDEC has the following parameters and check value: - - polynomial = 0x09 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0x75 - @return CRC-7 JEDEC parameters -*/ -inline const CRC::Parameters & CRC::CRC_7() -{ - static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-8 SMBus. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 SMBus has the following parameters and check value: - - polynomial = 0x07 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = false - - reflect output = false - - check value = 0xF4 - @return CRC-8 SMBus parameters -*/ -inline const CRC::Parameters & CRC::CRC_8() -{ - static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-8 EBU (aka CRC-8 AES). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 EBU has the following parameters and check value: - - polynomial = 0x1D - - initial value = 0xFF - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x97 - @return CRC-8 EBU parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_EBU() -{ - static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 MAXIM (aka CRC-8 DOW-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 MAXIM has the following parameters and check value: - - polynomial = 0x31 - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0xA1 - @return CRC-8 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_MAXIM() -{ - static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-8 WCDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-8 WCDMA has the following parameters and check value: - - polynomial = 0x9B - - initial value = 0x00 - - final XOR = 0x00 - - reflect input = true - - reflect output = true - - check value = 0x25 - @return CRC-8 WCDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_8_WCDMA() -{ - static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 ITU. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 ITU has the following parameters and check value: - - polynomial = 0x233 - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x199 - @return CRC-10 ITU parameters -*/ -inline const CRC::Parameters & CRC::CRC_10() -{ - static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-10 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-10 CDMA2000 has the following parameters and check value: - - polynomial = 0x3D9 - - initial value = 0x3FF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x233 - @return CRC-10 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_10_CDMA2000() -{ - static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-11 FlexRay. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-11 FlexRay has the following parameters and check value: - - polynomial = 0x385 - - initial value = 0x01A - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0x5A3 - @return CRC-11 FlexRay parameters -*/ -inline const CRC::Parameters & CRC::CRC_11() -{ - static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 CDMA2000 has the following parameters and check value: - - polynomial = 0xF13 - - initial value = 0xFFF - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xD4D - @return CRC-12 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_CDMA2000() -{ - static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 DECT (aka CRC-12 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 DECT has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = false - - check value = 0xF5B - @return CRC-12 DECT parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_DECT() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-12 UMTS (aka CRC-12 3GPP). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-12 UMTS has the following parameters and check value: - - polynomial = 0x80F - - initial value = 0x000 - - final XOR = 0x000 - - reflect input = false - - reflect output = true - - check value = 0xDAF - @return CRC-12 UMTS parameters -*/ -inline const CRC::Parameters & CRC::CRC_12_UMTS() -{ - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-13 BBC. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-13 BBC has the following parameters and check value: - - polynomial = 0x1CF5 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x04FA - @return CRC-13 BBC parameters -*/ -inline const CRC::Parameters & CRC::CRC_13_BBC() -{ - static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 CAN has the following parameters and check value: - - polynomial = 0x4599 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x059E - @return CRC-15 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_15() -{ - static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-15 MPT1327. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-15 MPT1327 has the following parameters and check value: - - polynomial = 0x6815 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x2566 - @return CRC-15 MPT1327 parameters -*/ -inline const CRC::Parameters & CRC::CRC_15_MPT1327() -{ - static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 ARC has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0xBB3D - @return CRC-16 ARC parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_ARC() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 BUYPASS (aka CRC-16 VERIFONE, CRC-16 UMTS). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 BUYPASS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xFEE8 - @return CRC-16 BUYPASS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_BUYPASS() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CCITT FALSE. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CCITT FALSE has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x29B1 - @return CRC-16 CCITT FALSE parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 CDMA2000. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CDMA2000 has the following parameters and check value: - - polynomial = 0xC867 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x4C06 - @return CRC-16 CDMA2000 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CDMA2000() -{ - static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 CMS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 CMS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xAEE7 - @return CRC-16 CMS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_CMS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-R (aka CRC-16 R-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-R has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0001 - - reflect input = false - - reflect output = false - - check value = 0x007E - @return CRC-16 DECT-R parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTR() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DECT-X (aka CRC-16 X-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DECT-X has the following parameters and check value: - - polynomial = 0x0589 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x007F - @return CRC-16 DECT-X parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DECTX() -{ - static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 DNP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 DNP has the following parameters and check value: - - polynomial = 0x3D65 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xEA82 - @return CRC-16 DNP parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_DNP() -{ - static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; - return parameters; -} -#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). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 GENIBUS has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = false - - reflect output = false - - check value = 0xD64E - @return CRC-16 GENIBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_GENIBUS() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 KERMIT (aka CRC-16 CCITT, CRC-16 CCITT-TRUE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 KERMIT has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x2189 - @return CRC-16 KERMIT parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_KERMIT() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-16 MAXIM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MAXIM has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0x0000 - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x44C2 - @return CRC-16 MAXIM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MAXIM() -{ - static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 MODBUS. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 MODBUS has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0x0000 - - reflect input = true - - reflect output = true - - check value = 0x4B37 - @return CRC-16 MODBUS parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_MODBUS() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 T10-DIF. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 T10-DIF has the following parameters and check value: - - polynomial = 0x8BB7 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0xD0DB - @return CRC-16 T10-DIF parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_T10DIF() -{ - static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 USB. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 USB has the following parameters and check value: - - polynomial = 0x8005 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0xB4C8 - @return CRC-16 USB parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_USB() -{ - static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -#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). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 X-25 has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0xFFFF - - final XOR = 0xFFFF - - reflect input = true - - reflect output = true - - check value = 0x906E - @return CRC-16 X-25 parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_X25() -{ - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-16 XMODEM (aka CRC-16 ZMODEM, CRC-16 ACORN, CRC-16 LTE). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-16 XMODEM has the following parameters and check value: - - polynomial = 0x1021 - - initial value = 0x0000 - - final XOR = 0x0000 - - reflect input = false - - reflect output = false - - check value = 0x31C3 - @return CRC-16 XMODEM parameters -*/ -inline const CRC::Parameters & CRC::CRC_16_XMODEM() -{ - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-17 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-17 CAN has the following parameters and check value: - - polynomial = 0x1685B - - initial value = 0x00000 - - final XOR = 0x00000 - - reflect input = false - - reflect output = false - - check value = 0x04F03 - @return CRC-17 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_17_CAN() -{ - static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-21 CAN. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-21 CAN has the following parameters and check value: - - polynomial = 0x102899 - - initial value = 0x000000 - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x0ED841 - @return CRC-21 CAN parameters -*/ -inline const CRC::Parameters & CRC::CRC_21_CAN() -{ - static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 OPENPGP. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 OPENPGP has the following parameters and check value: - - polynomial = 0x864CFB - - initial value = 0xB704CE - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x21CF02 - @return CRC-24 OPENPGP parameters -*/ -inline const CRC::Parameters & CRC::CRC_24() -{ - static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-A. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-A has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xFEDCBA - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x7979BD - @return CRC-24 FlexRay-A parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() -{ - static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-24 FlexRay-B. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-24 FlexRay-B has the following parameters and check value: - - polynomial = 0x5D6DCB - - initial value = 0xABCDEF - - final XOR = 0x000000 - - reflect input = false - - reflect output = false - - check value = 0x1F23B8 - @return CRC-24 FlexRay-B parameters -*/ -inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() -{ - static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-30 CDMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-30 CDMA has the following parameters and check value: - - polynomial = 0x2030B9C7 - - initial value = 0x3FFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3B3CB540 - @return CRC-30 CDMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_30() -{ - static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -/** - @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xCBF43926 - @return CRC-32 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 BZIP2 (aka CRC-32 AAL5, CRC-32 DECT-B, CRC-32 B-CRC). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 BZIP2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xFC891918 - @return CRC-32 BZIP2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_BZIP2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 C (aka CRC-32 ISCSI, CRC-32 Castagnoli, CRC-32 Interlaken). - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 C has the following parameters and check value: - - polynomial = 0x1EDC6F41 - - initial value = 0xFFFFFFFF - - final XOR = 0xFFFFFFFF - - reflect input = true - - reflect output = true - - check value = 0xE3069283 - @return CRC-32 C parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_C() -{ - static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; -} -#endif - -/** - @brief Returns a set of parameters for CRC-32 MPEG-2. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 MPEG-2 has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0xFFFFFFFF - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x0376E6E7 - @return CRC-32 MPEG-2 parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_MPEG2() -{ - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-32 POSIX. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 POSIX has the following parameters and check value: - - polynomial = 0x04C11DB7 - - initial value = 0x00000000 - - final XOR = 0xFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0x765E7680 - @return CRC-32 POSIX parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_POSIX() -{ - static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; - return parameters; -} - -#ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS -/** - @brief Returns a set of parameters for CRC-32 Q. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-32 Q has the following parameters and check value: - - polynomial = 0x814141AB - - initial value = 0x00000000 - - final XOR = 0x00000000 - - reflect input = false - - reflect output = false - - check value = 0x3010BF7F - @return CRC-32 Q parameters -*/ -inline const CRC::Parameters & CRC::CRC_32_Q() -{ - static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-40 GSM. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-40 GSM has the following parameters and check value: - - polynomial = 0x0004820009 - - initial value = 0x0000000000 - - final XOR = 0xFFFFFFFFFF - - reflect input = false - - reflect output = false - - check value = 0xD4164FC646 - @return CRC-40 GSM parameters -*/ -inline const CRC::Parameters & CRC::CRC_40_GSM() -{ - static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; - return parameters; -} - -/** - @brief Returns a set of parameters for CRC-64 ECMA. - @note The parameters are static and are delayed-constructed to reduce memory footprint. - @note CRC-64 ECMA has the following parameters and check value: - - polynomial = 0x42F0E1EBA9EA3693 - - initial value = 0x0000000000000000 - - final XOR = 0x0000000000000000 - - reflect input = false - - reflect output = false - - check value = 0x6C40DF5F0B497347 - @return CRC-64 ECMA parameters -*/ -inline const CRC::Parameters & CRC::CRC_64() -{ - static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; - return parameters; -} -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - -#ifdef CRCPP_USE_NAMESPACE -} -#endif - -#endif // CRCPP_CRC_H_ diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.h index 1497a20..5b498b3 100644 --- a/vesc_driver/include/vesc_driver/datatypes.h +++ b/vesc_driver/include/vesc_driver/datatypes.h @@ -40,18 +40,21 @@ 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 +#ifndef DATATYPES_H +#define DATATYPES_H -#include +// #include +// #include +// #include +// #include +#include typedef struct { bool isVesc; -} -VSerialInfo_t; +} VSerialInfo_t; typedef enum { @@ -61,8 +64,7 @@ typedef enum CFG_T_QSTRING, CFG_T_ENUM, CFG_T_BOOL -} -CFG_T; +} CFG_T; typedef enum { @@ -76,8 +78,7 @@ typedef enum VESC_TX_DOUBLE16, VESC_TX_DOUBLE32, VESC_TX_DOUBLE32_AUTO -} -VESC_TX_T; +} VESC_TX_T; typedef enum { @@ -88,8 +89,7 @@ typedef enum FAULT_CODE_ABS_OVER_CURRENT, FAULT_CODE_OVER_TEMP_FET, FAULT_CODE_OVER_TEMP_MOTOR -} -mc_fault_code; +} mc_fault_code; typedef enum { @@ -100,8 +100,7 @@ typedef enum DISP_POS_MODE_PID_POS, DISP_POS_MODE_PID_POS_ERROR, DISP_POS_MODE_ENCODER_OBSERVER_ERROR -} -disp_pos_mode; +} disp_pos_mode; struct MC_VALUES { @@ -135,9 +134,9 @@ typedef enum DEBUG_SAMPLING_TRIGGER_START_NOSEND, DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, DEBUG_SAMPLING_SEND_LAST_SAMPLES -} -debug_sampling_mode; +} debug_sampling_mode; +// Communication commands typedef enum { COMM_FW_VERSION = 0, @@ -177,9 +176,61 @@ typedef enum COMM_FORWARD_CAN, COMM_SET_CHUCK_DATA, COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING -} -COMM_PACKET_ID; + 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; typedef struct { @@ -190,8 +241,7 @@ typedef struct int acc_z; bool bt_c; bool bt_z; -} -chuck_data; +} chuck_data; struct bldc_detect { @@ -206,7 +256,6 @@ typedef enum NRF_PAIR_STARTED = 0, NRF_PAIR_OK, NRF_PAIR_FAIL -} -NRF_PAIR_RES; +} NRF_PAIR_RES; -#endif // VESC_DRIVER_DATATYPES_H +#endif // DATATYPES_H diff --git a/vesc_driver/include/vesc_driver/v8stdint.h b/vesc_driver/include/vesc_driver/v8stdint.h new file mode 100644 index 0000000..082e1ec --- /dev/null +++ b/vesc_driver/include/vesc_driver/v8stdint.h @@ -0,0 +1,57 @@ +// This header is from the v8 google project: +// http://code.google.com/p/v8/source/browse/trunk/include/v8stdint.h + +// Copyright 2012 the V8 project authors. 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 Google Inc. 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 +// OWNER 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. + +// Load definitions of standard types. + +#ifndef V8STDINT_H_ +#define V8STDINT_H_ + +#include +#include + +#if defined(_WIN32) && !defined(__MINGW32__) + +typedef signed char int8_t; +typedef unsigned char uint8_t; +typedef short int16_t; // NOLINT +typedef unsigned short uint16_t; // NOLINT +typedef int int32_t; +typedef unsigned int uint32_t; +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; +// intptr_t and friends are defined in crtdefs.h through stdio.h. + +#else + +#include + +#endif + +#endif // V8STDINT_H_ diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.h index bdc5ffd..9eca3a2 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.h +++ b/vesc_driver/include/vesc_driver/vesc_driver.h @@ -28,7 +28,6 @@ #ifndef VESC_DRIVER_VESC_DRIVER_H_ #define VESC_DRIVER_VESC_DRIVER_H_ -#include #include #include @@ -44,13 +43,12 @@ namespace vesc_driver class VescDriver { public: - VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh); + VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); private: // interface to the VESC VescInterface vesc_; - void vescPacketCallback(const std::shared_ptr& packet); + void vescPacketCallback(const boost::shared_ptr& packet); void vescErrorCallback(const std::string& error); // limits on VESC commands @@ -74,6 +72,9 @@ class VescDriver // ROS services ros::Publisher state_pub_; ros::Publisher servo_sensor_pub_; + ros::Publisher imu_pub_; + // ros::Publisher imu_std_pub_; + ros::Subscriber duty_cycle_sub_; ros::Subscriber current_sub_; ros::Subscriber brake_sub_; @@ -87,13 +88,12 @@ class VescDriver { MODE_INITIALIZING, MODE_OPERATING - } - driver_mode_t; + } 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 + 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); diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.h index 558ec2b..66be4c0 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.h +++ b/vesc_driver/include/vesc_driver/vesc_interface.h @@ -28,12 +28,15 @@ #ifndef VESC_DRIVER_VESC_INTERFACE_H_ #define VESC_DRIVER_VESC_INTERFACE_H_ -#include -#include -#include +#include #include +#include #include -#include + +#include +#include +#include +#include #include "vesc_driver/vesc_packet.h" @@ -43,11 +46,11 @@ namespace vesc_driver /** * Class providing an interface to the Vedder VESC motor controller via a serial port interface. */ -class VescInterface +class VescInterface : private boost::noncopyable { public: - typedef std::function PacketHandlerFunction; - typedef std::function ErrorHandlerFunction; + typedef boost::function PacketHandlerFunction; + typedef boost::function ErrorHandlerFunction; /** * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not @@ -64,12 +67,6 @@ class VescInterface const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); - /** - * Delete copy constructor and equals operator. - */ - VescInterface(const VescInterface &) = delete; - VescInterface & operator=(const VescInterface &) = delete; - /** * VescInterface destructor. */ @@ -119,10 +116,12 @@ class VescInterface void setPosition(double position); void setServo(double servo); + void requestImuData(); + private: // Pimpl - hide serial port members from class users class Impl; - std::unique_ptr impl_; + boost::scoped_ptr impl_; }; // todo: review @@ -131,15 +130,20 @@ class SerialException : public std::exception // Disable copy constructors SerialException& operator=(const SerialException&); std::string e_what_; + public: - explicit SerialException(const char *description) + SerialException(const char* description) { std::stringstream ss; ss << "SerialException " << description << " failed."; e_what_ = ss.str(); } - SerialException(const SerialException& other) : e_what_(other.e_what_) {} - virtual ~SerialException() throw() {} + SerialException(const SerialException& other) : e_what_(other.e_what_) + { + } + virtual ~SerialException() throw() + { + } virtual const char* what() const throw() { return e_what_.c_str(); diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.h index 307e2f5..e1b909b 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.h +++ b/vesc_driver/include/vesc_driver/vesc_packet.h @@ -28,14 +28,16 @@ #ifndef VESC_DRIVER_VESC_PACKET_H_ #define VESC_DRIVER_VESC_PACKET_H_ -#include -#include #include #include #include -#define CRCPP_USE_CPP11 -#include "vesc_driver/crc.h" +#include +#include + +// #include "vesc_driver/crc.hpp" + +#include "vesc_driver/v8stdint.h" namespace vesc_driver { @@ -48,7 +50,9 @@ typedef std::pair BufferRangeCon class VescFrame { public: - virtual ~VescFrame() {} + virtual ~VescFrame() + { + } // getters virtual const Buffer& frame() const @@ -57,21 +61,21 @@ 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_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 - 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 + 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 }; + typedef boost::crc_optimal<16, 0x1021, 0, 0, false, false> CRC; protected: /** Construct frame with specified payload size. */ - explicit VescFrame(int payload_size); + VescFrame(int payload_size); - std::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy + boost::shared_ptr frame_; ///< Stores frame data, shared_ptr for shallow copy BufferRange payload_; ///< View into frame's payload section private: @@ -88,7 +92,9 @@ class VescFrame class VescPacket : public VescFrame { public: - virtual ~VescPacket() {} + virtual ~VescPacket() + { + } virtual const std::string& name() const { @@ -97,21 +103,21 @@ class VescPacket : public VescFrame 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, boost::shared_ptr raw); private: std::string name_; }; -typedef std::shared_ptr VescPacketPtr; -typedef std::shared_ptr VescPacketConstPtr; +typedef boost::shared_ptr VescPacketPtr; +typedef boost::shared_ptr VescPacketConstPtr; /*------------------------------------------------------------------------------------------------*/ class VescPacketFWVersion : public VescPacket { public: - explicit VescPacketFWVersion(std::shared_ptr raw); + VescPacketFWVersion(boost::shared_ptr raw); int fwMajor() const; int fwMinor() const; @@ -128,7 +134,7 @@ class VescPacketRequestFWVersion : public VescPacket class VescPacketValues : public VescPacket { public: - explicit VescPacketValues(std::shared_ptr raw); + VescPacketValues(boost::shared_ptr raw); double v_in() const; double temp_mos1() const; @@ -162,7 +168,7 @@ class VescPacketRequestValues : public VescPacket class VescPacketSetDuty : public VescPacket { public: - explicit VescPacketSetDuty(double duty); + VescPacketSetDuty(double duty); // double duty() const; }; @@ -172,7 +178,7 @@ class VescPacketSetDuty : public VescPacket class VescPacketSetCurrent : public VescPacket { public: - explicit VescPacketSetCurrent(double current); + VescPacketSetCurrent(double current); // double current() const; }; @@ -182,7 +188,7 @@ class VescPacketSetCurrent : public VescPacket class VescPacketSetCurrentBrake : public VescPacket { public: - explicit VescPacketSetCurrentBrake(double current_brake); + VescPacketSetCurrentBrake(double current_brake); // double current_brake() const; }; @@ -192,7 +198,7 @@ class VescPacketSetCurrentBrake : public VescPacket class VescPacketSetRPM : public VescPacket { public: - explicit VescPacketSetRPM(double rpm); + VescPacketSetRPM(double rpm); // double rpm() const; }; @@ -202,7 +208,7 @@ class VescPacketSetRPM : public VescPacket class VescPacketSetPos : public VescPacket { public: - explicit VescPacketSetPos(double pos); + VescPacketSetPos(double pos); // double pos() const; }; @@ -212,11 +218,74 @@ class VescPacketSetPos : public VescPacket class VescPacketSetServoPos : public VescPacket { public: - explicit VescPacketSetServoPos(double servo_pos); + VescPacketSetServoPos(double servo_pos); // double servo_pos() const; }; +/*------------------------------------------------------------------------------------------------*/ +class VescPacketRequestImu : public VescPacket +{ +public: + VescPacketRequestImu(); +}; + +class VescPacketImu : public VescPacket +{ +public: + explicit VescPacketImu(boost::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_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.h index 643d6f9..8befc18 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.h +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.h @@ -28,13 +28,15 @@ #ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ #define VESC_DRIVER_VESC_PACKET_FACTORY_H_ -#include -#include +#include #include -#include #include -#include +#include +#include +#include + +#include "vesc_driver/v8stdint.h" #include "vesc_driver/vesc_packet.h" namespace vesc_driver @@ -43,7 +45,7 @@ namespace vesc_driver /** * Class for creating VESC packets from raw data. */ -class VescPacketFactory +class VescPacketFactory : private boost::noncopyable { public: /** Return the global factory object */ @@ -69,45 +71,34 @@ 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, + static VescPacketPtr createPacket(const Buffer::const_iterator& begin, const Buffer::const_iterator& end, int* num_bytes_needed, std::string* what); - typedef std::function)> CreateFn; + typedef boost::function)> CreateFn; /** Register a packet type with the factory. */ static void registerPacketType(int payload_id, CreateFn fn); - /** - * Delete copy constructor and equals operator. - */ - VescPacketFactory(const VescPacketFactory &) = delete; - VescPacketFactory & operator=(const VescPacketFactory &) = delete; - private: - VescPacketFactory(); - typedef std::map FactoryMap; + typedef std::map FactoryMap; static FactoryMap* getMap(); }; -template -class PacketFactoryTemplate -{ -public: - explicit PacketFactoryTemplate(int payload_id) - { - VescPacketFactory::registerPacketType(payload_id, &PacketFactoryTemplate::create); - } - - static VescPacketPtr create(std::shared_ptr frame) - { - return VescPacketPtr(new PACKETTYPE(frame)); - } -}; - /** Use this macro to register packets */ -#define REGISTER_PACKET_TYPE(id, klass) \ -static PacketFactoryTemplate global_##klass##Factory((id)); +#define REGISTER_PACKET_TYPE(id, klass) \ + class klass##Factory \ + { \ + public: \ + klass##Factory() \ + { \ + VescPacketFactory::registerPacketType((id), &klass##Factory::create); \ + } \ + static VescPacketPtr create(boost::shared_ptr frame) \ + { \ + return VescPacketPtr(new klass(frame)); \ + } \ + }; \ + static klass##Factory global_##klass##Factory; } // namespace vesc_driver diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch index f7d4765..654e348 100644 --- a/vesc_driver/launch/vesc_driver_node.launch +++ b/vesc_driver/launch/vesc_driver_node.launch @@ -5,7 +5,7 @@ - + diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 8b58cc4..0e5c0b2 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -15,16 +15,23 @@ Joshua Whitley catkin - roslint + + nodelet + pluginlib + roscpp + std_msgs + vesc_msgs + serial nodelet pluginlib roscpp - serial std_msgs vesc_msgs + serial + diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index ed90113..4d2f466 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -29,26 +29,27 @@ #include #include -#include #include -#include +#include #include +#include namespace vesc_driver { -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) +VescDriver::VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) + : vesc_(std::string(), boost::bind(&VescDriver::vescPacketCallback, this, _1), + boost::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) { // get vesc serial port address std::string port; @@ -73,14 +74,14 @@ VescDriver::VescDriver(ros::NodeHandle nh, // create vesc state (telemetry) publisher state_pub_ = nh.advertise("sensors/core", 10); + imu_pub_ = nh.advertise("sensors/imu", 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); // subscribe to motor and servo command topics - duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, - &VescDriver::dutyCycleCallback, this); + 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); @@ -122,19 +123,22 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) */ if (driver_mode_ == MODE_INITIALIZING) { + // ROS_INFO_STREAM("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_); + ROS_INFO("Connected to VESC with firmware version %d.%d", fw_version_major_, fw_version_minor_); driver_mode_ = MODE_OPERATING; } } else if (driver_mode_ == MODE_OPERATING) { + // ROS_INFO_STREAM("driver_mode = MODE_OPERATING"); // poll for vesc state (telemetry) vesc_.requestState(); + // poll for vesc imu + vesc_.requestImuData(); } else { @@ -143,12 +147,13 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) } } -void VescDriver::vescPacketCallback(const std::shared_ptr& packet) +void VescDriver::vescPacketCallback(const boost::shared_ptr& packet) { + // ROS_INFO_STREAM(packet->name()); + if (packet->name() == "Values") { - std::shared_ptr values = - std::dynamic_pointer_cast(packet); + boost::shared_ptr values = boost::dynamic_pointer_cast(packet); vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); state_msg->header.stamp = ros::Time::now(); @@ -170,12 +175,45 @@ void VescDriver::vescPacketCallback(const std::shared_ptr& pac } else if (packet->name() == "FWVersion") { - std::shared_ptr fw_version = - std::dynamic_pointer_cast(packet); + boost::shared_ptr fw_version = + boost::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); } + else if (packet->name() == "ImuData") + { + boost::shared_ptr imuData = boost::dynamic_pointer_cast(packet); + + auto imu_msg = new vesc_msgs::VescImuStamped(); + // auto std_imu_msg = Imu(); + + imu_msg->header.stamp = ros::Time::now(); + // std_imu_msg.header.stamp = ros::Time::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(); + + imu_pub_.publish(*imu_msg); + } } void VescDriver::vescErrorCallback(const std::string& error) @@ -190,7 +228,7 @@ void VescDriver::vescErrorCallback(const std::string& error) */ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) { - if (driver_mode_ == MODE_OPERATING) + if (driver_mode_ = MODE_OPERATING) { vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } @@ -203,7 +241,7 @@ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle */ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) { - if (driver_mode_ == MODE_OPERATING) + if (driver_mode_ = MODE_OPERATING) { vesc_.setCurrent(current_limit_.clip(current->data)); } @@ -216,7 +254,7 @@ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) */ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) { - if (driver_mode_ == MODE_OPERATING) + if (driver_mode_ = MODE_OPERATING) { vesc_.setBrake(brake_limit_.clip(brake->data)); } @@ -230,7 +268,7 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) */ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) { - if (driver_mode_ == MODE_OPERATING) + if (driver_mode_ = MODE_OPERATING) { vesc_.setSpeed(speed_limit_.clip(speed->data)); } @@ -242,7 +280,7 @@ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) */ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& 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; @@ -255,7 +293,7 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) */ void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) { - if (driver_mode_ == MODE_OPERATING) + if (driver_mode_ = MODE_OPERATING) { double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); @@ -268,8 +306,8 @@ void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, const boost::optional& min_lower, - const boost::optional& max_upper) : - name(str) + const boost::optional& max_upper) + : name(str) { // check if user's minimum value is outside of the range min_lower to max_upper double param_min; @@ -278,14 +316,14 @@ VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::str if (min_lower && param_min < *min_lower) { lower = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is less than the feasible minimum (" << *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) { lower = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << - ") is greater than the feasible maximum (" << *max_upper << ")."); + ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is greater than the feasible maximum (" + << *max_upper << ")."); } else { @@ -304,14 +342,14 @@ VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::str if (min_lower && param_max < *min_lower) { upper = *min_lower; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is less than the feasible minimum (" << *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) { upper = *max_upper; - ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << - ") is greater than the feasible maximum (" << *max_upper << ")."); + ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is greater than the feasible maximum (" + << *max_upper << ")."); } else { @@ -326,8 +364,8 @@ VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::str // check for min > max if (upper && lower && *lower > *upper) { - ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper - << ") is less than parameter " << name << "_min (" << *lower << ")."); + ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper << ") is less than parameter " << name << "_min (" + << *lower << ")."); double temp(*lower); lower = *upper; upper = temp; @@ -335,29 +373,30 @@ 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)"; + if (lower) + oss << *lower << " "; + else + oss << "(none) "; + if (upper) + oss << *upper; + else + oss << "(none)"; ROS_DEBUG_STREAM(oss.str()); } double VescDriver::CommandLimit::clip(double value) -{ +{ // check if (lower && value < lower) { - ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", - name.c_str(), value, *lower); + ROS_INFO_THROTTLE(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); + ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", name.c_str(), value, *upper); return *upper; } return value; } - } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver_nodelet.cpp b/vesc_driver/src/vesc_driver_nodelet.cpp index 34134af..59ef279 100644 --- a/vesc_driver/src/vesc_driver_nodelet.cpp +++ b/vesc_driver/src/vesc_driver_nodelet.cpp @@ -1,48 +1,25 @@ -// 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 - #include "vesc_driver/vesc_driver.h" namespace vesc_driver { -class VescDriverNodelet: public nodelet::Nodelet +class VescDriverNodelet : public nodelet::Nodelet { public: - VescDriverNodelet() {} + VescDriverNodelet() + { + } private: virtual void onInit(void); - std::shared_ptr vesc_driver_; + boost::shared_ptr vesc_driver_; + }; // class VescDriverNodelet void VescDriverNodelet::onInit() diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 618185f..cc02dbc 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -29,15 +29,13 @@ #include -#include -#include -#include -#include -#include #include -#include +#include +#include +#include #include +#include #include "vesc_driver/vesc_packet_factory.h" @@ -47,26 +45,28 @@ 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() + : serial_(std::string(), 115200, serial::Timeout::simpleTimeout(100), serial::eightbits, serial::parity_none, + serial::stopbits_one, serial::flowcontrol_none) + { + } - void rxThread(); + void* rxThread(void); - std::thread rxThreadHelper() + static void* rxThreadHelper(void* context) { - return std::thread(&Impl::rxThread, this); + return ((VescInterface::Impl*)context)->rxThread(); } - std::thread rx_thread_; + pthread_t rx_thread_; bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; serial::Serial serial_; + VescFrame::CRC send_crc_; }; -void VescInterface::Impl::rxThread() +void* VescInterface::Impl::rxThread(void) { Buffer buffer; buffer.reserve(4096); @@ -82,13 +82,11 @@ void VescInterface::Impl::rxThread() 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) + if (VescFrame::VESC_SOF_VAL_SMALL_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); + VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); if (packet) { // good packet, check if we skipped any data @@ -137,8 +135,7 @@ void VescInterface::Impl::rxThread() } // 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_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()) { @@ -147,11 +144,9 @@ void VescInterface::Impl::rxThread() } } - -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); @@ -201,7 +196,8 @@ void VescInterface::connect(const std::string& port) // start up a monitoring thread impl_->rx_thread_run_ = true; - impl_->rx_thread_ = impl_->rxThreadHelper(); + int result = pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); + assert(0 == result); } void VescInterface::disconnect() @@ -212,7 +208,9 @@ void VescInterface::disconnect() { // bring down read thread impl_->rx_thread_run_ = false; - impl_->rx_thread_.join(); + int result = pthread_join(impl_->rx_thread_, NULL); + assert(0 == result); + impl_->serial_.close(); } } @@ -273,4 +271,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..bfcb03a 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -29,17 +29,19 @@ #include #include -#include -#include + +#include +#include +#include #include "vesc_driver/vesc_packet_factory.h" #include "vesc_driver/datatypes.h" +#include + namespace vesc_driver { -constexpr CRC::Parameters VescFrame::CRC_TYPE; - VescFrame::VescFrame(int payload_size) { assert(payload_size >= 0 && payload_size <= 1024); @@ -70,34 +72,30 @@ VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payl { /* 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(boost::distance(frame) >= VESC_MIN_FRAME_SIZE); + assert(boost::distance(frame) <= VESC_MAX_FRAME_SIZE); + assert(boost::distance(payload) <= VESC_MAX_PAYLOAD_SIZE); + assert(std::distance(frame.first, payload.first) > 0 && std::distance(payload.second, frame.second) > 0); - frame_.reset(new Buffer(frame.first, frame.second)); + frame_.reset(new Buffer(boost::begin(frame), boost::end(frame))); 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); + assert(boost::distance(payload_) > 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, boost::shared_ptr raw) : VescFrame(*raw), name_(name) { } /*------------------------------------------------------------------------------------------------*/ -VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : - VescPacket("FWVersion", raw) +VescPacketFWVersion::VescPacketFWVersion(boost::shared_ptr raw) : VescPacket("FWVersion", raw) { } @@ -113,55 +111,57 @@ int VescPacketFWVersion::fwMinor() const 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); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -VescPacketValues::VescPacketValues(std::shared_ptr raw) : - VescPacket("Values", raw) +VescPacketValues::VescPacketValues(boost::shared_ptr raw) : VescPacket("Values", raw) { } double VescPacketValues::temp_mos1() 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 + 59)) << 8) + + (static_cast(*(payload_.first + 60)))); return static_cast(v) / 10.0; } double VescPacketValues::temp_mos2() 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 + 61)) << 8) + + (static_cast(*(payload_.first + 62)))); return static_cast(v) / 10.0; } +double VescPacketValues::temp_mos3() const +{ + int16_t v = static_cast((static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)))); + return static_cast(v) / 10.0; +} double VescPacketValues::current_motor() 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 { - 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 { int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + @@ -171,46 +171,41 @@ double VescPacketValues::duty_now() const 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))); + 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); } double VescPacketValues::amp_hours() 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))); + 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); } double VescPacketValues::amp_hours_charged() 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))); + 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); } double VescPacketValues::tachometer() 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))); + 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); } double VescPacketValues::tachometer_abs() 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))); + 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); } @@ -245,20 +240,18 @@ double VescPacketValues::watt_hours_charged() const 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); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ - -VescPacketSetDuty::VescPacketSetDuty(double duty) : - VescPacket("SetDuty", 5, COMM_SET_DUTY) +VescPacketSetDuty::VescPacketSetDuty(double duty) : VescPacket("SetDuty", 5, COMM_SET_DUTY) { /** @todo range check duty */ @@ -269,16 +262,16 @@ VescPacketSetDuty::VescPacketSetDuty(double duty) : *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -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); @@ -287,16 +280,17 @@ VescPacketSetCurrent::VescPacketSetCurrent(double current) : *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -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); @@ -305,16 +299,16 @@ VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -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); @@ -323,16 +317,16 @@ VescPacketSetRPM::VescPacketSetRPM(double rpm) : *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -VescPacketSetPos::VescPacketSetPos(double pos) : - VescPacket("SetPos", 5, COMM_SET_POS) +VescPacketSetPos::VescPacketSetPos(double pos) : VescPacket("SetPos", 5, COMM_SET_POS) { /** @todo range check pos */ @@ -343,16 +337,16 @@ VescPacketSetPos::VescPacketSetPos(double pos) : *(payload_.first + 3) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 4) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); *(frame_->end() - 3) = static_cast(crc >> 8); *(frame_->end() - 2) = static_cast(crc & 0xFF); } /*------------------------------------------------------------------------------------------------*/ -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 */ @@ -361,10 +355,218 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : *(payload_.first + 1) = static_cast((static_cast(v) >> 8) & 0xFF); *(payload_.first + 2) = static_cast(static_cast(v) & 0xFF); - uint16_t crc = CRC::Calculate( - &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + uint16_t crc = crc_calc.checksum(); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} + +VescPacketImu::VescPacketImu(boost::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); + + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*payload_.first), boost::distance(payload_)); + + uint16_t crc = crc_calc.checksum(); + *(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..02e5a40 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -29,8 +29,10 @@ #include #include -#include -#include + +#include +#include +#include #include "vesc_driver/vesc_packet.h" @@ -52,21 +54,24 @@ void VescPacketFactory::registerPacketType(int payload_id, CreateFn 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)); @@ -75,8 +80,7 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi 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) + if (VescFrame::VESC_SOF_VAL_SMALL_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 @@ -96,7 +100,7 @@ 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 (boost::distance(view_payload) > 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 @@ -105,26 +109,26 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi BufferRangeConst view_frame(begin, iter_eof + 1); // do we have enough data in the buffer to complete the frame? - int frame_size = std::distance(view_frame.first, view_frame.second); + int frame_size = boost::distance(view_frame); if (buffer_size < frame_size) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - frame_size - buffer_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) 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)) + unsigned short crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); + VescFrame::CRC crc_calc; + crc_calc.process_bytes(&(*view_payload.first), boost::distance(view_payload)); + if (crc != crc_calc.checksum()) 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)); + boost::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 (boost::distance(view_payload) > 0) { // get constructor function from payload id FactoryMap* p_map(getMap()); diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index d1365bd..0fa1a3d 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -4,6 +4,7 @@ project(vesc_msgs) find_package(catkin REQUIRED COMPONENTS std_msgs message_generation + geometry_msgs ) add_message_files( @@ -11,12 +12,16 @@ add_message_files( FILES VescState.msg VescStateStamped.msg + VescImu.msg + VescImuStamped.msg ) + generate_messages( DEPENDENCIES std_msgs + geometry_msgs ) catkin_package( - CATKIN_DEPENDS std_msgs message_runtime + CATKIN_DEPENDS std_msgs message_runtime geometry_msgs ) diff --git a/vesc_msgs/msg/VescImu.msg b/vesc_msgs/msg/VescImu.msg new file mode 100644 index 0000000..09bccd1 --- /dev/null +++ b/vesc_msgs/msg/VescImu.msg @@ -0,0 +1,6 @@ +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/package.xml b/vesc_msgs/package.xml index dc14307..a5128aa 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -15,9 +15,13 @@ Joshua Whitley catkin + + std_msgs message_generation + geometry_msgs std_msgs + message_runtime + geometry_msgs - message_runtime